modularization/src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

#include <decition/decide_gps_00.h>
#include <decition/adc_tools/compute_00.h>
#include <decition/adc_tools/gps_distance.h>
#include <decition/decition_type.h>
#include <decition/adc_tools/transfer.h>
#include <decition/obs_predict.h>
#include <common/constants.h>
#include <common/car_status.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <control/can.h>
#include <time.h>
#include <decition/adc_controller/base_controller.h>
#include <decition/adc_controller/pid_controller.h>
#include <decition/adc_planner/lane_change_planner.h>
#include <decition/adc_planner/frenet_planner.h>
#include <QTime>
#include <iomanip>

using namespace std;


#include "ivlog.h"
extern iv::Ivlog * givlog;

#define PI (3.1415926535897932384626433832795)

iv::decition::DecideGps00::DecideGps00() {

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

}

float iv::decition::DecideGps00::xiuzhengCs=0;

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

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

double iv::decition::DecideGps00::lidarDistance = -1;
double iv::decition::DecideGps00::myesrDistance = -1;
double iv::decition::DecideGps00::lastLidarDis = -1;

bool iv::decition::DecideGps00::parkMode = false;
bool iv::decition::DecideGps00::readyParkMode = false;

bool iv::decition::DecideGps00::zhucheMode = false;
bool iv::decition::DecideGps00::readyZhucheMode = false;
bool iv::decition::DecideGps00::hasZhuched = false;

double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
double iv::decition::DecideGps00::lidarDistanceAvoid = -1;

int iv::decition::DecideGps00::finishPointNum = -1;
int iv::decition::DecideGps00::zhuchePointNum = 0;

///kkcai, 2020-01-03
bool iv::decition::DecideGps00::isFirstRun = true;
//////////////////////////////////////////////

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

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


iv::Point2D obsPoint(-1, -1);
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;
int    turnLampFlag=0;  //  if <0:left , if >0:right
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;   //标志是否用老方法避障

//数据存储功能 ,20210903,cxw
bool file_cnt_add_en =false;  //用于提示是否需要将文件计数值增加
bool file_cnt_add=false;
bool map_start_point = true;
bool first_start_en=true;  //自主巡迹数据存储用
int  start_tracepoint;
int  end_tracepoint;
//

std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量

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,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY;

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);

std::vector<iv::Point2D> traceOriLeft,traceOriRight;

bool qiedianCount = false;

static int obstacle_avoid_flag=0;
//日常展示

iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                   const std::vector<GPSData> gpsMapLine,
                                                                   iv::LidarGridPtr lidarGridPtr,
                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
                                                                   iv::TrafficLight trafficLight)
{
    //   QTime xTime;
    //    xTime.start();
    //39.14034649083606 117.0863975920476 20507469.630314853  4334165.046101382 353
    Decition gps_decition(new DecitionBasic);
    //    vector<iv::Point2D> fpTraceTmp;




    //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
    if(!(useFrenet^useOldAvoid)){
        useFrenet = false;
        useOldAvoid = true;
    }

    //    //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
    //    if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
    //        useFrenet = false;
    //        useOldAvoid = true;
    //    }

    //  ServiceCarStatus.group_control=false;




    //    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
    //    now_gps_ins.gps_x=gps.gps_x;
    //    now_gps_ins.gps_y=gps.gps_y;

    //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);

  static int file_num; //数据存储，20210903，cxw
    if (isFirstRun)
    {
        file_num=0;
        initMethods();
        vehState = normalRun;
        startAvoid_gps_ins = now_gps_ins;
        init();
        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
                                                          DecideGps00::lastGpsIndex,
                                                          DecideGps00::minDis,
                                                          DecideGps00::maxAngle);
        DecideGps00::lastGpsIndex = PathPoint;


        if(ServiceCarStatus.
                speed_control==true){
            //Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
            Compute00().getPlanSpeed(gpsMapLine);
        }

        //	ServiceCarStatus.carState = 1;
        //        roadOri = gpsMapLine[PathPoint]->roadOri;
        //        roadNow = roadOri;
        //        roadSum = gpsMapLine[PathPoint]->roadSum;
        //  busMode = false;
        //  vehState = dRever;

        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
        if(dis<15){
            circleMode=true;  //201200102
        }else{
            circleMode=false;
        }

        //     circleMode = true;


        getV2XTrafficPositionVector(gpsMapLine);
        //        group_ori_gps=*gpsMapLine[0];
        ServiceCarStatus.bocheMode=false;
        ServiceCarStatus.daocheMode=false;
        parkMode=false;
        readyParkMode=false;
        finishPointNum=-1;
        roadNowStart=true;
        isFirstRun = false;
        obstacle_avoid_flag=0;

        givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
                        0,0);
    }


    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
        now_gps_ins.gps_x=gpsOffset.gps_x;
        now_gps_ins.gps_y=gpsOffset.gps_y;
        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
    }



    //1227
    //  ServiceCarStatus.daocheMode=true;

    //1220
    changingDangWei=false;

    minDecelerate=0;
    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
        //  int a=0;
        gps_decition->wheel_angle = 0;
        gps_decition->speed = dSpeed;
        gps_decition->torque =0; //20210906,cxw
        gps_decition->accelerator = -0.5;
        gps_decition->brake=10;
        return gps_decition;
    }





    //1220


    if(ServiceCarStatus.daocheMode){

        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
        if( now_gps_ins.ins_heading_angle>=360)
            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
    }
    //1220 end



    //1125 traficc
    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);


    //xiesi
    ///kkcai, 2020-01-03
    //ServiceCarStatus.busmode=true;
    //ServiceCarStatus.busmode=false;//20200102
    ///////////////////////////////////////////////////


    busMode = ServiceCarStatus.busmode;
    nearStation=false;

    gps_decition->leftlamp = false;
    gps_decition->rightlamp = false;
    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);


    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;

    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;




    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
    is_arrivaled = false;




    //  xiuzhengCs=-0.8;  //1026

    xiuzhengCs=0;
    //    if (parkMode)
    //    {


    //        handBrakePark(gps_decition,10000,now_gps_ins);
    //        return gps_decition;
    //    }





    realspeed = now_gps_ins.speed;

    secSpeed = realspeed / 3.6;




    //sidePark

    if(ServiceCarStatus.mnParkType==1){

        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
            if(bocheEN==1){
                ServiceCarStatus.bocheEnable=1;
            }else if(bocheEN==0){
                ServiceCarStatus.bocheEnable=0;
            }
        }else{
            ServiceCarStatus.bocheEnable=2;
        }



        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&
                vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4&&  vehState!=reverseArr ){
            if(abs(realspeed)>0.1){
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
                gps_decition->wheel_angle=0;
                return gps_decition;
            }else{
                if(trumpet()>3000){
                    trumpetFirstCount=true;
                    vehState=dRever;
                }

            }
            //         ServiceCarStatus.bocheMode=false;
        }

    }


    //chuizhiPark

    if(ServiceCarStatus.mnParkType==0){
        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr ){
            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
            if(bocheEN==1){
                ServiceCarStatus.bocheEnable=1;
            }else if(bocheEN==0){
                ServiceCarStatus.bocheEnable=0;
            }
        }else{
            ServiceCarStatus.bocheEnable=2;
        }



        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){

            if(abs(realspeed)>0.1){
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
                gps_decition->wheel_angle=0;
                return gps_decition;
            }else{
                if(trumpet()>3000){
                    trumpetFirstCount=true;
                    vehState=reverseCar;
                }

            }
            //     ServiceCarStatus.bocheMode=false;

        }
    }
    // ChuiZhiTingChe
    if (vehState == reverseCar)
    {
        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);

        vehState = reversing;
        qiedianCount=true;


    }
    if (vehState == reversing)
    {
        double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
        if (dis<2.0)//0.5
        {
            vehState = reverseCircle;
            qiedianCount = true;
            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        }
        else {
            controlAng = 0;
            dSpeed = 2;
            dSecSpeed = dSpeed / 3.6;
            gps_decition->wheel_angle = 0;
            gps_decition->speed = dSpeed;
            obsDistance=-1;
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            //	gps_decition->accelerator = 0;
            return gps_decition;
        }
    }

    if (vehState == reverseCircle)
    {
        double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
        {
            vehState = reverseDirect;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<0)
                //         if (qiedianCount && trumpet()<1500)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = Compute00().bocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng*1.05;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }

    if (vehState == reverseDirect)
    {
        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);

        if ((pt.y)<0.5)
        {

            qiedianCount=true;
            vehState=reverseArr;
            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
            //            gps_decition->accelerator = -3;
            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            gps_decition->wheel_angle=0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            return gps_decition;




        }
        else {

            //if (qiedianCount && trumpet()<0)
            if (qiedianCount && trumpet()<1000)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;
                dSpeed = 1;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }



            controlAng = 0;

            gps_decition->wheel_angle = 0;

            return gps_decition;
        }
    }



    if (vehState == reverseArr)
    {
        //

        ServiceCarStatus.bocheMode=false;
        if (qiedianCount && trumpet()<1500)
        {

            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            ServiceCarStatus.bocheMode=false;
        }
        else
        {
            qiedianCount = false;
            trumpetFirstCount = true;
            ServiceCarStatus.bocheEnable=0;
            vehState=normalRun;
            ServiceCarStatus.mbRunPause=true;
            ServiceCarStatus.mnBocheComplete=100;
            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
        }


        gps_decition->wheel_angle = 0 ;



        return gps_decition;

    }



    //ceFangWeiBoChe

    if (vehState == dRever)
    {
        GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);

        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);

        vehState = dRever0;
        qiedianCount=true;


        std::cout<<"enter side boche mode"<<std::endl;



    }
    if (vehState == dRever0)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
        if (dis<2.0)
        {
            vehState = dRever1;
            qiedianCount = true;
            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
        }
        else {
            controlAng = 0;
            dSpeed = 2;
            dSecSpeed = dSpeed / 3.6;
            gps_decition->wheel_angle = 0;
            gps_decition->speed = dSpeed;
            obsDistance=-1;
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            //	gps_decition->accelerator = 0;
            return gps_decition;
        }
    }


    if (vehState == dRever1)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);

        if(dis<2.0)
        {
            vehState = dRever2;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = Compute00().dBocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }


    if (vehState == dRever2)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
        double xx= (pt1.x-pt2.x);
        // if(xx>0)
        if(xx>-0.5)
        {
            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
            Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
            double xxx=ptt.x;
            double yyy =ptt.y;
            //            if(xxx<-1.5||xx>1){
            //                int a=0;
            //            }
            vehState = dRever3;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
            cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
            cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;


        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                /*  gps_decition->brake = 10;
                gps_decition->torque = 0;  */
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }


            gps_decition->wheel_angle = 0 ;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }


    if (vehState == dRever3)
    {
        double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
        {
            vehState = dRever4;
            qiedianCount = true;
            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;

        }
        else {
            if (qiedianCount && trumpet()<1000)

            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;


                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            }

            controlAng = 0-Compute00().dBocheAngle*16.5;
            gps_decition->wheel_angle = 0 - controlAng*0.95;

            cout<<"farTpointDistance================"<<dis<<endl;

            return gps_decition;
        }
    }



    if (vehState == dRever4)
    {
        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);

        if ((pt.y)<0.5)
        {

            qiedianCount=true;
            vehState=reverseArr;
            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
            //            gps_decition->accelerator = -3;
            //            gps_decition->brake =10 ;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);

            gps_decition->wheel_angle=0;
            return gps_decition;




        }
        else {

            //if (qiedianCount && trumpet()<0)
            if (qiedianCount && trumpet()<1000)
            {

                //                gps_decition->brake = 10;
                //                gps_decition->torque = 0;
                dSpeed=0;
                minDecelerate=min(minDecelerate,-0.5f);
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }
            else
            {
                qiedianCount = false;
                trumpetFirstCount = true;
                dSpeed = 2;
                dSecSpeed = dSpeed / 3.6;
                gps_decition->speed = dSpeed;
                obsDistance=-1;
                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
            }



            controlAng = 0;

            gps_decition->wheel_angle = 0;

            return gps_decition;
        }
    }



    if (vehState == reverseArr)
    {
        //
        ServiceCarStatus.bocheMode=false;

        if (qiedianCount && trumpet()<1500)
        {

            //            gps_decition->brake = 10;
            //            gps_decition->torque = 0;
            dSpeed=0;
            minDecelerate=min(minDecelerate,-0.5f);
            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
        }
        else
        {
            qiedianCount = false;
            trumpetFirstCount = true;
            ServiceCarStatus.bocheEnable=0;
            vehState=normalRun;
            ServiceCarStatus.mbRunPause=true;
            ServiceCarStatus.mnBocheComplete=100;
            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
        }


        gps_decition->wheel_angle = 0 ;



        return gps_decition;

    }

    obsDistance = -1;
    esrIndex = -1;
    lidarDistance = -1;

    obsDistanceAvoid = -1;
    esrIndexAvoid = -1;
    roadPre = -1;
    //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;

    gpsTraceOri.clear();
    gpsTraceNow.clear();
    gpsTraceAim.clear();
    gpsTraceAvoid.clear();
    gpsTraceBack.clear();
    gpsTraceNowLeft.clear();
    gpsTraceNowRight.clear();



    if (!isFirstRun)
    {
            PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
            if(PathPoint<0){
                PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
            }

    }


    if (PathPoint<0)
    {

        minDecelerate=-1.0;
//        if((ServiceCarStatus.msysparam.mvehtype=="pixloop")||(ServiceCarStatus.msysparam.mvehtype=="yika")
//               || (ServiceCarStatus.msysparam.mvehtype=="hunter"))
//        {
//            dSpeed=0.0;   //20210903,cxw,这三种车型都是用决策速度控制的
//        }
        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
        return gps_decition;

    }

    DecideGps00::lastGpsIndex = PathPoint;

    double brake_distance=200;
    brake_distance=max(250,(int)(dSpeed*dSpeed+150));
    if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
            minDecelerate=-0.5;
