#ifndef DATA_CONVERT_TF3_SENSOR_MSGS_H
#define DATA_CONVERT_TF3_SENSOR_MSGS_H

#include <tf3/time.h>
#include <tf3/compat.h>
#include <tf3/LinearMath/Transform.h>
#include <robot_geometry_msgs/TransformStamped.h>
#include <robot/time.h>
#include <cmath>

namespace tf3
{

    inline robot::Time convertTime(const tf3::Time& time)
    {
        robot::Time time_tmp;
        time_tmp.sec = time.sec;
        time_tmp.nsec = time.nsec;
        return time_tmp;
    }

    inline tf3::Time convertTime(const robot::Time& time)
    {
        tf3::Time time_tmp;
        time_tmp.sec = time.sec;
        time_tmp.nsec = time.nsec;
        return time_tmp;
    }

    inline tf3::Quaternion convertQuaternion(const robot_geometry_msgs::Quaternion& q)
    {
        tf3::Quaternion out(q.x,q.y,q.z,q.w);
        return out;
    }
    
    inline robot_geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
    {
        return robot_geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
    }

    inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg)
    {
        tf3::Transform out;

        out.setOrigin(tf3::Vector3(
            msg.transform.translation.x,
            msg.transform.translation.y,
            msg.transform.translation.z
        ));

        tf3::Quaternion q(
            msg.transform.rotation.x,
            msg.transform.rotation.y,
            msg.transform.rotation.z,
            msg.transform.rotation.w
        );
        out.setBasis(tf3::Matrix3x3(q));

        return out;
    }

    inline tf3::Transform convertToTransform(const robot_geometry_msgs::TransformStamped& msg)
    {
        tf3::Transform out;

        out.setOrigin(tf3::Vector3(
            msg.transform.translation.x,
            msg.transform.translation.y,
            msg.transform.translation.z
        ));

        tf3::Quaternion q(
            msg.transform.rotation.x,
            msg.transform.rotation.y,
            msg.transform.rotation.z,
            msg.transform.rotation.w
        );
        out.setBasis(tf3::Matrix3x3(q));

        return out;
    }


    inline tf3::TransformStampedMsg convertToTransformStampedMsg(const robot_geometry_msgs::TransformStamped& msg)
    {
        tf3::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;
    }
    inline robot_geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
    {
        robot_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_TF3_SENSOR_MSGS_H