//
// Created by zx on 2019/9/6.
//

//
// Created by zx on 2019/8/26.
//

#include <iostream>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Path.h>
#include <string>
#include <chrono>
#include <thread>
#include <turtlesim/Pose.h>

#include "mpc/TowBotMPC.h"
#include "trajectory/make_trajectory.h"
#include "trajectory/FourthTrajectory.h"
#include "mpc/mpc_tools.h"
#include <tf/transform_broadcaster.h>

ros::Publisher g_vel_pub1,g_vel_pub2;
ros::Publisher g_pose_pub1,g_pose_pub2;
ros::Publisher g_local_plan_pub,g_local_plan_pub_b,g_global_plan_pub,g_fit_plan_pub,g_fit_plan_pub_b;
tf::Pose g_pose_f,g_pose_b,g_goal;
geometry_msgs::Twist g_cmd_vel_f,g_cmd_vel_b;
bool g_goal_valid=false;
int g_direction=1;      //朝向  向前为0  先后为1

FourthTrajectory maker;
mpc_tools mpc_tool;
double g_max_vx;

// 目标设置回调
void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    printf("new goal recieve ...  \n");
    double x=msg->pose.position.x;
    double y=msg->pose.position.y;
    double z=msg->pose.position.z;
    double qx=msg->pose.orientation.x;
    double qy=msg->pose.orientation.y;
    double qz=msg->pose.orientation.z;
    double qw=msg->pose.orientation.w;
    g_goal.setOrigin(tf::Vector3(x,y,z));
    tf::Quaternion quat(qx,qy,qz,qw);
    g_goal.setRotation(quat);

    double distance1 = g_pose_f.getOrigin().distance(g_goal.getOrigin());
    double distance2 = g_pose_b.getOrigin().distance(g_goal.getOrigin());
    tf::Pose start_point=distance1>distance2?g_pose_f:g_pose_b;
    g_direction=distance1>distance2?-1:1;
    ///更新轨迹

    if(maker.Make(start_point,g_goal,30)) {
        nav_msgs::Path gui_path;
        gui_path.poses.resize(maker.Size());
        gui_path.header.frame_id = "map";
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < maker.Size(); i++) {
            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
            tf::Quaternion quat=maker[i].getRotation();

            gui_path.poses[i].pose.orientation.x = quat.x();
            gui_path.poses[i].pose.orientation.y = quat.y();
            gui_path.poses[i].pose.orientation.z = quat.z();
            gui_path.poses[i].pose.orientation.w = quat.w();
        }

        g_global_plan_pub.publish(gui_path);
        g_goal_valid=true;

    }

}

// 一号车位姿回调
void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
{
    double theta=msg->theta;
    tf::Quaternion q;
    q.setRPY(0,0,theta);
    g_pose_f.setRotation(q);
    g_pose_f.setOrigin(tf::Vector3(msg->x-2.0,msg->y,0));

    g_cmd_vel_f.linear.x=msg->linear_velocity;
    g_cmd_vel_f.angular.z=msg->angular_velocity;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id="map";
    pose.header.stamp=ros::Time::now();
    pose.pose.position.x=g_pose_f.getOrigin().getX();
    pose.pose.position.y=g_pose_f.getOrigin().getY();
    pose.pose.position.z=0;
    pose.pose.orientation.x=q.x();
    pose.pose.orientation.y=q.y();
    pose.pose.orientation.z=q.z();
    pose.pose.orientation.w=q.w();
    g_pose_pub1.publish(pose);

    static tf::TransformBroadcaster broadcaster;
    tf::Pose conn_point;
    conn_point.setOrigin(tf::Vector3(-1.3,0,0));
    conn_point.setRotation(tf::Quaternion(0,0,0,1));
    tf::Pose conn=g_pose_f*conn_point;

    broadcaster.sendTransform(
        tf::StampedTransform(conn,ros::Time::now(),"map", "conn1"));


}

