#include "frenet_planner.h"

#include <adc_tools/transfer.h>
#include <common/car_status.h>
#include <adc_tools/parameter_status.h>
//#include <decide_gps_00.h>
#include <adc_planner/lane_change_planner.h>

#include<Eigen/Dense>

using namespace std;
using namespace Eigen;

iv::decition::FrenetPlanner::FrenetPlanner(){
    this->planner_id = 1;
    this->planner_name = "Frenet";
    this->lidarGridPtr = NULL;
}

iv::decition::FrenetPlanner::~FrenetPlanner(){
    free(this->lidarGridPtr);
}


/**
 * @brief iv::decition::FrenetPlanner::getPath
 * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
 * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
 *
 * @param now_gps_ins          实时gps信息
 * @param gpsMapLine           地图数据点
 * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
 * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。offset = -(roadNow - roadOri)*roadWidth
 * @param lidarGridPtr         激光雷达信息网格
 * @return                     返回一条车辆坐标系下的路径
 */
std::vector<iv::Point2D> iv::decition::FrenetPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
    this->roadNow = -1;
    this->now_gps_ins = now_gps_ins;
    this->gpsMapLine = gpsMapLine;
    this->PathPoint = PathPoint;
    this->lidarGridPtr = lidarGridPtr;
    LaneChangePlanner *lcp = new LaneChangePlanner();
    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
    delete lcp;
    double realSecSpeed = now_gps_ins.speed / 3.6;
    leftWidth = offset;
    rightWidth = offset;
    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
    return trace;
}

/**
 * @brief iv::decition::FrenetPlanner::chooseRoadByFrenet
 * 用frenet的方法对每条道路考察，选择一个最优的道路
 * @param now_gps_ins          实时gps信息
 * @param gpsMapLine           地图数据点
 * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
 * @param offsetL              在选择道路中，地图源路线左边的最大宽度。可以为负值。offsetL = -(roadSum - 1)*roadWidth
 * @param offsetR              在选择道路中，地图源路线右边的最大宽度。可以为非负值。offsetR = (roadOri - 0)*roadWidth
 * @param lidarGridPtr         激光雷达信息网格
 * @return
 */
int iv::decition::FrenetPlanner::chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow){
    this->roadNow = roadNow;
    this->aimRoadByFrenet = -1;
    this->now_gps_ins = now_gps_ins;
    this->gpsMapLine = gpsMapLine;
    this->PathPoint = PathPoint;
    this->lidarGridPtr = lidarGridPtr;

    LaneChangePlanner *lcp = new LaneChangePlanner();
    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
    delete lcp;
    double realSecSpeed = now_gps_ins.speed / 3.6;
    leftWidth = offsetL;
    rightWidth = offsetR;
    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
    return aimRoadByFrenet;
}


/**
 * @brief iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace
 * 进行frenet规划的总函数，还包括了规划前的一些准备工作，比如frenet坐标系建立、车辆起始状态等。
 * @param gpsTrace             地图路线上从PathPoint开始的600个点
 * @param realsecSpeed         实时车速 [m/s]
 * @return                     返回一条frenet规划后并转换到车辆坐标系下的路径
 */
std::vector<iv::Point2D> iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed){
    vector<Point2D> trace;
    vector<FrenetPoint> frenet_s;
    double stp=0.0;
    //把gpsTrace里的点全部转为frenet坐标系下的点，存储在frenet_s中。相当于frenet坐标系的S轴。
    FrenetPoint tmp={.x=gpsTrace[0].x,.y=gpsTrace[0].y,.s=0.0,.d=0.0};
    frenet_s.push_back(tmp);
    for(int i=1; i<gpsTrace.size(); ++i){
        stp+=iv::decition::GetDistance(gpsTrace[i-1],gpsTrace[i]);
        tmp={.x=gpsTrace[i].x,.y=gpsTrace[i].y,.s=stp,.d=0.0};
        frenet_s.push_back(tmp);
    }

    //求出车辆当前位置在frenet坐标系下的坐标
//    FrenetPoint car_now_at_frenet = XY2Frenet(0,0,gpsTrace);
    FrenetPoint car_now_at_frenet = getFrenetfromXY(0,0,gpsTrace,this->gpsMapLine,this->PathPoint,this->now_gps_ins);

    double c_s_speed = realsecSpeed * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
    double c_d_speed = realsecSpeed * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
    double realAcc = ServiceCarStatus.mfCalAcc;
    double c_s_dd = realAcc * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
    double c_d_dd = realAcc * sin(car_now_at_frenet.tangent_Ang-M_PI/2);

    vector<Point2D> path=frenet_optimal_planning(car_now_at_frenet,frenet_s,c_s_speed,c_d_speed,c_d_dd);

    return path;
}


