modularization/src1/decision/decision_brain/ivdecision_brain.h

#ifndef IVDECISION_BRAIN_H
#define IVDECISION_BRAIN_H

#include "ivdecision.h"

#include "decition_type.h"
#include "gps_type.h"
#include "constants.h"

#include <vector>


#include <gnss_coordinate_convert.h>
#include <adc_planner/frenet_planner.h>
#include <adc_controller/pid_controller.h>
#include <adc_adapter/ge3_adapter.h>
#include <adc_adapter/qingyuan_adapter.h>
#include <adc_adapter/vv7_adapter.h>
#include <adc_adapter/zhongche_adapter.h>
#include <adc_adapter/bus_adapter.h>
#include <adc_adapter/hapo_adapter.h>
#include <adc_adapter/zhongche_adapter.h>
#include <adc_adapter/yuhesen_adapter.h>
#include "common/perceptionoutput.h"
#include "adc_tools/compute_00.h"
#include "common/car_status.h"
#include "adc_tools/gps_distance.h"
#include "adc_tools/transfer.h"

namespace  iv {



class ivdecision_brain : public ivdecision
{
public:
    enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                    waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
                    dRever,dRever0,dRever1,dRever2,dRever3,dRever4} mvehState;
public:
    ivdecision_brain();

public:
    virtual int getdecision(iv::brain::decition & xdecition,iv::brain::brainstate & xbs);

private:


//    int getdecision_normal(iv::decition::Decition &xdecition,GPS_INS now_gps_ins,
//                                                                       const std::vector<GPSData> gpsMapLine,
//                                                                       iv::LidarGridPtr lidarGridPtr,
//                                                              std::vector<iv::Perception::PerceptionOutput> lidar_per,
//                                                                       iv::TrafficLight trafficLight);

private:
    std::vector<GPSData> mgpsMapLine;

private:

    iv::decition::Decition getDecideFromGPS(GPS_INS now_gps_ins,const std::vector<GPSData> gpsMapLine,
                                                                       iv::LidarGridPtr lidarGridPtr,
                                            std::vector<iv::Perception::PerceptionOutput> lidar_per,
                                                                       iv::TrafficLight trafficLight);

private:
    void updatev2x();
    void updateultra();
    void updateradar();
    void updategroupinfo();
    void updatechassistocarstatus();
    void loadxml();
    void adjuseGpsLidarPosition();
private:
    int mnRunMode = 0;
    bool mbisFirstRun = true;
    int mnPathPoint;


private:
    float  xiuzhengCs=0;

    int  PathPoint = -1;
    double  minDis = iv::MaxValue;
    double  maxAngle = iv::MinValue;
    //int  lastGpsIndex = iv::MaxValue;
    int  lastGpsIndex = 0;
    double  controlSpeed = 0;
    double  controlBrake = 0;
    double  controlAng = 0;
    double  Iv = 0;
    double  lastU = 0;
    double  lastVt = 0;
    double  lastEv = 0;

    int  gpsLineParkIndex = 0;
    int  gpsMapParkIndex = 0;
    double  lastDistance = MaxValue;
    double  lastPreObsDistance = MaxValue;
    double  obsDistance = -1;
    double  obsSpeed=0;
    double  obsDistanceAvoid = -1;
    double  lastDistanceAvoid = -1;

    double  lidarDistance = -1;
    double  myesrDistance = -1;
    double  lastLidarDis = -1;

    bool  parkMode = false;
    bool  readyParkMode = false;

    bool  zhucheMode = false;
    bool  readyZhucheMode = false;
    bool  hasZhuched = false;

    double  lastLidarDisAvoid = -1;
    double  lidarDistanceAvoid = -1;

    int  finishPointNum = -1;
    int  zhuchePointNum = 0;

    ///kkcai, 2020-01-03
    bool  isFirstRun = true;
    //////////////////////////////////////////////

    float  minDecelerate=0;
    //bool  startingFlag = true;//起步标志，在起步时需要进行frenet规划。

    double offset_real = 0;
    double realspeed;
    double dSpeed;
    double dSecSpeed;
    double secSpeed;
    double ac;


    iv::Point2D obsPoint;
    iv::Point2D obsPointAvoid;

//    iv::Point2D obsPoint(-1.0, -1.0);
//    iv::Point2D obsPointAvoid(-1, -1);


    int esrIndex = -1;
    int esrIndexAvoid = -1;
    double obsSpeedAvoid = 0;

