#include <ros/ros.h>
#include "../mpc_two_vehicle/trajectory_tool.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <turtlesim/Pose.h>

void position_callback(const turtlesim::Pose::ConstPtr& msg)
{
    // 发布tf模板
    tf::TransformBroadcaster tf_broadcaster_;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    transform.setRotation(tf::Quaternion(0, 0, cos(msg->theta/2.0), sin(msg->theta/2.0)));
    tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "base_link");
    tf_broadcaster_.sendTransform(transform_msg);
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "mpc_two_vehicle_test_node");
    ros::NodeHandle nh;
    ros::Subscriber position_subscriber = nh.subscribe("/turtule1/pose", 10, position_callback);
    // ros::Subscriber target_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, targetHandler);
    ros::Publisher global_trajectory_publisher = nh.advertise<nav_msgs::Path>("global_path", 2);
    ros::Publisher local_trajectory_publisher = nh.advertise<nav_msgs::Path>("local_path", 2);

    nav_msgs::Path path;
    int path_count = 0;

    // 路径测试，从(1,1)到(3,1.5),theta=pi/4.0
    Trajectory_tool traj_tool;
    Eigen::Isometry2d base;
    Eigen::Isometry2d target;
    Eigen::Matrix2d rotation;
    trajectory_type trajectory;
    rotation << cos(M_PI_4), -sin(M_PI_4), sin(M_PI_4), cos(M_PI_4);
    base.setIdentity();
    base.pretranslate(Eigen::Vector2d(1, 1));
    target.setIdentity();
    target.rotate(rotation);
    target.pretranslate(Eigen::Vector2d(9, -1));
    bool create_trajectory_result = traj_tool.create_trajectory(base, target, trajectory);

    ros::Rate loop_rate(20);
    while (ros::ok())
    {
        // 发布全局路径
        if (create_trajectory_result)
        {
            path.header.frame_id = "map";
            path.header.seq = ++path_count;
            path.header.stamp = ros::Time::now();
            path.poses.clear();
            for (size_t i = 0; i < trajectory.size(); i++)
            {
                geometry_msgs::PoseStamped t_pose;
                t_pose.header = path.header;
                t_pose.pose.position.x = trajectory[i].translation().x();
                t_pose.pose.position.y = trajectory[i].translation().y();
                double theta = atan2(trajectory[i].rotation()(1, 0), trajectory[i].rotation()(0, 0));
                t_pose.pose.orientation.w = cos(theta / 2.0);
                t_pose.pose.orientation.z = sin(theta / 2.0);
                path.poses.push_back(t_pose);
            }
            global_trajectory_publisher.publish(path);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}