#ifndef FRENET_PLANNER_H
#define FRENET_PLANNER_H

#include "base_planner.h"

namespace iv{
namespace decition{

    //x，y分别为frenet轨迹点相对于车辆的x，y轴坐标
    //s，d分别为frenet轨迹点相对于frenet坐标系的s，d轴坐标值
    //tangent_Ang为轨迹点在frenet坐标系的s轴的投影处的 切线 与车辆坐标系x轴之间的夹角。
    //tangent_Ang用于分解计算车辆分别在s轴、d轴方向上的运动速度、加速度等。
    struct FrenetPoint{
        double x;
        double y;
        double s;
        double d;
        double tangent_Ang;
    };



    class Quintic_polynomial
    {
    public:
        Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T);
        ~Quintic_polynomial();
    private:
        // 计算五次多项式系数
        double xs;    //d0
        double vxs;   //d.0
        double axs;   //d..0
        double xe;    //d1
        double vxe;   //d.1
        double axe;   //d..1

        double a0;
        double a1;
        double a2;
        double a3;
        double a4;
        double a5;

    public:
        double calc_point(double t);
        double calc_first_derivative(double t);
        double calc_second_derivative(double t);
        double calc_third_derivative(double t);
    };


    class Quartic_polynomial
    {
    public:
        Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T);
        ~Quartic_polynomial();

    private:
        //计算四次多项式系数
        double xs;
        double vxs;
        double axs;
        double vxe;
        double axe;

        double a0;
        double a1;
        double a2;
        double a3;
        double a4;

    public:
        double calc_point(double t);
        double calc_first_derivative(double t);
        double calc_second_derivative(double t);
        double calc_third_derivative(double t);
    };


    class Frenet_path
    {
    public:
        std::vector<double> t;
        std::vector<double> d;
        std::vector<double> d_d;
        std::vector<double> d_dd;
        std::vector<double> d_ddd;
        std::vector<double> s;
        std::vector<double> s_d;
        std::vector<double> s_dd;
        std::vector<double> s_ddd;
        int roadflag = -1;  //标记每一条frenet路径，是以哪一条道路为目标道路的
        double cd = 0.0;
        double cv = 0.0;
        double cf = 0.0;

        std::vector<double> x;
        std::vector<double> y;
        std::vector<double> yaw;
        std::vector<double> ds;
        std::vector<double> c;
    };

    class FrenetPlanner : public BasePlanner{
    public:
        int aimRoadByFrenet = -1;          //记录由frenet规划选择出来的道路序号
        int roadNow = -1;                  //记录避障时车辆当前道路序号,规划路径时，避开此条道路
        double leftWidth = 0.0;            //中心线左边偏移距离，为负值
        double rightWidth = 0.0;           //中心线右边偏移距离，为正值
        GPS_INS now_gps_ins;               //实时gps信息
        std::vector<GPSData> gpsMapLine;   //地图数据点
        int PathPoint = 0;                 //地图路线中距离车辆位置最近的一个点的序号
        iv::LidarGridPtr lidarGridPtr;     //激光雷达信息网格

    public:
        FrenetPlanner();
        ~FrenetPlanner();

        /**
         * @brief iv::decition::FrenetPlanner::getPath
         * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
         * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
         *
         * @param now_gps_ins          实时gps信息
         * @param gpsMapLine           地图数据点
         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
         * @param lidarGridPtr         激光雷达信息网格
         * @return                     返回一条车辆坐标系下的路径
         */
        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
        int chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow);
        static iv::decition::FrenetPoint XY2Frenet(double x, double y, const std::vector<Point2D>& gpsTracet);
        static void Frenet2XY(double *x, double *y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace);
        static double distance(double x1, double y1, double x2, double y2);
        static int ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace);
        static FrenetPoint getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
        static void getXYfromFrenet(double *res_x, double *res_y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
        static bool comp(const iv::decition::Frenet_path &a,const iv::decition::Frenet_path &b);

    private:
        std::vector<iv::Point2D> getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed);
        std::vector<iv::Point2D> frenet_optimal_planning(FrenetPoint car_now_frenet_point,
                                                         const std::vector<FrenetPoint>& frenet_s,
                                                         double c_speed, double c_d_d, double c_d_dd);
        std::vector<Frenet_path> calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0);
        void calc_global_paths(std::vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s);
        void calc_global_single_path(Frenet_path& fp,const std::vector<FrenetPoint>& frenet_s);
        std::vector<Frenet_path> check_paths(const std::vector<Frenet_path>& fplist);
        bool check_single_path(const Frenet_path &fp);
        bool check_collision(const Frenet_path &frenet_path);

        std::vector<Point2D> frenet_path_to_gpsTrace(const Frenet_path& frenet_path);


    };

}
}

#endif // FRENET_PLANNER_H