//            if((ServiceCarStatus.msysparam.mvehtype=="pixloop")||(ServiceCarStatus.msysparam.mvehtype=="yika")
//                    || (ServiceCarStatus.msysparam.mvehtype=="hunter"))
//            {
//                dSpeed=0.0;   //20210622,cxw,地图到达终点决策速度设置为0,防止车子乱动
//            }
            std::cout<<"map finish brake"<<std::endl;
    }

    if(!circleMode){

            double forecast_final=secSpeed*secSpeed+5;
            int forecast_final_point=((int)forecast_final)*10;
            static int BrakePoint=-1;
            static bool final_brake=false,final_brake_lock=false;
            if(PathPoint+forecast_final_point<gpsMapLine.size())
            {
                if(gpsMapLine[PathPoint+forecast_final_point]->mode2==23 && realspeed>3){
                                            final_brake=true;
                                            if(BrakePoint==-1)
                                                 BrakePoint=PathPoint-10;
                }
            }
            if(PathPoint<BrakePoint)
            {
                final_brake=false;
                final_brake_lock=false;
                BrakePoint=-1;
            }
            if(final_brake==true){
                    if((realspeed>3)&&(final_brake_lock==false)){
                                minDecelerate=-0.5;
                    }else{
                                dSpeed=min(dSpeed, 3.0);
                                final_brake_lock=true;
                                if(PathPoint+50<gpsMapLine.size()){
                                    if(gpsMapLine[PathPoint+50]->mode2==23){
                                       minDecelerate=-0.5;      //主master核对，check                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     =-0.5;
                                    }
                                }
                    }
            }
//            if(PathPoint+500<gpsMapLine.size())
//            {
//                if(gpsMapLine[PathPoint+500]->mode2==23 && realspeed>20)
//                                        minDecelerate=-0.5;
//                else if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
//                                        minDecelerate=-0.5;
//                else if(gpsMapLine[PathPoint+100]->mode2==23)
//                                        minDecelerate=-0.6;
//            }
//            else if(PathPoint+300<gpsMapLine.size()){
//                if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
//                                        minDecelerate=-0.5;
//                else if(gpsMapLine[PathPoint+100]->mode2==23)
//                                        minDecelerate=-0.6;
//            }
//            else if(PathPoint+150<gpsMapLine.size()){
//                if(gpsMapLine[PathPoint+150]->mode2==23 && realspeed>10)
//                                        minDecelerate=-0.5;
//                else if(gpsMapLine[PathPoint+100]->mode2==23)
//                                        minDecelerate=-0.6;
//            }
//            else if(PathPoint+100<gpsMapLine.size()){
//                    if(gpsMapLine[PathPoint+100]->mode2==23)
//                                        minDecelerate=-0.6;
//            }
    }

    if(!ServiceCarStatus.inRoadAvoid){
        roadOri = gpsMapLine[PathPoint]->roadOri;
        roadSum = gpsMapLine[PathPoint]->roadSum;      
        givlog->debug("decition_brain","roadOri: %d",
                        roadOri);
    }else{
        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
        roadSum = gpsMapLine[PathPoint]->roadSum*3;
    }

//    roadOri =0;
//    roadSum =2;

    if(roadNowStart){
        roadNow=roadOri;
        roadNowStart=false;
    }


    if(ServiceCarStatus.avoidObs){
         gpsMapLine[PathPoint]->runMode=1;
    }else{
         gpsMapLine[PathPoint]->runMode=0;
    }
    if(obstacle_avoid_flag==1){
        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
    }else{
        avoidX=0;
        roadNow = roadOri;
        if(vehState==backOri){
            vehState=normalRun;
        }
    }
    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
    {
        vehState = normalRun;
        roadNow = roadOri;
    }

    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);

    if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0)&&(obstacle_avoid_flag==1))
    {
        gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
    }

    if(gpsTraceOri.empty()){
        gps_decition->wheel_angle = 0;
        gps_decition->speed = dSpeed;
        gps_decition->torque=0;//20210906,cxw
        gps_decition->accelerator = -0.5;
        gps_decition->brake=10;
        return gps_decition;
    }



    traceDevi=gpsTraceOri[0].x;

    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
    //        startingFlag = false;
    //    }
    startingFlag = false;
    if(startingFlag){
        //        gpsTraceAim = gpsTraceOri;
        if(abs(gpsTraceOri[0].x)>1){
            cout<<"frenetPlanner->getPath:pre"<<endl;
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
            if(gpsTraceNow.size()==0){
                gps_decition->accelerator = 0;
                gps_decition->brake=10;
                gps_decition->wheel_angle = lastAngle;
                gps_decition->speed = 0;
                return gps_decition;
            }
        }else{
            startingFlag = false;
        }
    }


    if(useFrenet){
        //如果车辆在变道中，启用frenet规划局部路径
        if(vehState == avoiding || vehState == backOri){
           // avoidX = (roadOri - roadNow)*roadWidth;
             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
            if(gpsTraceNow.size()==0){
                gps_decition->accelerator = 0;
                gps_decition->brake=10;
                gps_decition->wheel_angle = lastAngle;
                gps_decition->speed = 0;
                return gps_decition;
            }
        }
    }

    if(gpsTraceNow.size()==0){
        if (roadNow==roadOri)
        {
            if(vehState == backOri)
            {
                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
                getGpsTraceNowLeftRight(gpsTraceNow);
            }
            else{
            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
            gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
            gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
            }
        }else
        {
            if((vehState == avoiding)||(vehState == backOri))
            {
                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
                getGpsTraceNowLeftRight(gpsTraceNow);
            }else{
                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
                getGpsTraceNowLeftRight(gpsTraceNow);
            }
        }
    }

    if(vehState==normalRun)
    {
        if(gpsTraceNow.size()>200){
            if(gpsTraceNow[200].x<-3){
                gps_decition->leftlamp = true;
                gps_decition->rightlamp = false;
            }else if(gpsTraceNow[200].x>3){
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = true;
            }else{
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = false;
            }
        }
    }

    //  dSpeed = getSpeed(gpsTraceNow);
    dSpeed =80;
    planTrace.clear();//Add By YuChuli, 2020.11.26
    for(int i=0;i<gpsTraceNow.size();i++){
        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
        planTrace.push_back(pt);
    }

    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
    if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
        if(realspeed<0.5)
            controlAng=0;
    }



    //1220
    if(ServiceCarStatus.daocheMode){
        controlAng=0-controlAng;
    }

    //2020-0106
    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){

        if(vehState==avoiding){
            ServiceCarStatus.msysparam.vehWidth=2.4;
            controlAng=max(-150.0,controlAng);//35  zj-150
            controlAng=min(150.0,controlAng);//35   zj-150
        }
        if(vehState==backOri){
            controlAng=max(-150.0,controlAng);//35   zj-150
            controlAng=min(150.0,controlAng);//35     zj-150
        }
    }