// 二号车回调
void poseCallback2(const turtlesim::Pose::ConstPtr& msg)
{
    double theta=msg->theta;
    tf::Quaternion q;
    q.setRPY(0,0,theta);

    g_pose_b.setRotation(q);
    g_pose_b.setOrigin(tf::Vector3(msg->x-5.0,msg->y,0));

    g_cmd_vel_b.linear.x=msg->linear_velocity;
    g_cmd_vel_b.angular.z=msg->angular_velocity;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id="map";
    pose.header.stamp=ros::Time::now();
    pose.pose.position.x=g_pose_b.getOrigin().getX();
    pose.pose.position.y=g_pose_b.getOrigin().getY();
    pose.pose.position.z=0;
    pose.pose.orientation.x=q.x();
    pose.pose.orientation.y=q.y();
    pose.pose.orientation.z=q.z();
    pose.pose.orientation.w=q.w();
    g_pose_pub2.publish(pose);

    static tf::TransformBroadcaster broadcaster;
    tf::Pose conn_point;
    conn_point.setOrigin(tf::Vector3(1.3,0,0));
    conn_point.setRotation(tf::Quaternion(0,0,0,1));
    tf::Pose conn=g_pose_b*conn_point;

    broadcaster.sendTransform(
        tf::StampedTransform(conn,ros::Time::now(),"map", "conn2"));
}

void publish_cmd(geometry_msgs::Twist cmd_vel1,geometry_msgs::Twist cmd_vel2)
{
    ////
    g_vel_pub1.publish(cmd_vel1);
    g_vel_pub2.publish(cmd_vel2);

}