/**
 * @brief iv::decition::FrenetPlanner::frenet_optimal_planning
 * 对frenet规划出的路径进行后续操作，并选出损失最小的一条路径作为最优路径
 * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
 * @param frenet_s             frenet坐标系的s轴
 * @param c_speed              车辆的纵向速度。沿s轴方向的速度。
 * @param c_d_d                车辆沿d方向的速度。
 * @param c_d_dd               车辆沿d方向的加速度。
 * @return                     返回一条frenet规划的最优路径
 */
vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_optimal_planning(iv::decition::FrenetPoint car_now_frenet_point,
                                                          const std::vector<FrenetPoint>& frenet_s, double c_speed, double c_d_d, double c_d_dd){
    double s0 = car_now_frenet_point.s;
    double c_d = car_now_frenet_point.d;
    vector<iv::Point2D> gpsTrace;
    vector<Frenet_path> fplist=calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0);  //frenet规划

    sort(fplist.begin(),fplist.end(),comp); //按损失度由小到大进行排序
    for(int i=0; i<fplist.size(); ++i){
        calc_global_single_path(fplist[i],frenet_s);
        if(check_single_path(fplist[i])){
            gpsTrace = frenet_path_to_gpsTrace(fplist[i]);
            aimRoadByFrenet = fplist[i].roadflag;
            return gpsTrace;
        }
    }
    return gpsTrace;

/*
//    calc_global_paths(fplist, frenet_s);    //生成frenet路径中每个点在车辆坐标系下的坐标
//    fplist = check_paths(fplist);  //检验计算出的每条路径
//    //找到损失最小的轨迹
//    double min_cost = (numeric_limits<double>::max)();
//    Frenet_path bestpath;
//    for(int i=0; i<fplist.size(); ++i){
//        if(min_cost > fplist[i].cf){
//            min_cost = fplist[i].cf;
//            bestpath = fplist[i];
//        }
//    }
//    gpsTrace = frenet_path_to_gpsTrace(bestpath);
//    aimRoadByFrenet = bestpath.roadflag;
//    return gpsTrace;
*/
}


/**
 * @brief iv::decition::FrenetPlanner::calc_frenet_paths
 * 正式规划出不同的frenet路径，并统计相应的损失度。单从车辆行驶的平滑性计算而得，后续需检测是否有障碍物。
 * @param c_speed              车辆的纵向速度。沿s轴方向的速度
 * @param c_d                  车辆的横向（d方向）偏移距离
 * @param c_d_d                车辆沿d方向的速度
 * @param c_d_dd               车辆沿d方向的加速度
 * @param s0                   车辆沿s方向的坐标
 * @return                     返回多条frenet路径
 */
vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0){
    vector<iv::decition::Frenet_path> frenet_paths;
    int roadNum = round(abs((leftWidth-rightWidth)/ServiceParameterStatus.D_ROAD_W));   //roadNum为frenet算法的起始道路序号
    //采样，并对每一个目标配置生成轨迹
    for(double di=leftWidth; di<=rightWidth; di+=ServiceParameterStatus.D_ROAD_W,roadNum--){
        if(roadNum == this->roadNow)
            continue;
        //横向动作规划
        for(double Ti=ServiceParameterStatus.MINT; Ti<ServiceParameterStatus.MAXT; Ti+=ServiceParameterStatus.DT){
            Frenet_path fp;
            fp.roadflag = roadNum;
            //计算出关于目标配置di，Ti的横向多项式
            Quintic_polynomial lat_qp = Quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti);

            for(double i=0; i<Ti; i+=ServiceParameterStatus.D_POINT_T)
                fp.t.push_back(i);

            for(int i=0; i<fp.t.size(); ++i){
                fp.d.push_back(lat_qp.calc_point(fp.t[i]));
                fp.d_d.push_back(lat_qp.calc_first_derivative(fp.t[i]));
                fp.d_dd.push_back(lat_qp.calc_second_derivative(fp.t[i]));
                fp.d_ddd.push_back(lat_qp.calc_third_derivative(fp.t[i]));
            }

            //纵向速度规划 (速度保持)
            for(double tv=ServiceParameterStatus.TARGET_SPEED - ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE;
                tv<(ServiceParameterStatus.TARGET_SPEED + ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE);
                tv+=ServiceParameterStatus.D_T_S){
                Frenet_path tfp = fp;
                Quartic_polynomial lon_qp = Quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti);

                for(int i=0; i<fp.t.size(); ++i){
                    tfp.s.push_back(lon_qp.calc_point(fp.t[i]));
                    tfp.s_d.push_back(lon_qp.calc_first_derivative(fp.t[i]));
                    tfp.s_dd.push_back(lon_qp.calc_second_derivative(fp.t[i]));
                    tfp.s_ddd.push_back(lon_qp.calc_third_derivative(fp.t[i]));
                }

                //square of jerk
                double Jp = inner_product(tfp.d_ddd.begin(), tfp.d_ddd.end(), tfp.d_ddd.begin(), 0);
                //square of jerk
                double Js = inner_product(tfp.s_ddd.begin(), tfp.s_ddd.end(), tfp.s_ddd.begin(), 0);

                //square of diff from target speed
                double ds = pow((ServiceParameterStatus.TARGET_SPEED - tfp.s_d.back()),2);
                //横向的损失函数
                tfp.cd = ServiceParameterStatus.KJ * Jp + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * tfp.d.back() * tfp.d.back();
                //纵向的损失函数
                tfp.cv = ServiceParameterStatus.KJ * Js + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * ds;
                //总的损失函数为d 和 s方向的损失函数乘对应的系数相加
                tfp.cf = ServiceParameterStatus.KLAT * tfp.cd + ServiceParameterStatus.KLON * tfp.cv;

                frenet_paths.push_back(tfp);
            }
        }
    }
    return frenet_paths;
}


/**
 * @brief iv::decition::FrenetPlanner::calc_global_paths
 * 计算出每条frenet路径中轨迹点的x，y坐标。x，y坐标是轨迹点在车辆坐标系下的横纵坐标。
 * 部分函数参数在第一种计算XY坐标的方法中没有使用，但在第二种方法中会用到。
 * @param fplist               多条frenet路径
 * @param frenet_s             frenet坐标系的s轴
 * @return                     通过引用传递返回带有x，y值的多条frenet路径
 */
void iv::decition::FrenetPlanner::calc_global_paths(vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s){

    for(auto& fp:fplist){
        for(int i=0; i<fp.s.size(); ++i){
            double ix,iy;
//            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
            getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法

            fp.x.push_back(ix);
            fp.y.push_back(iy);
        }

        for(int i=0; i<(fp.x.size()-1); ++i){
            double dx = fp.x[i+1] - fp.x[i];
            double dy = fp.y[i+1] - fp.y[i];
            fp.yaw.push_back(atan2(dy,dx));
            fp.ds.push_back(sqrt(dx*dx + dy*dy));
        }

        fp.ds.push_back(fp.ds.back());
        fp.yaw.push_back(fp.yaw.back());

        for(int i = 0; i < (fp.yaw.size() - 1); ++i){
            fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
        }
    }
}

void iv::decition::FrenetPlanner::calc_global_single_path(Frenet_path &fp, const std::vector<FrenetPoint> &frenet_s){
    for(int i=0; i<fp.s.size(); ++i){
        double ix,iy;
        //            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
        getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法

        fp.x.push_back(ix);
        fp.y.push_back(iy);
    }

    for(int i=0; i<(fp.x.size()-1); ++i){
        double dx = fp.x[i+1] - fp.x[i];
        double dy = fp.y[i+1] - fp.y[i];
        fp.yaw.push_back(atan2(dy,dx));
        fp.ds.push_back(sqrt(dx*dx + dy*dy));
    }

    fp.ds.push_back(fp.ds.back());
    fp.yaw.push_back(fp.yaw.back());

    for(int i = 0; i < (fp.yaw.size() - 1); ++i){
        fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
    }
}