//    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
//            vehState,controlAng);


    //准备驻车
    if (readyZhucheMode)
    {

        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
        cout<<"zhuche point : "<<zhuchePointNum<<endl;

        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);

        if (dis<35)
        {
            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
            double zhucheDistance = pt.y;
#if 0

            if (dSpeed > 15)
            {
                dSpeed = 15;
            }



            if (zhucheDistance < 20 && dis < 25)
            {
                dSpeed = min(dSpeed, 5.0);
            }
#else
            if (dSpeed > 12)
            {
                dSpeed = 12;
            }



            if (zhucheDistance < 9 && dis < 9)
            {
                dSpeed = min(dSpeed, 5.0);
            }
#endif


            if (zhucheDistance < 3.0 && dis < 3.5)
            {
                dSpeed = min(dSpeed, 5.0);
                zhucheMode = true;
                readyZhucheMode = false;
                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;

            }

        }

    }





    if (readyParkMode)
    {
        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
        hasZhuched = true;

        if (parkDis < 25)
        {
            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
            double parkDistance = pt.y;


            if (dSpeed > 15)
            {
                dSpeed = 15;
            }

            //dSpeed = 15;//////////////////////////////

            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
            {
                dSpeed = min(dSpeed, 8.0);
            }else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0){
                dSpeed = min(dSpeed, 5.0);
            }else if(parkDistance < 3.5 && parkDis<4.0){
                dSpeed = min(dSpeed, 3.0);
            }


            //            float stopDis=2;
            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
            //                stopDis=1.6;
            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
            //                stopDis=1.8;
            //            }

            if (parkDistance < 1.6  && parkDis<2.0)
            {
                // dSpeed = 0;
                parkMode = true;
                readyParkMode = false;
            }


        }
    }

    if(ServiceCarStatus.speed_control==true){
            dSpeed = min(doubledata[PathPoint][4],dSpeed);
    }
    else
    {
            if (gpsMapLine[PathPoint]->roadMode ==0)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = false;

            }else  if (gpsMapLine[PathPoint]->roadMode == 5)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = false;

            }else if (gpsMapLine[PathPoint]->roadMode == 11)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);

            }else if (gpsMapLine[PathPoint]->roadMode == 14)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
                gps_decition->leftlamp = true;
                gps_decition->rightlamp = false;
            }else if (gpsMapLine[PathPoint]->roadMode == 15)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = true;
            }else if (gpsMapLine[PathPoint]->roadMode == 16)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
                gps_decition->leftlamp = true;
                gps_decition->rightlamp = false;
            }else if (gpsMapLine[PathPoint]->roadMode == 17)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
                gps_decition->leftlamp = false;
                gps_decition->rightlamp = true;
            }else if (gpsMapLine[PathPoint]->roadMode == 18)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
            }else if (gpsMapLine[PathPoint]->roadMode == 1)
            {
                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
            }else if (gpsMapLine[PathPoint]->roadMode == 2)
            {
                dSpeed = min(15.0,dSpeed);
            }
            else if (gpsMapLine[PathPoint]->roadMode == 7)
            {
                dSpeed = min(15.0,dSpeed);
                xiuzhengCs=1.5;
            }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
            {
                dSpeed = min(4.0,dSpeed);

            }else{
                    dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
                    gps_decition->leftlamp = false;
                    gps_decition->rightlamp = false;

            }
    }

    if (gpsMapLine[PathPoint]->speed_mode == 2)
    {
        dSpeed = min(25.0,dSpeed);

    }

    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
    {
        dSpeed = min(8.0,dSpeed);
    }

    if((gpsMapLine[PathPoint]->speed)>0.001)
    {
        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
//        if((gpsMapLine[PathPoint]->speed)>4.5)
//        {
//            dSpeed =gpsMapLine[PathPoint]->speed*3.6;
//        }
    }


    dSpeed = limitSpeed(controlAng, dSpeed);
    dSecSpeed = dSpeed / 3.6;
//    givlog->debug("brain_decition_speed","dspeed: %f",
//                  dSpeed);


    if(vehState==changingRoad){
        if(gpsTraceNow[0].x>1.0){
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }else if(gpsTraceNow[0].x<-1.0){
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }else{
            vehState==normalRun;
        }
    }

    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
    static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
    if(!ServiceCarStatus.daocheMode){
        //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
        computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);


        if(obs_filter_flag==0)
        {
            if(obsDistance>0&&obsDistance<60)
            {
                    obs_filter++;
                    if(obs_filter<20)                          //80
                    {
                        obsDistance=-1;
                        obsSpeed=0;
                    }
                    else
                    {
                        obs_filter_flag=1;
                        obs_filter_dis_memory=obsDistance;
                        obs_filter_speed_memory=obsSpeed;
                        obs_filter=0;
                    }
            }else{
                obs_filter=0;
            }
        }else
        {
            if(obsDistance<0||obsDistance>60)
            {
                obs_filter++;
                if(obs_filter<20)                           //80
                {
                    obsDistance=obs_filter_dis_memory;
                    obsSpeed=obs_filter_speed_memory;
                }
                else
                {
                    obs_filter_flag=0;
                    obs_filter=0;
                }
            }else
            {
                obs_filter=0;
                obs_filter_dis_memory=obsDistance;
                obs_filter_speed_memory=obsSpeed;
            }
        }


        obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;


    }else{
        gpsTraceRear.clear();
        for(int i=0;i<gpsTraceNow.size();i++){
            Point2D pt;
            pt.x=0-gpsTraceNow[i].x;
            pt.y=0-gpsTraceNow[i].y;
            gpsTraceRear.push_back(pt);
        }
        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
        //  obsDistance=-1; //1227
    }
    //old_bz--------------------------------------------------------------------------------------------------

    if (vehState == avoiding)
    {
        double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins);
        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
            vehState = normalRun;
            //            useFrenet = false;
            //            useOldAvoid = true;
        }
        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
            // vehState = avoidObs; 0929
            vehState = normalRun;
            turnLampFlag=0;
        }
        else if (turnLampFlag>0)
        {
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }
        else if(turnLampFlag<0)
        {
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }
    }


    if (vehState==preBack)
    {
        vehState = backOri;
    }


    if (vehState==backOri)
    {
        double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
            vehState = normalRun;
            turnLampFlag=0;
            //            useFrenet = false;
            //            useOldAvoid = true;
        }
        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
            // vehState = avoidObs; 0929
            vehState = normalRun;
            obstacle_avoid_flag=0;
        }
        else if (turnLampFlag>0)
        {
            gps_decition->leftlamp = false;
            gps_decition->rightlamp = true;
        }
        else if (turnLampFlag<0)
        {
            gps_decition->leftlamp = true;
            gps_decition->rightlamp = false;
        }
    }

    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
    //   计算回归原始路线

    if ((roadNow!=roadOri))
    {
        if(useFrenet){
            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
            {
                double offsetL = -(roadSum - 1)*roadWidth;
                double offsetR = (roadOri - 0)*roadWidth;
                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
            }
        }
        else if(useOldAvoid){
            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
          //  avoidX = (roadOri - roadNow)*roadWidth; //1012
             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        }
    }
    if ((roadNow != roadOri && roadPre!=-1))
    {

        roadNow = roadPre;
   //     avoidX = (roadOri - roadNow)*roadWidth;
        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);

        if(avoidX<0)
            turnLampFlag<0;
        else if(avoidX>0)
            turnLampFlag>0;
        else
            turnLampFlag=0;

        if(useOldAvoid){
            //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
            gpsTraceAvoidXY.clear();
            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
            startBackGpsIns = now_gps_ins;
        }
        else if(useFrenet){
            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
        }

        vehState = backOri;
        hasCheckedBackLidar = false;
        //  checkBackObsTimes = 0;

        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;

    }

    static bool avoid_speed_flag=false;
    /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
            (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
    {
        minDecelerate=-0.4;
        avoid_speed_flag=true;
    }*/


    //shiyanbizhang 0929
    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
            &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
//        ObsTimeStart=GetTickCount();
        ObsTimeEnd+=1.0;
        //dSpeed = min(6.0,dSpeed);
        cout<<"\n====================preAvoid time count start==================\n"<<endl;
    }