    double esrDistance = -1;
    double esrDistanceAvoid = -1;
    int obsLostTime = 0;
    int obsLostTimeAvoid = 0;

    // double avoidTime = 0;


    double avoidX =0;
    float roadWidth = 3.5;
    int roadSum =10;
    int roadNow = 0;
    int roadOri =0;
    int roadPre = -1;
    int lastRoadOri = 0;

    int lightTimes = 0;


    int lastRoadNum=1;

    int stopCount = 0;

    int gpsMissCount = 0;

    bool rapidStop = false;

    bool hasTrumpeted = false;


    bool changeRoad=false;
    //实验手刹
    bool handFirst = true;
    double handTimeSpan = 0;
    double handStartTime = 0;
    double handLastTime = 0;
    bool handPark = false;
    long handParkTime=10000;

    //喇叭
    bool trumpetFirstCount=true;
    double trumpetTimeSpan = 0;
    double trumpetStartTime = 0;
    double trumpetLastTime = 0;

    //过渡
    bool transferFirstCount=true;
    double transferTimeSpan = 0;
    double transferStartTime = 0;
    double transferLastTime = 0;

    bool busMode = false;
    bool parkBesideRoad=false;

    bool chaocheMode = false;
    bool specialSignle = false;

    double offsetX = 0;

    double steerSpeed=9000;

    bool transferPieriod;
    bool transferPieriod2;
    double traceDevi;

    bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
    bool useFrenet = false;    //标志是否启用frenet算法避障
    bool useOldAvoid = true;   //标志是否用老方法避障

//    enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
//                    waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
//                    dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;


    std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
    std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;

    std::vector<double> esrDisVector;
    std::vector<double> lidarDisVector;
    std::vector<double> obsDisVector;
    std::vector<double> obsSpeedVector;

    std::vector<int> obsLostTimeVector;

    std::vector<double> lastLidarDisVector;
    std::vector<double> lastDistanceVector;

//    std::vector<double> esrDisVector(roadSum, -1);
//    std::vector<double> lidarDisVector(roadSum, -1);
//    std::vector<double> obsDisVector(roadSum,-1);
//    std::vector<double> obsSpeedVector(roadSum, 0);

//    std::vector<int> obsLostTimeVector(roadSum, 0);

//    std::vector<double> lastLidarDisVector(roadSum, -1);
//    std::vector<double> lastDistanceVector(roadSum, -1);

    bool qiedianCount = false;


private:
//    static double minDis;
//    static double maxAngle;
//    static int lastGpsIndex;
//    static double lidarDistance;
//    static double myesrDistance;
//    static double lastLidarDis;
//    static double lastLidarDisAvoid;

//    static double lastPreObsDistance;

//    static int gpsLineParkIndex;
//    static int gpsMapParkIndex;

//    static double lastDistance;

//    static float xiuzhengCs;

//    static double controlAng;
//    double laneAng;
//    static double controlSpeed;
//    static double controlBrake;

//    static bool parkMode;
//    static bool readyParkMode;

//    static bool zhucheMode;
//    static bool readyZhucheMode;
//    static bool hasZhuched;
//    static double Iv;
//    static double lastEv;
//    static double lastVt;
//    static double lastU;
//    static double obsDistance;
//    static double obsSpeed;
//    static double lidarDistanceAvoid;
//    static double obsDistanceAvoid;
//    static double lastDistanceAvoid;

    GPS_INS group_ori_gps;
    GPS_INS startAvoid_gps_ins;
//    static int finishPointNum;
//    static int zhuchePointNum;
    double avoidRunDistance=0;

    std::vector<iv::GPS_INS> startAvoidGpsInsVector;
    std::vector<double> avoidMinDistanceVector;
    std::vector<int> v2xTrafficVector;

    ///kkcai, 2020-01-03
    //bool isFirstRun = true;
//    static bool isFirstRun;
    /////////////////////////////////////

//    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
//    static float minDecelerate;


    double startTime = 0;
    double firstTime = 0;

    bool circleMode=false;

    int avoidTimes = 0;
    int backTimes = 0;

    int checkAvoidObsTimes = 0;
    int checkBackObsTimes = 0;

    bool hasCheckedAvoidLidar=false;
    bool hasCheckedBackLidar=false;

    bool personWaited = false;
    bool roadLightWaited = false;

