/*
 * @Description: 运动控制节点，接收goal，odom，tf生成并发布路径，优化并发布cmd_vel
 * @Author: yct
 * @Date: 2020-06-11 17:27:41
 * @LastEditTime: 2020-06-11 17:41:22
 * @LastEditors: yct
 */ 
#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 <turtlesim/Pose.h>

#include "moving_control/MonitorMPC.h"
#include "moving_control/make_trajectory.h"
#include "moving_control/FourthTrajectory.h"
#include "moving_control/mpc_tools.h"
#include <tf/transform_broadcaster.h>

geometry_msgs::Twist g_cmd_vel;
ros::Publisher g_vel_pub, g_vel_pub1, g_pose_pub1;
ros::Publisher g_local_plan_pub,g_global_plan_pub,g_fit_plan_pub;
tf::Pose g_cur_pose,g_goal;
bool g_goal_valid=false;

FourthTrajectory maker;
mpc_tools mpc_tool;
double g_max_vx;

void MPC();

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);

    ///更新轨迹

    if(maker.Make(g_cur_pose,g_goal,20)) {
        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_cur_pose.setRotation(q);
    g_cur_pose.setOrigin(tf::Vector3(msg->x,msg->y,0));

    g_cmd_vel.linear.x=msg->linear_velocity;
    g_cmd_vel.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_cur_pose.getOrigin().getX();
    pose.pose.position.y=g_cur_pose.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);

    MPC();

}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    ROS_INFO_ONCE("odom received!");
    static tf::TransformListener tf_listen(ros::Duration(10));

    g_cmd_vel.linear.x = msg->twist.twist.linear.x;
    g_cmd_vel.angular.z = msg->twist.twist.angular.z;
    std::string frame_id = msg->child_frame_id;
    std::string tf_error;
    tf::StampedTransform transform;
    try{
        tf_listen.lookupTransform("/map", "/base_link",ros::Time(0), transform);
        g_cur_pose=transform;
    }
    catch (tf::TransformException &ex) {
        if(g_goal_valid)
            ROS_ERROR(" tf listen error : %s",ex.what());
        return;
    }
    MPC();
}

void MPC()
{
    auto start = std::chrono::system_clock::now();
    static double dt=0.05;
    int fitNum=20;
    if(g_goal_valid==false || maker.Size()<fitNum ) return ;
//////判断是否到达目标点
    double x = g_cur_pose.getOrigin().getX();
    double y = g_cur_pose.getOrigin().getY();
    double gx = g_goal.getOrigin().getX();
    double gy = g_goal.getOrigin().getY();
    double yaw;
    double theta,a,b;
    g_cur_pose.getBasis().getEulerZYX(theta,a,b);
    g_goal.getBasis().getEulerZYX(yaw, a, b);

    double dx=g_goal.getOrigin().getX()-g_cur_pose.getOrigin().getX();
    double dy=g_goal.getOrigin().getY()-g_cur_pose.getOrigin().getY();
    double dis_th = fabs(yaw - theta);
    if (fabs(dx) < 0.05 && fabs(dy) < 0.03 && dis_th < 0.02 && fabs(g_cmd_vel.linear.x) < 0.05 && fabs(g_cmd_vel.angular.z) < 0.05)
    {
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = 0;
        g_vel_pub.publish(cmd_vel);
        g_vel_pub1.publish(cmd_vel);

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

    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
    tf::Vector3 length(dx,dy,0);

    double direct_x = dx * cos(-theta) - dy * sin(-theta);

    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;

    ///// 选点
    std::vector<tf::Pose> poses;
    if(!mpc_tool.filterPath(g_cur_pose, maker.trajectory_,poses,fitNum,direct_x<0))
    {
        ROS_ERROR(" fit error ...");
        return ;
    }
    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);

    }

    //////   fit   select points
    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_cur_pose.getOrigin().getX();
        double y = poses[i].getOrigin().getY() - g_cur_pose.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 state(5);
    state << g_cmd_vel.linear.x, g_cmd_vel.angular.z, ref_v, g_max_vx,dt;
    MonitorMPC mpc;
    std::vector<double> out;

    if(mpc.Solve(state,coeffs,out))
    {
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = out[0];
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = out[1];
        g_vel_pub.publish(cmd_vel);
        g_vel_pub1.publish(cmd_vel);

        nav_msgs::Path gui_path;
        gui_path.poses.resize(mpc.m_trajectory.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.size(); i++) {
            float x = mpc.m_trajectory[i].getOrigin().getX();
            float y = mpc.m_trajectory[i].getOrigin().getY();
            float rx = x * cos(theta) - y * sin(theta) + g_cur_pose.getOrigin().getX();
            float ry = x * sin(theta) + y * cos(theta) + g_cur_pose.getOrigin().getY();
            gui_path.poses[i].pose.position.x = rx;
            gui_path.poses[i].pose.position.y = ry;
            tf::Quaternion quat = mpc.m_trajectory[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);

        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 updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    printf("update goal recieve ...  \n");
    double x=g_cur_pose.getOrigin().getX();
    double y=g_cur_pose.getOrigin().getY();
    double z=g_cur_pose.getOrigin().getZ();
    double qx=g_cur_pose.getRotation().getX();
    double qy=g_cur_pose.getRotation().getY();
    double qz=g_cur_pose.getRotation().getZ();
    double qw=g_cur_pose.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_cur_pose,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;

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

    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 odom_sub_ = gn.subscribe<nav_msgs::Odometry>( "/odom", 1, odomCallback);
    ros::Subscriber pose_sub1_ = gn.subscribe<turtlesim::Pose>( "/turtlesim1/turtle1/pose", 1, poseCallback1);

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

    g_vel_pub = gn.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    g_vel_pub1 = gn.advertise<geometry_msgs::Twist>("/turtlesim1/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_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);

    ros::spin();

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

    return 0;
}
