#ifndef DATA_CONVERT_H
#define DATA_CONVERT_H

#include <geometry_msgs/TransformStamped.h>
#include <tf2/utils.h>
#include <tf2/compat.h>
#include <robot/time.h>

namespace costmap_2d
{
    robot::Time convertTime(tf2::Time time)
    {
        robot::Time time_tmp;
        time_tmp.sec = time.sec;
        time_tmp.nsec = time.nsec;
        return time_tmp;
    }

    tf2::Time convertTime(robot::Time time)
    {
        tf2::Time time_tmp;
        time_tmp.sec = time.sec;
        time_tmp.nsec = time.nsec;
        return time_tmp;
    }

    
    tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
    {
        tf2::Quaternion q(
            msg.rotation.x,
            msg.rotation.y,
            msg.rotation.z,
            msg.rotation.w
        );

        tf2::Vector3 t(
            msg.translation.x,
            msg.translation.y,
            msg.translation.z
        );

        tf2::Transform tf(q, t);
        return tf;
    }

    tf2::Transform convertToTf2Transform(const tf2::TransformMsg& msg)
    {
        tf2::Quaternion q(
            msg.rotation.x,
            msg.rotation.y,
            msg.rotation.z,
            msg.rotation.w
        );

        tf2::Vector3 t(
            msg.translation.x,
            msg.translation.y,
            msg.translation.z
        );

        tf2::Transform tf(q, t);
        return tf;
    }

    tf2::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
    {
        tf2::TransformStampedMsg out;
        out.header.seq = msg.header.seq;
        out.header.stamp = convertTime(msg.header.stamp);
        out.header.frame_id = msg.header.frame_id;
        out.child_frame_id = msg.child_frame_id;
        out.transform.translation.x = msg.transform.translation.x;
        out.transform.translation.y = msg.transform.translation.y;
        out.transform.translation.z = msg.transform.translation.z;
        out.transform.rotation.x = msg.transform.rotation.x; 
        out.transform.rotation.y = msg.transform.rotation.y; 
        out.transform.rotation.z = msg.transform.rotation.z; 
        out.transform.rotation.w = msg.transform.rotation.w; 
        return out;
    }
    geometry_msgs::TransformStamped convertToTransformStamped(const tf2::TransformStampedMsg& msg)
    {
        geometry_msgs::TransformStamped out;
        out.header.seq = msg.header.seq;
        out.header.stamp = convertTime(msg.header.stamp);
        out.header.frame_id = msg.header.frame_id;
        out.child_frame_id = msg.child_frame_id;
        out.transform.translation.x = msg.transform.translation.x;
        out.transform.translation.y = msg.transform.translation.y;
        out.transform.translation.z = msg.transform.translation.z;
        out.transform.rotation.x = msg.transform.rotation.x; 
        out.transform.rotation.y = msg.transform.rotation.y; 
        out.transform.rotation.z = msg.transform.rotation.z; 
        out.transform.rotation.w = msg.transform.rotation.w; 
        return out;
    }
}

#endif // DATA_CONVERT_H