bool MPC()
{
    static tf::TransformListener tf_listen(ros::Duration(10));
    static double dt = 0.05;
    auto start = std::chrono::system_clock::now();

    std::string tf_error;

    int fitNum = 6;
    if (g_goal_valid == false || maker.Size() < fitNum) return false;

//////判断是否到达目标点
    double yaw;
    double theta, a, b;
    g_pose_f.getBasis().getEulerZYX(theta, a, b);
    g_goal.getBasis().getEulerZYX(yaw, a, b);

    double theta_b;
    g_pose_b.getBasis().getEulerZYX(theta_b, a, b);

    // 设置线速度、角速度，以及终点判定
    double dis_th=0,offsetx = 0, offsety = 0,liner_vx=0,angular_vz=0;
    // ?????
    if (g_direction == 1) {
        dis_th=fabs(yaw - theta);
        offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
        offsety = g_goal.getOrigin().getY() - g_pose_f.getOrigin().getY();
        liner_vx=g_cmd_vel_f.linear.x;
        angular_vz=g_cmd_vel_f.angular.z;
    }else {
        dis_th=fabs(yaw - theta_b);
        offsetx = g_goal.getOrigin().getX() - g_pose_b.getOrigin().getX();
        offsety = g_goal.getOrigin().getY() - g_pose_b.getOrigin().getY();
        liner_vx=g_cmd_vel_b.linear.x;
        angular_vz=g_cmd_vel_b.angular.z;
    }
    if (fabs(offsetx) < 0.05 && fabs(offsety) < 0.03 && dis_th < 0.02 && fabs(liner_vx) < 0.05
        && fabs(angular_vz) < 0.05) {
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = 0;
        publish_cmd(cmd_vel, cmd_vel);

        g_goal_valid = false;
        std::cout << "   到达目标点 !!!!" << std::endl;
        return true;
    }

    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
    double dx=offsetx,dy=offsety;
    int pick_index = 0;

    tf::Vector3 length(dx, dy, 0);
    double direct_x = dx * cos(-theta) - dy * sin(-theta);
    double ref_v = g_max_vx;
    // double ref_v = length.length() / (g_max_vx * 1.5);
    // ref_v = std::max(ref_v, 0.02);
    // ref_v = std::min(ref_v, g_max_vx);
    if (direct_x < 0) ref_v = -ref_v;

    // added by yct. 计算当前到目标的偏移(在靠近目标的车的坐标系下)
    double target_dx, target_dy;
    if (direct_x > 0)
    {
        offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
        offsety = g_goal.getOrigin().getY() - g_pose_f.getOrigin().getY();
        target_dx = offsetx * cos(-theta) - offsety * sin(-theta);
        target_dy = offsetx * sin(-theta) + offsety * cos(-theta);
        // std::cout << " forward target dx dy: " << target_dx << ", " << target_dy << std::endl;
    }else{
        offsetx = g_goal.getOrigin().getX() - g_pose_b.getOrigin().getX();
        offsety = g_goal.getOrigin().getY() - g_pose_b.getOrigin().getY();
        target_dx = offsetx * cos(-theta_b) - offsety * sin(-theta_b);
        target_dy = offsetx * sin(-theta_b) + offsety * cos(-theta_b);
        // std::cout << " backward target dx dy: " << target_dx << ", " << target_dy << std::endl;
    }

    ///// 选点
    std::vector<tf::Pose> poses;
    std::vector<tf::Pose> poses_b;
    if (!mpc_tool.filterPath(g_pose_f, maker.trajectory_, poses, fitNum, pick_index, direct_x < 0)) {
        ROS_ERROR(" fit error ...");
        return false;
    }
    else {
        nav_msgs::Path gui_path;
        gui_path.poses.resize(poses.size());
        gui_path.header.frame_id = "map";
        gui_path.header.stamp = ros::Time::now();
        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < poses.size(); i++) {
            float x = poses[i].getOrigin().getX();
            float y = poses[i].getOrigin().getY();

            gui_path.poses[i].pose.position.x = x;
            gui_path.poses[i].pose.position.y = y;
            tf::Quaternion quat = poses[i].getRotation();
            gui_path.poses[i].pose.orientation.x = quat.x();
            gui_path.poses[i].pose.orientation.y = quat.y();
            gui_path.poses[i].pose.orientation.z = quat.z();
            gui_path.poses[i].pose.orientation.w = quat.w();
        }
        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
        g_fit_plan_pub.publish(gui_path);
    }
    if (!mpc_tool.filterPath(g_pose_b, maker.trajectory_, poses_b, fitNum, pick_index, direct_x < 0)) {
        ROS_ERROR(" fit error ...");
        return false;
    }
    else {
        nav_msgs::Path gui_path;
        gui_path.poses.resize(poses.size());
        gui_path.header.frame_id = "map";
        gui_path.header.stamp = ros::Time::now();
        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < poses_b.size(); i++) {
            float x = poses_b[i].getOrigin().getX();
            float y = poses_b[i].getOrigin().getY();

            gui_path.poses[i].pose.position.x = x;
            gui_path.poses[i].pose.position.y = y;
            tf::Quaternion quat = poses_b[i].getRotation();
            gui_path.poses[i].pose.orientation.x = quat.x();
            gui_path.poses[i].pose.orientation.y = quat.y();
            gui_path.poses[i].pose.orientation.z = quat.z();
            gui_path.poses[i].pose.orientation.w = quat.w();
        }
        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
        g_fit_plan_pub_b.publish(gui_path);

    }

    // 生成参数
    Eigen::VectorXd ptsx_car(poses.size());
    Eigen::VectorXd ptsy_car(poses.size());
    for (int i = 0; i < poses.size(); i++) {
        double x = poses[i].getOrigin().getX() - g_pose_f.getOrigin().getX();
        double y = poses[i].getOrigin().getY() - g_pose_f.getOrigin().getY();
        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
    }
    Eigen::VectorXd coeffs = mpc_tool.polyfit(ptsx_car, ptsy_car, 3);

    Eigen::VectorXd ptsx_car_b(poses_b.size());
    Eigen::VectorXd ptsy_car_b(poses_b.size());
    for (int i = 0; i < poses_b.size(); i++) {
        double x = poses_b[i].getOrigin().getX() - g_pose_b.getOrigin().getX();
        double y = poses_b[i].getOrigin().getY() - g_pose_b.getOrigin().getY();
        ptsx_car_b[i] = x * cos(-theta_b) - y * sin(-theta_b);
        ptsy_car_b[i] = x * sin(-theta_b) + y * cos(-theta_b);
    }
    Eigen::VectorXd coeffs_b = mpc_tool.polyfit(ptsx_car_b, ptsy_car_b, 3);

    Eigen::VectorXd state(12);
    double vx1 = g_cmd_vel_f.linear.x;
    double vx2 = g_cmd_vel_b.linear.x;
    double vth1 = g_cmd_vel_f.angular.z;
    double vth2 = g_cmd_vel_b.angular.z;
    double dx0 = g_pose_f.getOrigin().getX() - g_pose_b.getOrigin().getX();
    double dy0 = g_pose_f.getOrigin().getY() - g_pose_b.getOrigin().getY();
    double dth1 = theta - theta_b;
    // 距离反向旋转到后车坐标系
    double dx1 = dx0 * cos(-theta_b) - dy0 * sin(-theta_b);
    double dy1 = dx0 * sin(-theta_b) + dy0 * cos(-theta_b);
    // double frontx = g_pose_f.getOrigin().getX();
    // double fronty = g_pose_f.getOrigin().getY();
    // double backx = g_pose_b.getOrigin().getX();
    // double backy = g_pose_b.getOrigin().getY();

    /*printf(" (%.3f %.3f ) , (%.3f %.3f)  (%.3f %.3f | %.3f %.3f)\n",g_pose_f.getOrigin().getX(),
        g_pose_f.getOrigin().getY(),g_pose_b.getOrigin().getX(),g_pose_b.getOrigin().getY(),
        g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/

    double slowdown_distance = 2.0;
    state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1, target_dx, target_dy, slowdown_distance;
    // std::cout << " target dx dy: " << target_dx << ", " << target_dy << std::endl;
    TowBotMPC mpc;
    std::vector<double> out;
    if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {
        geometry_msgs::Twist cmd_vel1, cmd_vel2;
        cmd_vel1.linear.x = out[0];
        cmd_vel1.linear.y = 0.0;
        cmd_vel1.angular.z = out[1];

        cmd_vel2.linear.x = out[2];
        cmd_vel2.linear.y = 0.0;
        cmd_vel2.angular.z = out[3];
        publish_cmd(cmd_vel1, cmd_vel2);

        nav_msgs::Path gui_path;
        gui_path.poses.resize(mpc.m_trajectory_f.size());
        gui_path.header.frame_id = "map";
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < mpc.m_trajectory_f.size(); i++) {
            float x = mpc.m_trajectory_f[i].getOrigin().getX();
            float y = mpc.m_trajectory_f[i].getOrigin().getY();
            float rx = x * cos(theta) - y * sin(theta) + g_pose_f.getOrigin().getX();
            float ry = x * sin(theta) + y * cos(theta) + g_pose_f.getOrigin().getY();
            gui_path.poses[i].pose.position.x = rx;
            gui_path.poses[i].pose.position.y = ry;
            tf::Quaternion quat = mpc.m_trajectory_f[i].getRotation();
            gui_path.poses[i].pose.orientation.x = quat.x();
            gui_path.poses[i].pose.orientation.y = quat.y();
            gui_path.poses[i].pose.orientation.z = quat.z();
            gui_path.poses[i].pose.orientation.w = quat.w();
        }
        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
        g_local_plan_pub.publish(gui_path);

        nav_msgs::Path gui_path_b;
        gui_path_b.poses.resize(mpc.m_trajectory_b.size());
        gui_path_b.header.frame_id = "map";
        gui_path_b.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < mpc.m_trajectory_b.size(); i++) {
            float x = mpc.m_trajectory_b[i].getOrigin().getX();
            float y = mpc.m_trajectory_b[i].getOrigin().getY();
            float rx = x * cos(theta_b) - y * sin(theta_b) + g_pose_b.getOrigin().getX();
            float ry = x * sin(theta_b) + y * cos(theta_b) + g_pose_b.getOrigin().getY();
            gui_path_b.poses[i].pose.position.x = rx;
            gui_path_b.poses[i].pose.position.y = ry;
            tf::Quaternion quat = mpc.m_trajectory_b[i].getRotation();
            gui_path_b.poses[i].pose.orientation.x = quat.x();
            gui_path_b.poses[i].pose.orientation.y = quat.y();
            gui_path_b.poses[i].pose.orientation.z = quat.z();
            gui_path_b.poses[i].pose.orientation.w = quat.w();
        }
        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
        g_local_plan_pub_b.publish(gui_path_b);

        auto end = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        double time =
            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
        //printf(" ref:%.3f, best_v:%.3f, best_th:%.3f, dt:%.3f\n",ref_v,out[0],out[1],time);
        double sleep_t = (dt - time) * 1000.0;
        if (sleep_t > 0)
            usleep(1000 * int(sleep_t));

    }
}

void thread_MPC()
{
    ros::Rate loop_rate(20);
    while(ros::ok())
    {
        if(g_goal_valid==true)
        {
            bool ret = MPC();
            if (ret) {

            }
            else {
                ROS_WARN(" MPC failed ... ");
            }
        }
        loop_rate.sleep();
    }

}

void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    printf("update goal recieve ...  \n");
    double x=g_pose_f.getOrigin().getX();
    double y=g_pose_f.getOrigin().getY();
    double z=g_pose_f.getOrigin().getZ();
    double qx=g_pose_f.getRotation().getX();
    double qy=g_pose_f.getRotation().getY();
    double qz=g_pose_f.getRotation().getZ();
    double qw=g_pose_f.getRotation().getW();
    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
    tf::Quaternion quat(qx,qy,qz,qw);
    g_goal.setRotation(quat);

    ///更新轨迹
    if(maker.Make(g_pose_f,g_goal,10)) {
        nav_msgs::Path gui_path;
        gui_path.poses.resize(maker.Size());
        gui_path.header.frame_id = "map";
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < maker.Size(); i++) {
            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
            tf::Quaternion quat=maker[i].getRotation();

            gui_path.poses[i].pose.orientation.x = quat.x();
            gui_path.poses[i].pose.orientation.y = quat.y();
            gui_path.poses[i].pose.orientation.z = quat.z();
            gui_path.poses[i].pose.orientation.w = quat.w();
        }

        g_global_plan_pub.publish(gui_path);
        g_goal_valid=true;
    }
}

int main(int argc ,char* argv[])
{
    // std::cout<<"  input max vx : ";
    // std::cin>>g_max_vx;
    // std::cout<<std::endl;
    g_max_vx = 1.5;

    ros::init(argc, argv, "MPC_node");

    ros::NodeHandle gn;

    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);

    ros::Subscriber pose_sub1_ = gn.subscribe<turtlesim::Pose>( "/turtlesim1/turtle1/pose", 1, poseCallback1);
    ros::Subscriber pose_sub2_ = gn.subscribe<turtlesim::Pose>( "/turtlesim2/turtle1/pose", 1, poseCallback2);

    g_pose_pub1=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose1", 1);
    g_pose_pub2=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose2", 1);

    g_vel_pub1 = gn.advertise<geometry_msgs::Twist>("/turtlesim1/turtle1/cmd_vel", 1);
    g_vel_pub2 = gn.advertise<geometry_msgs::Twist>("/turtlesim2/turtle1/cmd_vel", 1);

    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
    g_local_plan_pub_b = gn.advertise<nav_msgs::Path>("local_plan_b", 1);
    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
    g_fit_plan_pub_b = gn.advertise<nav_msgs::Path>("fit_plan_b", 1);

    std::thread* pthread=new std::thread(thread_MPC);
    pthread->detach();

    ros::spin();


    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0;
    publish_cmd(cmd_vel,cmd_vel);

    return 0;
}