//    if(ObsTimeStart!=-1){
//        ObsTimeEnd=GetTickCount();
//    }
//    if(ObsTimeEnd!=-1){
//        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
//    }



//    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
//        vehState=avoidObs;
//        ObsTimeStart=-1;
//        ObsTimeEnd=-1;
//        ObsTimeSpan=-1;
//        avoid_speed_flag=false;
//        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
//    }




//    if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
//            ObsTimeEnd+=1.0;
//    }

    if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
        vehState=avoidObs;
        ObsTimeStart=-1;
        ObsTimeEnd=-1;
        ObsTimeSpan=-1;
        avoid_speed_flag=false;
        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
    }

    if(obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis){
        ObsTimeStart=-1;
        ObsTimeEnd=-1;
        ObsTimeSpan=-1;
        avoid_speed_flag=false;
    }

    //避障模式
    if (vehState == avoidObs   || vehState==waitAvoid ) {

        //      if (obsDistance <20 && obsDistance >0)
        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
        {
            dSpeed = min(6.0,dSpeed);
            avoidTimes++;
            //          if (avoidTimes>=6)
            if (avoidTimes>=ServiceCarStatus.aocfTimes)
            {
                vehState = preAvoid;
                avoidTimes = 0;
            }

        }
        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
        //        {
        //            dSpeed = 10;
        //            avoidTimes = 0;
        //        }
        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
        {
            dSpeed =  min(15.0,dSpeed);
            avoidTimes = 0;
        }
        else
        {
            avoidTimes = 0;
        }

    }



      //givlog->debug("brain_decition","mfRoadwidth: %f",
                   //gpsMapLine[PathPoint]->mfLaneWidth );


    if (vehState == preAvoid)
    {
        cout<<"\n====================preAvoid==================\n"<<endl;
        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
        {
            // vehState = avoidObs; 0929
            vehState=normalRun;
        }
        else
        {
            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
            if(useOldAvoid){
                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            }
            else if(useFrenet){
                double offsetL = -(roadSum - 1)*roadWidth;
                double offsetR = (roadOri - 0)*roadWidth;
                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
            }

            if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
            {
                //  vehState = waitAvoid; 0929
                vehState = avoidObs;
            }
            else if (roadPre != -1)
            {
                if(useOldAvoid){
                    roadNow = roadPre;
                  //  avoidX = (roadOri - roadNow)*roadWidth;
                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);

                     if(avoidX<0)
                         turnLampFlag<0;
                     else if(avoidX>0)
                         turnLampFlag>0;
                     else
                         turnLampFlag=0;

                    gpsTraceNow.clear();
                    //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
                    gpsTraceAvoidXY.clear();
                    gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX,now_gps_ins);
                    startAvoidGpsIns = now_gps_ins;
                }
                else if(useFrenet){
                    if(roadPre != roadNow){
                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                        startAvoidGpsInsVector[roadNow] = now_gps_ins;
                    }
                    roadNow = roadPre;
                //    avoidX = (roadOri - roadNow)*roadWidth;
                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                    gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
                }


                vehState = avoiding;
                obstacle_avoid_flag=1;

                hasCheckedAvoidLidar = false;

                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;

            }
        }
    }

    //--------------------------------------------------------------------------------old_bz_end


    if (parkMode)
    {
        minDecelerate=-1.0;

        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
            parkMode=false;
        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
            parkMode=false;
        }

    }



    //驻车

    if (zhucheMode)
    {
        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;

        //        if(trumpet()<16000)
        if (trumpet()<mzcTime)
        {
            //            if (trumpet()<12000){
            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
            minDecelerate=-1.0;
            if(abs(realspeed)<0.2){
                controlAng=0;  //tju
            }
        }
        else
        {
            hasZhuched = true; //1125
            zhucheMode = false;
            trumpetFirstCount = true;
            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
        }

    }


    //判断驻车标志位
    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
    {
        hasZhuched = false;
    }

    if ( vehState==changingRoad || vehState==chaocheBack)
    {
        double lastAng = 0.0 - lastAngle;

        if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){

            if (controlAng>40)
            {
                controlAng =40;
            }
            else if (controlAng<-40)
            {
                controlAng = - 40;
            }
        }

    }


    //速度结合角度的限制
    controlAng = limitAngle(realspeed, controlAng);


    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;



    //1220
    if(PathPoint+80<gpsMapLine.size()-1){
        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
            changingDangWei=true;
        }
    }

    if(changingDangWei){
        if(abs(realspeed)>0.1){
            dSpeed=0;
        }else{
            dSpeed=0;
            gps_decition->dangWei=2;
            ServiceCarStatus.daocheMode=true;
        }
    }
    //1220 end




    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
    {
        GPS_INS gpsIns;
        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);

        double dis = GetDistance(gpsIns, now_gps_ins);
        if(dis<20)
            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
    }




    if (ServiceCarStatus.carState == 0 && busMode)
    {

        minDecelerate=-1.0;
    }



    if (ServiceCarStatus.carState == 3 && busMode)
    {

        if(realspeed<0.1){
            ServiceCarStatus.carState=0;
            minDecelerate=-1.0;
        }else{
            nearStation=true;
            dSpeed=0;
        }

    }

    ///kkcai, 2020-01-03
    //if (ServiceCarStatus.carState == 2 && busMode)
    if (ServiceCarStatus.carState == 2)
    {

        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
        qDebug("lat:%f", aim_gps_ins.gps_lat);
        qDebug("lon:%f", aim_gps_ins.gps_lng);

        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
        double dis = GetDistance(aim_gps_ins, now_gps_ins);
        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);

        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;

        //         if (dis<20 && pt.y<8&& realspeed<0.1)
        if (dis<20 && pt.y<5 && abs(pt.x)<3)
        {
            dSpeed = 0;
            nearStation=true;
            //is_arrivaled = true;
            //ServiceCarStatus.status[0] = true;
            ServiceCarStatus.istostation=1;
            minDecelerate=-1.0;
        }

        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
        {
            nearStation=true;
            dSpeed = min(8.0, dSpeed);
        }
        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
        {
            dSpeed = min(15.0, dSpeed);
        }

        else if (dis<50 && abs(pt.x)<3)
        {
            dSpeed = min(20.0, dSpeed);
        }
    }


    dSecSpeed = dSpeed / 3.6;
    gps_decition->speed = dSpeed;



    if (gpsMapLine[PathPoint]->runMode == 2)
    {
        obsDistance = -1;

    }

    std::cout<<"juecesudu0="<<dSpeed<<std::endl;



    //-----------------------------------------1+X采集车车路协同,add---------------------------------------------
    unsigned char traffic_type=0;//路况信息
    unsigned char warning_type=0;//预警信息，RSU获得
    float distance_to_center=0;
    float radiation_distance=0;//事件辐射范围，从RSU获得
    float limit_spd=0; //从RSU获得的限速值
    //以上变量信息都需要存储到log文件中
//    distance_to_center=GetDistance(now_gps_ins,gps_center);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
    if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
    {
       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
       {
          dSpeed = min(1.0,realspeed-0.5);
       }
       else if(distance_to_center<radiation_distance)
       {
           dSpeed=0.0;
           minDecelerate=-2.0;
       }
       else
       {}
    }
   else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
    {
       if(distance_to_center<radiation_distance)
       {
          dSpeed=min((double)limit_spd,realspeed-0.5);
       }
    }
//碰撞预警，1减速，2 停车
    if(warning_type==0x01)
    {
        dSpeed=limit_spd;//此处要判断限速值是不是在有效值范围以内
    }
    else if(warning_type==0x01)
    {
        dSpeed=0.0;
    }
    //-----------------------------------------1+X采集车车路协同，end---------------------------------------------
    //-------------------------------traffic light----------------------------------------------------------------------------------------

    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
        //    traffic_light_pathpoint=130;
        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
                                                     traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
        dSpeed = min((double)traffic_speed,dSpeed);
        if(traffic_speed==0){
            minDecelerate=-2.0;
        }
    }
    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------


    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------

    //vbox传递的红绿灯信息中，1是绿灯，2是红灯，3是黄灯

    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine,  v2xTrafficVector,
                                                         PathPoint, secSpeed, dSpeed,  circleMode);


    dSpeed = min(v2xTrafficSpeed,dSpeed);//???这里是不是应该是max,因为V2Xspeed应该是穿过红绿灯的最低速度吧？速度还不能超过当前的限速，如果V2Xspeed超过限速，需要判断车辆是否要
    //停车？

    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------


    transferGpsMode2(gpsMapLine);

//    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
//        if(obsDistance>0 && obsDistance<8){
//                dSpeed=0;
//            }
//    }else if(obsDistance>0 && obsDistance<15){
//        dSpeed=0;
//    }