/**
 * @brief iv::decition::FrenetPlanner::check_paths
 * 对多条frenet路径进行检验，排除不符合要求的路径。
 * @param fplist               多条frenet路径
 * @return                     排除部分路径后的多条frenet路径
 */
vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::check_paths(const vector<Frenet_path>& fplist){
    vector<Frenet_path> okind;
    int i=0;
    int j=0;

    for(i=0; i<fplist.size(); ++i){
        cout<<"&&&&&&&&&&fplist[i].roadflag: "<<fplist[i].roadflag<<endl;
        for(j=0; j<fplist[i].s_d.size(); ++j){
            if(fplist[i].s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
                goto ContinueFlag;
        }
        for(j=0; j<fplist[i].s_dd.size(); ++j){
            if(abs(fplist[i].s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
                goto ContinueFlag;
        }
        for(j=30; j<fplist[i].c.size()-30; ++j){
            if(abs(fplist[i].c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
                goto ContinueFlag;
        }
        if(!check_collision(fplist[i]))
            continue;
        okind.push_back(fplist[i]);
        ContinueFlag:continue;
    }
    return okind;
}


bool iv::decition::FrenetPlanner::check_single_path(const iv::decition::Frenet_path &fp){
    int j=0;

    cout<<"&&&&&&&&&&fp.roadflag: "<<fp.roadflag<<endl;
    for(j=0; j<fp.s_d.size(); ++j){
        if(fp.s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
            return false;
    }
    for(j=0; j<fp.s_dd.size(); ++j){
        if(abs(fp.s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
            return false;
    }
    for(j=30; j<fp.c.size()-30; ++j){
        if(abs(fp.c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
            return false;
    }
    if(!check_collision(fp))
        return false;

    return true;
}


/**
 * @brief iv::decition::FrenetPlanner::check_collision
 * 检验frenet路径上是否有障碍物，调用的是改进的函数computeObsOnRoadByFrenet。
 * @param frenet_path          一条frenet路径
 * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
 * @return                     在路径上有障碍物返回false，否则返回true。
 */
bool iv::decition::FrenetPlanner::check_collision(const iv::decition::Frenet_path &frenet_path){
    std::vector<Point2D> gpsTrace = frenet_path_to_gpsTrace(frenet_path);
    double obs=-1;
 //   iv::decition::DecideGps00().computeObsOnRoadByFrenet(this->lidarGridPtr,gpsTrace,obs,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
    if(obs > 0 && obs < 30)
        return false;
    else
        return true;
}


/**
 * @brief iv::decition::FrenetPlanner::frenet_path_to_gpsTrace
 * 将frenet路径转换到车辆坐标系下。
 * @param frenet_path          一条frenet路径
 * @return                     一条车辆坐标系下的路径
 */
vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_path_to_gpsTrace(const Frenet_path& frenet_path){
    vector<iv::Point2D> gpsTrace;
    for (int i=0; i<frenet_path.x.size(); ++i) {
        gpsTrace.push_back(Point2D(frenet_path.x[i],frenet_path.y[i]));
        gpsTrace[i].speed = sqrt(frenet_path.d_d[i]*frenet_path.d_d[i]+frenet_path.s_d[i]*frenet_path.s_d[i])*3.6;
    }
    return gpsTrace;
}


double iv::decition::FrenetPlanner::distance(double x1, double y1, double x2, double y2)
{
    return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}


//找出gpsTrace中距离（x，y）最近的一个点
int iv::decition::FrenetPlanner::ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace)
{

    double closestLen = 100000; //large number
    int closestWaypoint = 0;

    for(int i = 1; i < gpsTrace.size(); i++)
    {
        double map_x = gpsTrace[i].x;
        double map_y = gpsTrace[i].y;
        double dist = distance(x,y,map_x,map_y);
        if(dist < closestLen)
        {
            closestLen = dist;
            closestWaypoint = i;
        }

    }
    return closestWaypoint;
}


 /* |==========================================================|
  * |         车辆坐标系与 frenet 坐标系互转的第一种方法。           |
  * |==========================================================| */
 // 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
 // Transform from Cartesian x,y coordinates to Frenet s,d coordinates
iv::decition::FrenetPoint iv::decition::FrenetPlanner::XY2Frenet(double x, double y, const std::vector<iv::Point2D>& gpsTrace){
    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
//    if(next_wp == 0){
//        prev_wp  = gpsTrace.size()-1;
//    }

    double n_x = gpsTrace[next_wp].x-gpsTrace[prev_wp].x;
    double n_y = gpsTrace[next_wp].y-gpsTrace[prev_wp].y;
    double x_x = x - gpsTrace[prev_wp].x;
    double x_y = y - gpsTrace[prev_wp].y;

    // find the projection of x onto n
    double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y);
    double proj_x = proj_norm*n_x;
    double proj_y = proj_norm*n_y;  //proj_x、proj_y应该为垂足的坐标

    double frenet_d = sqrt((x_x - proj_x) * (x_x - proj_x) + (x_y - proj_y) * (x_y - proj_y));
    //经手动推算frenet_d = abs(n_x * x_y - n_y * x_x) / sqrt(n_x * n_x + n_y * n_y)。
    //即frenet_d，等于点(x,y)到 过prev_wp、next_wp两点的直线 的垂直距离。

    //see if d value is positive or negative by comparing it to a center point
    double center_x = 1000-gpsTrace[prev_wp].x;
    double center_y = 2000-gpsTrace[prev_wp].y;
    double centerToPos = distance(center_x,center_y,x_x,x_y);
    double centerToRef = distance(center_x,center_y,proj_x,proj_y);
    if(centerToPos <= centerToRef){
        frenet_d *= -1;
    }

    // calculate s value
    double frenet_s = 0;
    for(int i = 0; i < prev_wp; i++){
        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
    }
    if(prev_wp<=0){
        frenet_s -= distance(0,0,proj_x,proj_y);
    }else{
        frenet_s += distance(0,0,proj_x,proj_y);
    }

    double tmp_Angle = atan2(n_y,n_x);

    return {x:x,y:y,s:frenet_s,d:frenet_d,tangent_Ang:tmp_Angle};
}

 // frenet坐标转车体坐标。
 // Transform from Frenet s,d coordinates to Cartesian x,y
void iv::decition::FrenetPlanner::Frenet2XY(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace){
    int prev_wp = 0;
    //退出循环时，prev_wp 最大是等于 frenetTrace.size()-2。
    while(s > frenetTrace[prev_wp+1].s && (prev_wp < (int)(frenetTrace.size()-2) )){
        prev_wp++;
    }
    int wp2 = prev_wp+1;
    double heading = atan2((frenetTrace[wp2].y-frenetTrace[prev_wp].y),(frenetTrace[wp2].x-frenetTrace[prev_wp].x));

    double seg_s = (s-frenetTrace[prev_wp].s);
    double seg_x = frenetTrace[prev_wp].x+seg_s*cos(heading);
    double seg_y = frenetTrace[prev_wp].y+seg_s*sin(heading);

    double perp_heading = heading-M_PI*0.5;
    *res_x = seg_x + d*cos(perp_heading);
    *res_y = seg_y + d*sin(perp_heading);
}

/* |==========================================================|
 * |         车辆坐标系与 frenet 坐标系互转的第二种方法。           |
 * |==========================================================| */
// 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
iv::decition::FrenetPoint iv::decition::FrenetPlanner::getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
    //    if(next_wp == 0){
    //        prev_wp  = gpsTrace.size()-1;
    //    }

    GPS_INS gps = Coordinate_UnTransfer(x,y,nowGps);
    Point2D pt = Coordinate_Transfer(gps.gps_x,gps.gps_y, *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);

    // calculate s value
    double frenet_s = 0;
    for(int i = 0; i < prev_wp; i++){
        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
    }
    frenet_s += pt.y;

    //theta为prev_wp处车的航向角 与 nowGps处的车辆坐标系下x轴之间的夹角，逆时针为正。
    double theta = (nowGps.ins_heading_angle - gpsMap[(pathpoint+prev_wp)%gpsMap.size()]->ins_heading_angle+90);
    double tmp_Angle = theta * M_PI / 180;

    return {x:x,y:y,s:frenet_s,d:pt.x,tangent_Ang:tmp_Angle};
}

void iv::decition::FrenetPlanner::getXYfromFrenet(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
{
    int prev_wp = -1;

    while((prev_wp < (int)(frenetTrace.size()-1) ) && s > frenetTrace[prev_wp+1].s)
    {
        prev_wp++;
    }
    if(prev_wp < 0){
        prev_wp = 0;
    }
//    int wp2 =prev_wp+1;
    GPS_INS gps=  Coordinate_UnTransfer( d, s-frenetTrace[prev_wp].s , *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
    Point2D pt =  Coordinate_Transfer( gps.gps_x, gps.gps_y, nowGps);

    *res_x = pt.x;
    *res_y = pt.y;
}


iv::decition::Quintic_polynomial::Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T){
    //计算五次多项式系数
    this->xs = xs;
    this->vxs = vxs;
    this->axs = axs;
    this->xe = xe;
    this->vxe = vxe;
    this->axe = axe;

    this->a0 = xs;
    this->a1 = vxs;
    this->a2 = axs/ 2.0;

    MatrixXd A(3, 3);
    MatrixXd b(3, 1);
    MatrixXd x(3, 1);

    A << pow(T, 3),   pow(T, 4),   pow(T, 5),
         3*pow(T, 2), 4*pow(T, 3), 5*pow(T, 4),
         6*T,         12*T*T,      20*pow(T, 3);
    b << xe - a0 - a1*T - a2*T*T,
         vxe - a1 - 2*a2*T,
         axe - 2*a2;
    x=A.colPivHouseholderQr().solve(b);

    this->a3 = x(0,0);
    this->a4 = x(1,0);
    this->a5 = x(2,0);
}

iv::decition::Quintic_polynomial::~Quintic_polynomial(){
}

double iv::decition::Quintic_polynomial::calc_point(double t){
    double xt = this->a0 + this->a1 * t + this->a2 * t*t + this->a3 * t*t*t + this->a4 * t*t*t*t + this->a5 * t*t*t*t*t;
    return xt;
}

double iv::decition::Quintic_polynomial::calc_first_derivative(double t){
    double xt = this->a1 + 2*this->a2 * t + 3*this->a3 * t*t + 4*this->a4 * t*t*t + 5*this->a5 * t*t*t*t;
    return xt;
}

double iv::decition::Quintic_polynomial::calc_second_derivative(double t){
    double xt = 2*this->a2 + 6*this->a3 * t + 12*this->a4 * t*t + 20*this->a5 * t*t*t;
    return xt;
}

double iv::decition::Quintic_polynomial::calc_third_derivative(double t){
    double xt = 6*this->a3 + 24*this->a4 * t + 60*this->a5 * t*t;
    return xt;
}



iv::decition::Quartic_polynomial::Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T){
    //计算四次多项式系数
    this->xs = xs;
    this->vxs = vxs;
    this->axs = axs;
    this->vxe = vxe;
    this->axe = axe;

    this->a0 = xs;
    this->a1 = vxs;
    this->a2 = axs / 2.0;

    MatrixXd A(2, 2);
    MatrixXd b(2, 1);
    MatrixXd x(2, 1);

    A << 3*pow(T, 2), 4*pow(T, 3),
         6*T,         12*T*T ;
    b << vxe - a1 - 2*a2*T,
         axe - 2*a2;
    x=A.colPivHouseholderQr().solve(b);

    this->a3 = x(0,0);
    this->a4 = x(1,0);
}

iv::decition::Quartic_polynomial::~Quartic_polynomial(){
}

double iv::decition::Quartic_polynomial::calc_point(double t){
    double xt = this->a0 + this->a1*t + this->a2*t*t + this->a3*t*t*t + this->a4*t*t*t*t;
    return xt;
}

double iv::decition::Quartic_polynomial::calc_first_derivative(double t){
    double xt = this->a1 + 2*this->a2*t + 3*this->a3*t*t + 4*this->a4*t*t*t;
    return xt;
}

double iv::decition::Quartic_polynomial::calc_second_derivative(double t){
    double xt = 2*this->a2 + 6*this->a3*t + 12*this->a4*t*t;
    return xt;
}

double iv::decition::Quartic_polynomial::calc_third_derivative(double t){
    double xt = 6*this->a3 + 24*this->a4*t;
    return xt;
}

bool iv::decition::FrenetPlanner::comp(const iv::decition::Frenet_path &a, const iv::decition::Frenet_path &b){
    return a.cf < b.cf;
}