/*
 * util.cpp
 *
 *  Created on: 24/06/2013
 *      Author: timothyw
 */

#include "victim_id/util.hpp"

#define DEFAULT_MAXWAIT_SERVICE     2.0
#define DEFAULT_MAXWAIT_TRANSFORM   2.0
#define LOG_START                   "VIDUtil ::"

using std::string;

namespace casualty {

Point3D VIDUtil::transformPoint(tf::TransformListener* tfListener, string fromFrame, string toFrame,
        ros::Time timestamp, Point3D p) {
    std::string error;
    bool transformLoaded = tfListener->waitForTransform(toFrame, fromFrame, timestamp,
            ros::Duration(DEFAULT_MAXWAIT_TRANSFORM), ros::Duration(0.01), &error);
    if (!transformLoaded) {
        LOG("%s \t\t Could not load transform from %s to %s\n", LOG_START, fromFrame.c_str(), toFrame.c_str());
        LOG("%s \t\t Error: %s\n", LOG_START, error.c_str());
        return p;
    }

    // Do transform
    geometry_msgs::PointStamped fromPoint;
    fromPoint.header.frame_id = fromFrame;
    fromPoint.header.stamp = timestamp;
    fromPoint.point = p.toROS();
    geometry_msgs::PointStamped toPoint;
    tfListener->transformPoint(toFrame, fromPoint, toPoint);
    return Point3D(toPoint.point);
}

Pose VIDUtil::transformPose(tf::TransformListener* tfListener, string fromFrame, string toFrame,
        ros::Time timestamp, Pose pose) {
    //LOG("%s transformPose\n", LOG_START);
    std::string error;
    bool transformLoaded = tfListener->waitForTransform(toFrame, fromFrame, timestamp,
            ros::Duration(DEFAULT_MAXWAIT_TRANSFORM), ros::Duration(0.01), &error);
    if (!transformLoaded) {
        LOG("%s \t\t Could not load transform from %s to %s\n", LOG_START, fromFrame.c_str(), toFrame.c_str());
        LOG("%s \t\t Error: %s\n", LOG_START, error.c_str());
        return pose;
    }
    //LOG("%s \t got transform\n", LOG_START);

    // Do transform
    geometry_msgs::PoseStamped fromPose;
    fromPose.header.frame_id = fromFrame;
    fromPose.header.stamp = timestamp;
    fromPose.pose = pose.toROS();
    geometry_msgs::PoseStamped toPose;
    tfListener->transformPose(toFrame, fromPose, toPose);
    return Pose(toPose.pose);
}

tf::StampedTransform VIDUtil::getTransform(tf::TransformListener* tfListener, string fromFrame, string toFrame,
        ros::Time timestamp) {
    // Wait(target, source, timestamp, wait)
    std::string error;
    bool transformLoaded = tfListener->waitForTransform(toFrame, fromFrame, timestamp,
            ros::Duration(DEFAULT_MAXWAIT_TRANSFORM), ros::Duration(0.01), &error);

    if (!transformLoaded) {
        //LOG("%s \t\t Could not load transform from %s to %s\n", LOG_START, frameWorld.c_str(), frameRobot.c_str());
        //LOG("%s \t\t Error: %s\n", LOG_START, error.c_str());
        return tf::StampedTransform();
    }
    //LOG("%s \t\t Transform obtained\n", LOG_START);

    // Lookup(target, source, timestamp, wait)
    tf::StampedTransform transform;
    tfListener->lookupTransform(toFrame, fromFrame, timestamp, transform);
    return transform;
}

} // namespace casualty