//    if(ServiceCarStatus.group_control){
//        dSpeed = ServiceCarStatus.group_comm_speed;
//    }
//    if(dSpeed==0){
//        if(realspeed<6)
//            minDecelerate=min(-0.5f,minDecelerate);
//        else
//            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
//    }

    gps_decition->wheel_angle = 0.0 - controlAng;

    //需要增加接收到路册单元限速或停车的命令后对dspeed或这minDecelerate做进一步限制，仅限于hunter车辆
    //当收到停车命令后，直接给出最小减速度
    //收到减速命令后，直接将目标速度给成限速值
    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);


    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d",
            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2);

   //shuju cunchu gognnenng add,20210624,cxw
   if(ServiceCarStatus.txt_log_on==true)
   {
   //    static int file_num=0;
//       double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[0]);
//       double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[gpsMapLine.size()-1]);
       if(first_start_en)
       {
          first_start_en=false;
          start_tracepoint = PathPoint;
          if(start_tracepoint==0)
          {

              end_tracepoint =gpsMapLine.size()-1;
          }
          else
          {
              end_tracepoint=start_tracepoint-1;
          }

       }
       double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
       double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
       if(circleMode)//闭环地图
       {
           if(dis1<3&&dis2<3&&circleMode) //距离不能太小，太小了可能会错过，所以适当将距离增大一些
          // if(dis1<1&&dis2<1&&circleMode)
           {
               file_cnt_add_en=true;  //201200102
           }
           else
           {
               file_cnt_add_en=false;
           }

           if(dis1>10)
           {
              file_cnt_add=true;
           }

           if(file_cnt_add_en&&file_num<256&&file_cnt_add)
           {
               file_num++;
               file_cnt_add=false;
               map_start_point=true;
           }
           if(file_num==256)
           {
               file_num=1;
           }
           else
           {
           }
           QDateTime dt=QDateTime::currentDateTime();
           QString date =dt.toString("yyyy_MM_dd_hhmm");
           QString strsen = "datalog_";
           date.append(strsen);
           QString s = QString::number(file_num);
           date.append(s);
           date.append(".txt");
           string filename;
           filename=date.toStdString();
           ofstream outfile;
           outfile.open(filename, ostream::app);
           if((file_cnt_add==false)&&(map_start_point==true))
           {
               QDateTime dt=QDateTime::currentDateTime();
               outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"
                      <<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
                      <<endl;
               outfile<<"***********************the vehicle at map start point!*************************"<<endl;
               outfile<<"the number of lap  is "<<":    "<<file_num<<""<<endl;
               map_start_point=false;
           }
           QDateTime dt2=QDateTime::currentDateTime();
           outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                  <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
                  <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                  <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                  <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
                  <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                  <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
                  <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
                  <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
                  <<"Trace_Point"<< ","<<PathPoint<< ","
                  <<"OBS_Distance"<< ","<<obsDistance<< ","
                  <<"OBS_Speed"<< ","<<obsSpeed<< ","
                  <<endl;
           outfile.close();
       }
       else //fei yuanxing luxian
       {
           if(dis1<3)
           {
               file_cnt_add_en=true;  //201200102
           }
           else
           {
               file_cnt_add_en=false;
           }

           if(dis1>3)
           {
              file_cnt_add=true;
           }

           if(file_cnt_add_en&&file_num<256&&file_cnt_add)
           {
               file_num++;
               file_cnt_add=false;
               map_start_point=true;
           }
           if(file_num==256)//20210712
           {
               file_num=1;
           }
           else
           {
           }
           QDateTime dt=QDateTime::currentDateTime();
           QString date =dt.toString("yyyy_MM_dd_hhmm");
           QString strsen = "datalog_";
           date.append(strsen);
           QString s = QString::number(file_num);
           date.append(s);
           date.append(".txt");
           string filename;
           filename=date.toStdString();
           ofstream outfile;
           outfile.open(filename, ostream::app);
           if((file_cnt_add==false)&&(map_start_point==true))
           {
               QDateTime dt=QDateTime::currentDateTime();
               outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
                     <<endl;
               outfile<<"********the vehicle near map start point!********"<<endl;
                 outfile<<"the number of lap  is "<<":" <<file_num<<""<<endl;
               map_start_point=false;
           }
           if(dis2<3)
           {
               outfile<<"********the vehicle near map end point!********" <<endl;
               QDateTime dt=QDateTime::currentDateTime();
               outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
                     <<endl;
           }
           else
           {
               float ttc = 0-(obsDistance/obsSpeed);
               QDateTime dt2=QDateTime::currentDateTime();
               outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                      <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
                      <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                      <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                      <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
                      <<"OBS_Distance"<< ","<<obsDistance<< ","
                      <<"Min_Decelation"","<<minDecelerate<< ","
                      <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                      <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
                      <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
                      <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
                      <<"Trace_Point"<< ","<<PathPoint<< ","
                      <<"OBS_Speed"<< ","<<obsSpeed<< ","
                      <<"TTC"<< ","<<ttc<< ","
                      <<endl;
               outfile.close();
           }
   }
       givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f",
               vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle);
       }


    lastAngle=gps_decition->wheel_angle;

    //   qDebug("decide1 time is %d",xTime.elapsed());
    return gps_decition;
}



void iv::decition::DecideGps00::initMethods(){

    pid_Controller= new PIDController();
    ge3_Adapter = new Ge3Adapter();
    qingyuan_Adapter = new QingYuanAdapter();
    vv7_Adapter = new VV7Adapter();
    zhongche_Adapter = new ZhongcheAdapter();
    bus_Adapter = new BusAdapter();
    hapo_Adapter = new HapoAdapter();
    hunter_Adapter = new HunterAdapter(); //20210903,cxw

    mpid_Controller.reset(pid_Controller);
    mge3_Adapter.reset(ge3_Adapter);
    mqingyuan_Adapter.reset(qingyuan_Adapter);
    mvv7_Adapter.reset(vv7_Adapter);
    mzhongche_Adapter.reset(zhongche_Adapter);
    mbus_Adapter.reset(bus_Adapter);
    mhapo_Adapter.reset(hapo_Adapter);
    mhunter_Adapter.reset(hunter_Adapter); //20210903,cxw

    frenetPlanner = new FrenetPlanner();
    //    laneChangePlanner = new LaneChangePlanner();

    mfrenetPlanner.reset(frenetPlanner);
    //    mlaneChangePlanner.reset(laneChangePlanner);

}


void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {

    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);

    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }else if(ServiceCarStatus.msysparam.mvehtype=="hunter"){
        hunter_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
    }
}



std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
    vector<iv::Point2D> trace;

    traceOriLeft.clear();
    traceOriRight.clear();

    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
        int aimIndex;
        if(circleMode){
            aimIndex=PathPoint+800;
        }else{
            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
        }

        for (int i = PathPoint; i < aimIndex; i++)
        {
            int index = i % gpsMapLine.size();
            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
            pt.x += offset_real * 0.032;
            pt.v1 = (*gpsMapLine[index]).speed_mode;
            pt.v2 = (*gpsMapLine[index]).mode2;
            pt.roadMode=(*gpsMapLine[index]).roadMode;
            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
            {
                readyZhucheMode = true;
                DecideGps00::zhuchePointNum = index;
            }

            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
            {
                readyZhucheMode = true;
                DecideGps00::zhuchePointNum = index;
            }

            //csvv7
            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
            {
                readyParkMode = true;
                DecideGps00::finishPointNum = index;
            }
            switch (pt.v1)
            {
            case 0:
                pt.speed = 80;
                break;
            case 1:
                pt.speed = 25;
                break;
            case 2:
                pt.speed =25;
                break;
            case 3:
                pt.speed = 20;
                break;
            case 4:
                pt.speed =18;
                break;
            case 5:
                pt.speed = 18;
                break;
            case 7:
                pt.speed = 10;
                break;
            case 22:
                pt.speed = 5;
                break;
            case 23:
                pt.speed = 5;
                break;
            default:
                break;
            }
            trace.push_back(pt);


            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
            if(hdg < 0)  hdg = hdg + 360;
            if(hdg >= 360) hdg = hdg - 360;

            double xyhdg  = hdg/180.0*PI;
            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);

            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);

            traceOriLeft.push_back(ptLeft);
            traceOriRight.push_back(ptRight);

        }
    }
    return trace;
}


std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
    vector<iv::Point2D> trace;

    int index = -1;
    float minDis = 10;

    for (unsigned int i = 0; i < gpsTrace.size(); i++)
    {
        double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));

        if (tmpdis < minDis)
        {
            index = i;
            minDis = tmpdis;
        }
    }

    trace.clear();
    if (index >= 0) {
        for (unsigned int i = index; i < gpsTrace.size(); i++)
        {
            Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
            trace.push_back(pt);
        }
    }
    return trace;
}



std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {

    if (offset==0)
    {
        return gpsTrace;
    }

    std::vector<iv::Point2D> trace;
    for (int j = 0; j < gpsTrace.size(); j++)
    {
        double sumx1 = 0, sumy1 = 0, count1 = 0;
        double sumx2 = 0, sumy2 = 0, count2 = 0;
        for (int k = max(0, j - 4); k <= j; k++)
        {
            count1 = count1 + 1;
            sumx1 += gpsTrace[k].x;
            sumy1 += gpsTrace[k].y;
        }
        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
        {
            count2 = count2 + 1;
            sumx2 += gpsTrace[k].x;
            sumy2 += gpsTrace[k].y;
        }
        sumx1 /= count1; sumy1 /= count1;
        sumx2 /= count2; sumy2 /= count2;

        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

        double avoidLenth = abs(offset);
        if (offset<0)
        {
            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
            ptLeft.speed = gpsTrace[j].speed;
            ptLeft.v1 = gpsTrace[j].v1;
            ptLeft.v2 = gpsTrace[j].v2;
            trace.push_back(ptLeft);
        }
        else
        {
            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
            ptRight.speed = gpsTrace[j].speed;
            ptRight.v1 = gpsTrace[j].v1;
            ptRight.v2 = gpsTrace[j].v2;


            trace.push_back(ptRight);
        }
    }

    return trace;
}

