//
// 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 "mpc/MonitorMPC.h"
#include "trajectory/make_trajectory.h"
#include "trajectory/FourthTrajectory.h"
#include "mpc/mpc_tools.h"

#include <random>

ros::Publisher g_vel_pub;
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;
double g_ref_v;

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,40)) {
        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;

    }

}
std::vector<tf::Transform> local_path;

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    // local_path.clear();
    ROS_INFO_ONCE("odom received!");
    static tf::TransformListener tf_listen(ros::Duration(10));
    static double dt=0.05;
    auto start = std::chrono::system_clock::now();

    double vx = msg->twist.twist.linear.x;
    double vth = 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;
        
        // std::random_device seed_device;
        // std::default_random_engine engine(seed_device());//通过operator（）取得设备产生的随机值
        // engine.seed(time(0)); // 如果不传进种子的话，该引擎的状态被回置以后，将会产生同样的值
        // std::uniform_int_distribution<int> distr(-50,50);//10到100，int类型，线性分布
        // tf::Vector3 translation = g_cur_pose.getOrigin();
        // g_cur_pose.setOrigin(tf::Vector3(translation.getX()+distr(engine)/100.0, translation.getY()+distr(engine)/100.0, translation.getZ()));
    }
    catch (tf::TransformException &ex) {
        if(g_goal_valid)
            ROS_ERROR(" tf listen error : %s",ex.what());
        return;
    }
    int fitNum=20;
    if(g_goal_valid==false || maker.Size()<fitNum ) return ;
//////判断是否到达目标点
    double x = transform.getOrigin().getX();
    double y = transform.getOrigin().getY();
    double gx = g_goal.getOrigin().getX();
    double gy = g_goal.getOrigin().getY();
    double yaw;
    double theta,a,b;
    transform.getBasis().getEulerZYX(theta,a,b);
    g_goal.getBasis().getEulerZYX(yaw, a, b);

    double dx=g_goal.getOrigin().getX()-transform.getOrigin().getX();
    double dy=g_goal.getOrigin().getY()-transform.getOrigin().getY();
    double dis_th = fabs(yaw - theta);
    if (fabs(dx) < 0.05 && fabs(dy) < 0.03 && dis_th < 0.02 && fabs(vx) < 0.05 && fabs(vth) < 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_goal_valid = false;
        std::cout<<"   到达目标点 !!!!"<<std::endl;
        return;
    }

    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
    tf::Vector3 length(dx,dy,0);
    double direct_x = dx * cos(-theta) - dy * sin(-theta);

    double g_ref_v=length.length()/(g_max_vx*3.0);
    g_ref_v=std::max(g_ref_v,0.02);
    g_ref_v=std::min(g_ref_v,g_max_vx);
    if (direct_x<0) g_ref_v=-g_ref_v;

    int pick_index = 0;
    ///// 选点
    std::vector<tf::Pose> poses;
    if (!mpc_tool.filterPath(transform, maker.trajectory_, poses, fitNum, pick_index, 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() - transform.getOrigin().getX();
        double y = poses[i].getOrigin().getY() - transform.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);

    // // 接近终点修改参考速度
    // if(local_path.size() > 1 && maker.trajectory_.size()>1){
    //     double local_dist = 0;
    //     double global_dist = 0;
    //     for (size_t i = 1; i < local_path.size(); i++)
    //     {
    //         local_dist += hypot(local_path[i].getOrigin().getX() - local_path[i-1].getOrigin().getX(), local_path[i].getOrigin().getY() - local_path[i-1].getOrigin().getY());
    //     }
    //     for (size_t i = pick_index; i < maker.trajectory_.size(); i++)
    //     {
    //         global_dist += hypot(maker.trajectory_[i].getOrigin().getX() - maker.trajectory_[i-1].getOrigin().getX(), maker.trajectory_[i].getOrigin().getY() - maker.trajectory_[i-1].getOrigin().getY());
    //     }
    //     if(local_dist > global_dist)
    //     {
    //         g_ref_v *= 0.5;
    //     }
    //     std::cout<<"dists: "<<local_dist<<", "<<global_dist<<std::endl;
    //     // ref_v=length.length()/(g_max_vx*1.5);
    //     g_ref_v = fabs(g_ref_v);
    //     g_ref_v=std::max(g_ref_v,0.02);
    //     g_ref_v=std::min(g_ref_v,g_max_vx);
    //     if (direct_x<0) g_ref_v=-g_ref_v;
    // }


    Eigen::VectorXd state(5);
    dt=0.05;
    state << vx, vth, g_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);

        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
        local_path.clear();
        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) + transform.getOrigin().getX();
            float ry = x * sin(theta) + y * cos(theta) + transform.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();
            local_path.push_back(mpc.m_trajectory[i]);
        }
        //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("input/(download): %.3f(%.3f)  %.3f(%.3f), ref:%.3f,  dt:%.3f\n",vx,out[0],vth,out[1],g_ref_v,time);
        double sleep_t=(dt-time)*1000.0;
        if(sleep_t>0)
            usleep(1000*int(sleep_t));
    }
    else{
        printf("solve failed   input: %.3f  %.3f, \n",vx,vth);
        //std::cout<<"solve failed."<<std::endl;
    }


}
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,40)) {
        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_ref_v = g_max_vx;

    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 odom_sub_ = gn.subscribe<nav_msgs::Odometry>( "/odom", 1, odomCallback);

    g_vel_pub = gn.advertise<geometry_msgs::Twist>("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;
}