#pragma once
#ifndef _IV_DECITION_COMPUTE_00_
#define _IV_DECITION_COMPUTE_00_

#include <gps_type.h>
#include <obstacle_type.h>
#include <decition_type.h>
#include <vector>
//#include <decision/decide_gps_00.h>

#include "adc_planner/frenet_planner.h"

namespace iv {
    namespace decition {
    //根据传感器传输来的信息作出决策
        class Compute00 {
        public:
            Compute00();
            ~Compute00();

            static   double maxAngle;
            static	 double angleLimit;  //角度限制
            static	 double lastEA;      //上一次角度误差
            static	 double lastEP;      //上一次位置误差
            static	 double decision_Angle;  //决策角度
            static	 double lastAng;         //上次角度
            static   int lastEsrID;          //上次毫米波障碍物ID
            static   int  lastEsrCount;      //毫米波障碍物累计次数

            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid

            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
            static double bocheAngle,dBocheAngle;

            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
            static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);

            static int getDesiredSpeed(std::vector<GPSData> gpsMap);

            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
            static double getAveDef(std::vector<Point2D> farTrace);
            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);


            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr,double & lidarDistanceAvoid);
            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint,const double xiuzhengCs);
            static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum,const double xiuzhengCs);
            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);

            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);

            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX
                                              ,const bool readyParkMode,const int gpsLineParkIndex);

            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);

            static double getDecideAngleByLane(double realSpeed);

            static double getDecideAngleByLanePID(double realSpeed);

            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);

            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);

            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);

            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);

            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
                                    double *x_2, double *y_2, double *real_rad);
            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
                                                double ange1,double real_rad,double *x_3, double *y_3);
            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
                                                   double min_rad,double *angle_3);
            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
                                          double real_rad,double angle_3,double R_M,
                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);

            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);

            static int getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps
                                           ,const double xiuzhengCs);
            double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);

        private:

        };
    }
}
extern std::vector<std::vector<iv::GPSData>> gmapsL;
extern std::vector<std::vector<iv::GPSData>> gmapsR;

extern std::vector<int> lastEsrIdVector;
extern std::vector<int> lastEsrCountVector;

#endif // !_IV_DECITION_COMPUTE_00_