void  iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
    for (int j = 0; j < gpsTrace.size(); j++)
    {
        double sumx1 = 0, sumy1 = 0, count1 = 0;
        double sumx2 = 0, sumy2 = 0, count2 = 0;
        for (int k = max(0, j - 4); k <= j; k++)
        {
            count1 = count1 + 1;
            sumx1 += gpsTrace[k].x;
            sumy1 += gpsTrace[k].y;
        }
        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
        {
            count2 = count2 + 1;
            sumx2 += gpsTrace[k].x;
            sumy2 += gpsTrace[k].y;
        }
        sumx1 /= count1; sumy1 /= count1;
        sumx2 /= count2; sumy2 /= count2;

        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));

        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));

        gpsTraceNowLeft.push_back(ptLeft);
        gpsTraceNowRight.push_back(ptRight);
    }

}




std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {

    if (offset==0)
    {
        return gpsTrace;
    }

    std::vector<iv::Point2D> trace;
    std::vector<iv::Point2D> traceXY;
    for (int j = 0; j < gpsTrace.size(); j++)
    {
        double sumx1 = 0, sumy1 = 0, count1 = 0;
        double sumx2 = 0, sumy2 = 0, count2 = 0;
        for (int k = max(0, j - 4); k <= j; k++)
        {
            count1 = count1 + 1;
            sumx1 += gpsTrace[k].x;
            sumy1 += gpsTrace[k].y;
        }
        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
        {
            count2 = count2 + 1;
            sumx2 += gpsTrace[k].x;
            sumy2 += gpsTrace[k].y;
        }
        sumx1 /= count1; sumy1 /= count1;
        sumx2 /= count2; sumy2 /= count2;

        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

        double avoidLenth = abs(offset);
        if (offset<0)
        {
            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
            ptLeft.speed = gpsTrace[j].speed;
            ptLeft.v1 = gpsTrace[j].v1;
            ptLeft.v2 = gpsTrace[j].v2;
            trace.push_back(ptLeft);
        }
        else
        {
            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
            ptRight.speed = gpsTrace[j].speed;
            ptRight.v1 = gpsTrace[j].v1;
            ptRight.v2 = gpsTrace[j].v2;


            trace.push_back(ptRight);
        }
    }


    bool use_new_method = true;
    if  (use_new_method)
    {
        const int val = 300;
        if(trace.size()>val)
        {
            double V = trace[300].y;
            for (int j = 0; j < val; j++)
            {
                    double t = (double)j / val;
                    double s = t*t*(3.-2.*t);
                    double ox = s;
                    double oy = t *( V-3.0)+3.0;

                    trace[j].x=ox*trace[j].x;
                    trace[j].y=oy;
            }
        }
    }

    traceXY.clear();
    for(int j=0;j<30;j++)
    {
                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
                traceXY.push_back(gpsinxy);
    }
    for(int j=0;j<trace.size();j++)
    {
                GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
                traceXY.push_back(gpsinxy);
    }
    return traceXY;
}

std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {


    std::vector<iv::Point2D> trace;
    std::vector<iv::Point2D> traceXY;

    if (offset==0)
    {
        trace.assign(gpsTrace.begin(), gpsTrace.end());
    }
    else
    {
        for (int j = 0; j < gpsTrace.size(); j++)
        {
            double sumx1 = 0, sumy1 = 0, count1 = 0;
            double sumx2 = 0, sumy2 = 0, count2 = 0;
            for (int k = max(0, j - 4); k <= j; k++)
            {
                count1 = count1 + 1;
                sumx1 += gpsTrace[k].x;
                sumy1 += gpsTrace[k].y;
            }
            for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
            {
                count2 = count2 + 1;
                sumx2 += gpsTrace[k].x;
                sumy2 += gpsTrace[k].y;
            }
            sumx1 /= count1; sumy1 /= count1;
            sumx2 /= count2; sumy2 /= count2;

            double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

            double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
            double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

            double avoidLenth = abs(offset);
            if (offset<0)
            {
                Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
                               carFronty + avoidLenth  * sin(anglevalue + PI / 2));
                ptLeft.speed = gpsTrace[j].speed;
                ptLeft.v1 = gpsTrace[j].v1;
                ptLeft.v2 = gpsTrace[j].v2;
                trace.push_back(ptLeft);
            }
            else
            {
                Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
                                carFronty + avoidLenth  * sin(anglevalue - PI / 2));
                ptRight.speed = gpsTrace[j].speed;
                ptRight.v1 = gpsTrace[j].v1;
                ptRight.v2 = gpsTrace[j].v2;


                trace.push_back(ptRight);
            }
        }
    }


    bool use_new_method = true;
    if  (use_new_method)
    {
        const int val = 300;
        if(trace.size()>val)
        {
            double V = trace[300].y;
            for (int j = 0; j < val; j++)
            {
                    double t = (double)j / val;
                    double s = t*t*(3.-2.*t);
                    double ox = s;
                    double oy = t *( V-3.0)+3.0;

                    trace[j].x=ox*trace[j].x;
                    trace[j].y=oy;
            }
        }
    }

    traceXY.clear();
    for(int j=0;j<30;j++)
    {
                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
                traceXY.push_back(gpsinxy);
    }
    for(int j=0;j<trace.size();j++)
    {
                GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
                traceXY.push_back(gpsinxy);
    }
    return traceXY;
}



double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
    double angle=0;
    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
    {
        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
        angle= decition->wheel_angle;
    }
    return angle;
}

double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
    double speed=0;
    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
    speed = gpsTrace[speedPoint].speed;
    for (int i = 0; i < speedPoint; i++) {
        speed = min(speed, gpsTrace[i].speed);
    }
    return speed;
}


//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
//
//	if (!obsRadars.empty())
//	{
//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
//
//		if (esrIndex != -1)
//		{
//			 esrDistance = obsRadars[esrIndex].nomal_y;
//
//
//
//			obsSpeed = obsRadars[esrIndex].speed_y;
//
//		}
//		else {
//			esrDistance = -1;
//		}
//
//	}
//	else
//	{
//		esrIndex = -1;
//		esrDistance = -1;
//	}
//	if (esrDistance < 0) {
//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
//	}
//	else {
//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
//	}
//
//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
//}



void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


    int esrPathpoint;

    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);

    if (esrIndex != -1)
    {
        //优化
        //        double distance = 0.0;
        //        for(int i=0; i<esrPathpoint; ++i){
        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
        //        }
        //        esrDistance = distance - Esr_Y_Offset;
        //        if(esrDistance<=0){
        //            esrDistance=1;
        //        }


        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;

    }
    else {
        esrDistance = -1;
    }
}




void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {

    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);

    if (esrIndexAvoid != -1)
    {
        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;

    }
    else {
        esrDistanceAvoid = -1;
    }

    if (esrDistanceAvoid < 0) {
        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
    }
    else {
        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
    }

    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
}




double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
    double preAngle = angle;

    if (speed > 15)
    {
        if (preAngle > 350)
        {
            preAngle = 350;
        }
        if (preAngle < -350)
        {
            preAngle = -350;
        }
    }
    if (speed > 22)
    {
        if (preAngle > 200)
        {
            preAngle = 200;
        }
        if (preAngle < -200)
        {
            preAngle = -200;
        }
    }
    if (speed > 25)
    {
        if (preAngle > 150)
        {
            preAngle = 150;
        }
        if (preAngle < -150)
        {
            preAngle = -150;
        }
    }
    if (speed > 30)
    {
        if (preAngle > 70)
        {
            preAngle = 70;
        }
        if (preAngle < -70)
        {
            preAngle = -70;
        }
    }
    if (speed > 45)                     //20
    {
        if (preAngle > 15)
        {
            preAngle = 15;
        }
        if (preAngle < -15)
        {
            preAngle = -15;
        }
    }
    return preAngle;
}



double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {

    if (abs(angle) > 500 && speed > 8) speed = 8;
    else if (abs(angle) > 350 && speed > 14) speed = 14;
    else if (abs(angle) > 200 && speed > 21) speed = 21;
    else if (abs(angle) > 150 && speed > 24) speed = 24;
    else if (abs(angle) > 60 && speed > 29) speed = 29;
    else if (abs(angle) > 20 && speed > 34) speed = 34;
    return max(0.0, speed);
}


bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {

    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
    {
        return false;
    }

    if (roadNum-roadNow>1)
    {
        for (int i = roadNow+1; i < roadNum; i++)
        {
            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }

    }

    else if (roadNow-roadNum>1)
    {
        for (int i = roadNow-1; i >roadNum; i--)
        {
            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }
    }
    return true;
}



bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
    //lsn
    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
    {
        return false;
    }
    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
    {
        return false;
    }


    if (roadNum - roadNow>1)
    {
        for (int i = roadNow + 1; i < roadNum; i++)
        {
            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }

    }

    else if (roadNow - roadNum>1)
    {
        for (int i = roadNow - 1; i >roadNum; i--)
        {
            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }
    }
    return true;
}


void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {



    if (lidarGridPtr == NULL)
    {
        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
    }
    else {
        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
    }
    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
    getEsrObsDistanceAvoid();

    //lidarDistanceAvoid = -1;   //20200103 apollo_fu

    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
    {
        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
        {
            obsDistanceAvoid = esrDistanceAvoid;
            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
        }
        else
        {
            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
        }
    }
    else if (esrDistanceAvoid>0)
    {
        obsDistanceAvoid = esrDistanceAvoid;
        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
    }
    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
    {
        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
    }
    else {
        obsDistanceAvoid = esrDistanceAvoid;
        obsSpeedAvoid = 0 - secSpeed;
    }

    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;





    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
    {
        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
        obsLostTimeAvoid++;
    }
    else
    {
        obsLostTimeAvoid = 0;
        iv::decition::DecideGps00::lastDistanceAvoid = -1;
    }




    if (obsDistanceAvoid>0)
    {
        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
    }

    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
}