    double decision_Angle = 0;
    double lastAngle=0;

//    iv::Point2D obsPoint;

    int checkAvoidTimes=0;
    int checkBackTimes=0;
    int chaocheCounts=0;

//    static int PathPoint;
    float lastGroupOffsetX=0.0;

    GPS_INS traffic_light_gps;
    int traffic_light_pathpoint;

    bool changingDangWei=false;

    iv::decition::BaseController *pid_Controller;
    iv::decition::BaseAdapter *ge3_Adapter;
    iv::decition::BaseAdapter *qingyuan_Adapter;
    iv::decition::BaseAdapter *vv7_Adapter;
    iv::decition::BaseAdapter *zhongche_Adapter;
    iv::decition::BaseAdapter *bus_Adapter;
    iv::decition::BaseAdapter *hapo_Adapter;
    iv::decition::BaseAdapter *yuhesen_Adapter;

    std::shared_ptr<iv::decition::BaseController> mpid_Controller;
    std::shared_ptr<iv::decition::BaseAdapter> mge3_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> mqingyuan_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> mvv7_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> mzhongche_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> mbus_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> mhapo_Adapter;
    std::shared_ptr<iv::decition::BaseAdapter> myuhesen_Adapter;

    iv::decition::FrenetPlanner *frenetPlanner;

    VehState vehState;
//    BasePlanner *laneChangePlanner;

    std::shared_ptr<iv::decition::FrenetPlanner> mfrenetPlanner;
//    std::shared_ptr<BasePlanner> mlaneChangePlanner;


//    Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
//                              std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);

    void initMethods();
    std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);


    double getAngle(std::vector<Point2D> gpsTrace);
    double getAngle(std::vector<Point2D> gpsTrace, GPS_INS gpsIns,iv::decition::Decition decition);
    double getSpeed(std::vector<Point2D> gpsTrace);

    void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
    void getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
     void getEsrObsDistanceAvoid();

    void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
    void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns );

    double limitAngle(double speed, double angle);
    double limitSpeed(double angle, double speed);

    bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
    bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);

    void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
    void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);

    void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
    void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);

    void predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);

    int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
    int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);

    void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);

    void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);

    bool checkChaoCheBack(iv::LidarGridPtr);
    double trumpet();
    double transferP();

    bool nearStation;

    void updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s);
    void init();

    std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);

    std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);

    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                   int pathpoint,float secSpeed,float dSpeed);
    void getV2XTrafficPositionVector( const std::vector<GPSData> gpsMapLine);
    float ComputeV2XTrafficLightSpeed(iv::TrafficLight TrafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
                                   int pathpoint,float secSpeed,float dSpeed,bool circleMode);
    float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);

    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
    float computeTrafficSpeedLimt(float trafficDis);

    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);

    bool adjuseultra();

    iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);

    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
    GPS_INS aim_gps_ins;
    GPS_INS chaoche_start_gps;
    bool is_arrivaled=false;

    double chaocheObsDis = 0;
    double preChaocheDis = 0;
    double aimObsSpeed = 0;
    double followSpeed = 30;
    double chaocheSpeed = 50;
    int checkChaoCheBackCounts = 0;

    float lastTorque=0;
    float splimit=-1;

    float  ObsTimeSpan=-1;
    double ObsTimeStart=-1;
    double ObsTimeEnd=-1;
    float ObsTimeWidth=1500;
    std::vector<iv::TracePoint> planTrace;
    bool roadNowStart=true;

private:
    iv::decition::Decition decision_reverseCar(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_reversing(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_reverseCircle(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_reverseDirect(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_reverseArr(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever0(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever1(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever2(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever3(iv::GPS_INS now_gps_ins);
    iv::decition::Decition decision_dRever4(iv::GPS_INS now_gps_ins);

    void decision_firstRun(GPS_INS now_gps_ins,
                           const std::vector<GPSData> & gpsMapLine);

    int decision_ParkCalc(GPS_INS now_gps_ins,iv::decition::Decition & gps_decition);

    void decision_readyZhucheMode(GPS_INS now_gps_ins,const std::vector<GPSData> & gpsMapLine);
    void decision_readyParkMode(GPS_INS now_gps_ins,const std::vector<GPSData> & gpsMapLine);
    void decision_speedctrl(iv::decition::Decition & gps_decition,const std::vector<GPSData> & gpsMapLine);

};

}

#endif // IVDECISION_BRAIN_H