void iv::decition::DecideGps00::init() {
    for (int i = 0; i < roadSum; i++)
    {
        lastEsrIdVector.push_back(-1);
        lastEsrCountVector.push_back(0);
        GPS_INS gps_ins;
        gps_ins.gps_x = 0;
        gps_ins.gps_y = 0;
        startAvoidGpsInsVector.push_back(gps_ins);
        avoidMinDistanceVector.push_back(0);
    }
}



#include <QTime>

void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {

    //    QTime xTime;
    //   xTime.start();
    //    qDebug("time 0 is %d ",xTime.elapsed());
    double obs,obsSd;

    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;

    }
    else {
        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        float lidarXiuZheng=0;
        if(!ServiceCarStatus.useMobileEye){
            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
        }
        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }

    if(lidarDistance<0){
        lidarDistance=500;
    }

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;

    ServiceCarStatus.mLidarObs = lidarDistance;
    obs = lidarDistance;
    obsSd= obsPoint.obs_speed_y;

    if(ServiceCarStatus.useLidarPerPredict){
       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
       if (preDis<obs){
           obs = preDis;
           if(abs(obs-preDis>0.5)){
            obsSd = 0-realspeed;
           }
       }
    }


    if(roadNum==roadNow){
        obsDistance=obs;
        obsSpeed=obsSd;
    }

    if(obsDistance<500&&obsDistance>0){
     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
    }
    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;


    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    //lsn
    if(obs<0){
        obsDisVector[roadNum]=500;
    }else{
        obsDisVector[roadNum]=obs;
    }

    //   qDebug("time 3 is %d ",xTime.elapsed());

}

void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {

    //    QTime xTime;
    //   xTime.start();
    //    qDebug("time 0 is %d ",xTime.elapsed());
    double obs,obsSd;

    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;

    }
    else {
        //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
        float lidarXiuZheng=0;
        if(!ServiceCarStatus.useMobileEye){
            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
        }
        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }






    if(lidarDistance<0){
        lidarDistance=500;
    }



    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;

    ServiceCarStatus.mLidarObs = lidarDistance;
    obs = lidarDistance;

    obsSd= obsPoint.obs_speed_y;

    if(ServiceCarStatus.useLidarPerPredict){
       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
       if (preDis<obs){
           obs = preDis;
           if(abs(obs-preDis>0.5)){
            obsSd = 0-realspeed;
           }
       }
    }


    if(roadNum==roadNow){
        obsDistance=obs;
        obsSpeed=obsSd;
    }

    if(obsDistance<500&&obsDistance>0){
     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
    }
    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;


    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    //lsn
    if(obs<0){
        obsDisVector[roadNum]=500;
    }else{
        obsDisVector[roadNum]=obs;
    }

    //   qDebug("time 3 is %d ",xTime.elapsed());

}


//1220
void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
                                                     const std::vector<GPSData> gpsMapLine) {

    double obs,obsSd;


    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;

    }
    else {
        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
        float lidarXiuZheng=0;
        if(!ServiceCarStatus.useMobileEye){
            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
        }
        if(abs(obsPoint.y)>lidarXiuZheng)
            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220

        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }



    if(lidarDistance<0){
        lidarDistance=500;
    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;

    if(lidarDistance==500){
        lidarDistance=-1;
    }




    ServiceCarStatus.mLidarObs = lidarDistance;

    obs=lidarDistance;
    //	obsSpeed = 0 - secSpeed;
    obsSd = 0 -secSpeed;




    if(roadNum==roadNow){
        obsDistance=obs;
        obsSpeed=obsSd;
    }


    if (obsDistance <0 && obsLostTime<4)
    {
        obsDistance = lastDistance;
        obsLostTime++;
    }
    else
    {
        obsLostTime = 0;
        lastDistance = -1;
    }


    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;

    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    //lsn
    if(obs<0){
        obsDisVector[roadNum]=500;
    }else{
        obsDisVector[roadNum]=obs;
    }

}



double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
    double preObsDis=500;

    if(!lidar_per.empty()){
        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
        lastPreObsDistance=preObsDis;

    }else{
        preObsDis=lastPreObsDistance;
    }

    ServiceCarStatus.mfttc = preObsDis;
    return preObsDis;
//    if(preObsDis<obsDistance){
//        obsDistance=preObsDis;
//        lastDistance=obsDistance;
//    }
}

int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
    roadPre = -1;

    for (int i =  0; i < roadSum; i++)
    {
        gpsTraceAvoid.clear();
        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
    }

    if (lidarGridPtr!=NULL)
    {
        hasCheckedAvoidLidar = true;
    }

    for(int i=0; i<roadSum;i++){
        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;

    }

    checkAvoidObsTimes++;
    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
    {
        return - 1;
    }


    for (int i = 1; i < roadSum; i++)
    {
        if (roadNow + i < roadSum) {
         //   avoidX = (roadOri-roadNow-i)*roadWidth;
             avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
            {
                /*if (roadNow==roadOri)
                {
                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                    startAvoid_gps_ins = now_gps_ins;
                }	*/
                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
                startAvoidGpsInsVector[roadNow] = now_gps_ins;
                roadPre = roadNow + i;
                return roadPre;
            }
        }

        if (roadNow - i >= 0)
        {
        //    avoidX = (roadOri - roadNow+i)*roadWidth;
             avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
            {
                /*if (roadNow == roadOri)
                {
                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                    startAvoid_gps_ins = now_gps_ins;
                }*/
                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
                startAvoidGpsInsVector[roadNow] = now_gps_ins;
                roadPre = roadNow - i;
                return roadPre;
            }
        }

    }
    return roadPre;
}

int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
    roadPre = -1;

    for (int i =  0; i <roadSum; i++)
    {
        gpsTraceBack.clear();
        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
    }


    if (lidarGridPtr != NULL)
    {
        hasCheckedBackLidar = true;
    }



    checkBackObsTimes++;
    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
    {
        return -1;
    }

    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
    {
        roadPre = roadOri;
        return roadPre;
    }

    if (roadNow-roadOri>1)
    {
        for (int i = roadOri + 1;i < roadNow;i++) {
            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
            {
                roadPre = i;
                return roadPre;
            }
        }
    }

    else if (roadNow <roadOri-1)
    {
        for (int i = roadOri - 1;i > roadNow;i--) {
            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
            {
                roadPre = i;
                return roadPre;
            }
        }
    }

    return roadPre;
}


double iv::decition::DecideGps00::trumpet() {
    if (trumpetFirstCount)
    {
        trumpetFirstCount = false;
        trumpetLastTime= GetTickCount();
        trumpetTimeSpan = 0.0;
    }
    else
    {
        trumpetStartTime= GetTickCount();
        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
        trumpetLastTime = trumpetStartTime;
    }

    return trumpetTimeSpan;
}




double iv::decition::DecideGps00::transferP() {
    if (transferFirstCount)
    {
        transferFirstCount = false;
        transferLastTime= GetTickCount();
        transferTimeSpan = 0.0;
    }
    else
    {
        transferStartTime= GetTickCount();
        transferTimeSpan += transferStartTime - transferLastTime;
        transferLastTime = transferStartTime;
    }

    return transferTimeSpan;
}



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

    if (abs(now_gps_ins.speed)>0.1)
    {
        decition->accelerator = 0;
        decition->brake = 20;
        decition->wheel_angle = 0;

    }
    else
    {

        decition->accelerator = 0;
        decition->brake = 20;
        decition->wheel_angle = 0;
        handPark = true;
        handParkTime = duringTime;
    }

}




void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
    gmapsL.clear();
    gmapsR.clear();
    for (int i = 0; i < 31; i++)
    {
        std::vector<iv::GPSData> gpsMapLineBeside;
        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);

        gmapsL.push_back(gpsMapLineBeside);
    }


    for (int i = 0; i < 31; i++)
    {
        std::vector<iv::GPSData> gpsMapLineBeside;
        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);

        gmapsR.push_back(gpsMapLineBeside);
    }

}


bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {

    if (lidarGridPtr == NULL)
    {
        return false;
        //	lidarDistance = lastlidarDistance;

    }
    else {




        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
        if (lidarDistance >-20 && lidarDistance<35)
        {
            checkChaoCheBackCounts = 0;
            return false;
        }
        else {
            checkChaoCheBackCounts++;
        }

        if (checkChaoCheBackCounts>2) {
            checkChaoCheBackCounts = 0;
            return true;
        }
    }

    return false;

}

void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);

    ServiceCarStatus.group_x_local=pt.x;
    //  ServiceCarStatus.group_y_local=pt.y;
    ServiceCarStatus.group_y_local=s;
    if(realspeed<0.36){
        ServiceCarStatus.group_velx_local=0;
        ServiceCarStatus.group_vely_local=0;
    }else{
        ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
        ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
    }


    ServiceCarStatus.group_pathpoint=PathPoint;

}



float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                           int pathpoint,float secSpeed,float dSpeed){
    float traffic_speed=200;
    float traffic_dis=0;
    float passTime;
    float passSpeed;
    bool passEnable=false;

    if(abs(secSpeed)<0.1){
        secSpeed=0;
    }



    if(pathpoint <= traffic_light_pathpoint){
        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }else{
        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
        for(int i=0;i<traffic_light_pathpoint;i++){
            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }

    //    if(traffic_light_color != 0)
    //    {
    //        int a = 3;
    //    }


    if(traffic_light_color==0 && traffic_dis<10){
        traffic_speed=0;
    }
    //    else   //20200108
    //    {
    //        traffic_speed=10;
    //    }
    return  traffic_speed;

    passSpeed = min((float)(dSpeed/3.6),secSpeed);
    passTime = traffic_dis/(dSpeed/3.6);





    switch(traffic_light_color){
    case 0:
        if(passTime>traffic_light_time+1 && traffic_dis>10){
            passEnable=true;
        }else{
            passEnable=false;
        }
        break;
    case 1:
        if(passTime<traffic_light_time-1 && traffic_dis<10){
            passEnable=true;
        }else{
            passEnable = false;
        }
        break;
    case 2:
        if(passTime<traffic_light_time){
            passEnable= true;
        }else{
            passEnable=false;
        }
        break;


    default:
        break;
    }

    if(!passEnable){
        if(traffic_dis<5){
            traffic_speed=0;
        }else if(traffic_dis<10){
            traffic_speed=5;
        }else if(traffic_dis<20){
            traffic_speed=15;
        }else if(traffic_dis<30){
            traffic_speed=25;
        }else if(traffic_dis<50){
            traffic_speed=30;
        }
    }
    return traffic_speed;

}

void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
{
    //    Point2D obsCombinePoint = Point2D(-1,-1);
    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
    double obsSd;
    if (lidarGridPtr == NULL)
    {
        lidarDistance = lastLidarDis;
        //	lidarDistance = lastlidarDistance;
    }
    else {
        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;

        //    lidarDistance=-1;
        if (lidarDistance<0)
        {
            lidarDistance = -1;
        }
        lastLidarDis = lidarDistance;
    }

    FrenetPoint esr_obs_frenet_point;
    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);

    if(lidarDistance<0){
        lidarDistance=500;
    }
    if(esrDistance<0){
        esrDistance=500;
    }

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
    myesrDistance = esrDistance;

    if(lidarDistance==500){
        lidarDistance=-1;
    }
    if(esrDistance==500){
        esrDistance=-1;
    }

    ServiceCarStatus.mRadarObs = esrDistance;
    ServiceCarStatus.mLidarObs = lidarDistance;

    //    //zhuanwan pingbi haomibo
    //    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
    //        esrDistance=-1;
    //    }

    if (esrDistance>0 && lidarDistance > 0)
    {
        if (lidarDistance >= esrDistance)
        {
            obs = esrDistance;
            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
            obsSd = obsSpeed;
            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
        }
        else if (!ServiceCarStatus.obs_radar.empty())
        {
            obs = lidarDistance;
            //            obsCombinePoint = obsPoint;
            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
        }
        else
        {
            obs=lidarDistance;
            //            obsCombinePoint = obsPoint;
            obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
        }
    }
    else if (esrDistance>0)
    {
        obs = esrDistance;
        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
        obsSd = obsSpeed;
        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
    }
    else if (lidarDistance > 0)
    {
        obs = lidarDistance;
        //        obsCombinePoint = obsPoint;
        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
    }
    else {
        obs = esrDistance;
        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
    }

    obsDistance=obs;
    obsSpeed=obsSd;

    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;

    ServiceCarStatus.mObs = obsDistance;
    if(ServiceCarStatus.mObs>100){
        ServiceCarStatus.mObs =-1;
    }

    if (obsDistance>0)
    {
        lastDistance = obsDistance;
    }

    if(obs<0){
        obsDistance=500;
    }else{
        obsDistance=obs;
    }
}

void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {


    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);

    if (esrIndex != -1)
    {
        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;

    }
    else {
        esrDistance = -1;
    }
}

void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {

    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);

    if (esrIndex != -1)
    {
        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
        //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。

        double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
        double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
        double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);

        obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
    }
    else {
        esrDistance = -1;
    }
}

void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
    v2xTrafficVector.clear();
    for (int var = 0; var < gpsMapLine.size(); var++) {
        if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001){
            v2xTrafficVector.push_back(var);
        }
    }
}

float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode){
    float trafficSpeed=200;
    int nearTraffixPoint=-1;
    float nearTrafficDis=0;
    int traffic_color=0;
    int traffic_time=0;
    bool passThrough=false;
    float dSecSpeed=dSpeed/3.6;

    if(v2xTrafficVector.empty()){
        return trafficSpeed;
    }
    if(!circleMode){
        if(pathpoint>v2xTrafficVector.back()){
            return trafficSpeed;
        }else {
            for(int i=0; i< v2xTrafficVector.size();i++){
                if (pathpoint<= v2xTrafficVector[i]){
                    nearTraffixPoint=v2xTrafficVector[i];
                    break;
                }
            }
        }
    }else if(circleMode){
        if(pathpoint>v2xTrafficVector.back()){
            nearTraffixPoint=v2xTrafficVector[0];
        }else {
            for(int i=0; i< v2xTrafficVector.size();i++){
                if (pathpoint<= v2xTrafficVector[i]){
                    nearTraffixPoint=v2xTrafficVector[i];
                    break;
                }
            }
        }
    }


    if(nearTraffixPoint!=-1){
        for(int i=pathpoint;i<nearTraffixPoint;i++){
            nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
        }
    }

    if(nearTrafficDis>50){
        return trafficSpeed;
    }


    int roadMode = gpsMapLine[pathpoint]->roadMode;
    if(roadMode==14 || roadMode==16){
        traffic_color=trafficLight.leftColor;
        traffic_time=trafficLight.leftTime;
    }else if(roadMode==15 ||roadMode==17){
        traffic_color=trafficLight.rightColor;
        traffic_time=trafficLight.rightTime;
    }else {
        traffic_color=trafficLight.straightColor;
        traffic_time=trafficLight.straightTime;
    }


    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
    if(passThrough){
        return trafficSpeed;
    }else{
        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
        if(nearTrafficDis<6){
            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
            minDecelerate=min(minDecelerate,decelerate);
        }
        return trafficSpeed;
    }

    return trafficSpeed;
}

bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
    float passTime=0;
    if (trafficColor==2 || trafficColor==3){
        return false;
    }else if(trafficColor==0){
        return true;
    }else{
        passTime=trafficDis/dSecSpeed;
        if(passTime+1< trafficTime){
            return true;
        }else{
            return false;
        }

    }

}

float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
    float limit=200;
    if(trafficDis<10){
        limit = 0;
    }else if(trafficDis<15){
        limit = 5;
    }else if(trafficDis<20){
        limit=10;
    }else if(trafficDis<30){
        limit=15;
    }
    return limit;
}

void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
    static int obstacle_disable=0;
    static int speed_slowdown_flag=0;
    static bool lock_flag=false;
    double forecast_distance=0;
    int forecast_point_num=0;
    bool cross=false;

    double secLowSpeed=ServiceCarStatus.mroadmode_vel.mfmode18/3.6;   //m/s
    if(secSpeed>secLowSpeed)
    {
        forecast_distance=secSpeed*secSpeed-secLowSpeed*secLowSpeed+5;
        forecast_point_num=((int)forecast_distance)*10;
        if((PathPoint+forecast_point_num+2)>gpsMapLine.size())
                                                forecast_point_num=0;
    }
    if((PathPoint+forecast_point_num-8>0)&&(PathPoint+forecast_point_num+8<gpsMapLine.size()))
    {
        for(int i=PathPoint+forecast_point_num-8;i<PathPoint+forecast_point_num+8;i++)
        {
            if(gpsMapLine[i]->mode2==5000)
                cross=true;
        }
    }

    //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d",
                //PathPoint+forecast_point_num,forecast_point_num,cross);
    if(  gpsMapLine[PathPoint]->mode2==3000){
        if(obsDistance>4){       //7   zj-5
            obsDistance=200;
        }else{
            lock_flag=false;
            obsSpeed=-realspeed/3.6;
        }
        if((realspeed>3)&&(lock_flag==false)){
            minDecelerate=-0.5;
        }else{
            dSpeed=min(dSpeed,3.0);
            lock_flag=true;
        }
    }
    else if(gpsMapLine[PathPoint]->mode2==3001){
             obstacle_disable=1;
    }else if(gpsMapLine[PathPoint]->mode2==3002){
             obstacle_disable=0;
    }else if(gpsMapLine[PathPoint]->mode2==4000){
        //ServiceCarStatus.msysparam.vehWidth=5.6;
    }else if(cross==true){
            speed_slowdown_flag=1;
            lock_flag=false;
    }else if(gpsMapLine[PathPoint]->mode2==5001){
            speed_slowdown_flag=0;
    }

    if(obstacle_disable==1){
            obsDistance=200;
    }


    if(speed_slowdown_flag==1)
    {
            if((realspeed>ServiceCarStatus.mroadmode_vel.mfmode18)&&(lock_flag==false)){
                minDecelerate=-0.5;
            }else{
                dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
                lock_flag=true;
            }
    }
}

float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
    if(roadAim==roadOri){
        return 0;
    }
    float x=0;
    float veh_to_roadSide=(gps->mfLaneWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
    if(!ServiceCarStatus.inRoadAvoid){
        x= (roadOri-roadAim)*gps->mfLaneWidth;
    }else{
        int num=roadOri-roadAim;
        switch (abs(num)%3) {
        case 0:
            x=(num/3)*gps->mfLaneWidth;
            break;
        case 1:
            if(num>0){
                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide;
            }else{
                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide;
            }
            break;
        case 2:
            if(num>0){
                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide+roadSide_to_roadSide;
            }else{
                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide-roadSide_to_roadSide;
            }

            break;
        default:
            break;
        }

    }
    return x;
}