modularization/src1/decision/common/adc_tools/compute_00.cpp

#include <adc_tools/compute_00.h>
//#include <decision/decide_gps_00.h>
#include <adc_tools/gps_distance.h>
#include <decition_type.h>
#include <adc_tools/transfer.h>
#include <constants.h>
#include <math.h>
#include <iostream>
#include <fstream>
//#include <control/can.h>
#include "common/car_status.h"

#include "adc_planner/frenet_planner.h"
using namespace std;

extern bool handPark;
extern long handParkTime;
extern bool rapidStop;
extern int gpsMissCount;
extern bool changeRoad;
extern double avoidX;
extern bool parkBesideRoad;
extern double steerSpeed;
extern bool transferPieriod;
extern bool transferPieriod2;
extern double traceDevi;

#define PI (3.1415926535897932384626433832795)

iv::decition::Compute00::Compute00() {

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

}


double iv::decition::Compute00::angleLimit = 700;
double iv::decition::Compute00::lastEA = 0;
double iv::decition::Compute00::lastEP = 0;
double iv::decition::Compute00::decision_Angle = 0;
double iv::decition::Compute00::lastAng = 0;
int iv::decition::Compute00::lastEsrID = -1;
int  iv::decition::Compute00::lastEsrCount = 0;
int iv::decition::Compute00::lastEsrIDAvoid = -1;
int  iv::decition::Compute00::lastEsrCountAvoid = 0;

iv::GPS_INS  iv::decition::Compute00::nearTpoint;
iv::GPS_INS  iv::decition::Compute00::farTpoint;
double  iv::decition::Compute00::bocheAngle;



iv::GPS_INS  iv::decition::Compute00::dTpoint0;
iv::GPS_INS  iv::decition::Compute00::dTpoint1;
iv::GPS_INS  iv::decition::Compute00::dTpoint2;
iv::GPS_INS  iv::decition::Compute00::dTpoint3;
double  iv::decition::Compute00::dBocheAngle;
int  iv::decition::Compute00::nParkType;


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



int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
{
    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
    int endIndex = gpsMap.size() - 1;
    static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
    static int FrontCount=0,BackCount=0;
    static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
    int MarkedIndex=0,CurveContinue=0;

    for (int j = startIndex; j < endIndex; j++)
    {
        int i = (j + gpsMap.size()) % gpsMap.size();
        if((*gpsMap[i]).roadMode!=6)
                (*gpsMap[i]).roadMode=5;
    }
    for (int j = startIndex; j < (endIndex-40); j+=40)
    {
        int i = (j + gpsMap.size()) % gpsMap.size();
        for(int front_k=i;front_k<i+20;front_k++)
        {
            if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
            {
                   FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
                   FrontCount++;
            }

        }
        i+=20;
        FrontAveFive=FrontTotalFive/FrontCount;
        FrontTotalFive=0;
        FrontCount=0;
        for(int back_k=i;back_k<i+20;back_k++)
        {
            if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
            {
                   BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
                   BackCount++;
            }

        }
        i+=20;
        CurrentIndex=i-1;
        BackAveFive=BackTotalFive/BackCount;
        BackTotalFive=0;
        BackCount=0;
        if(fabs(FrontAveFive-BackAveFive)<50)
        {
                   if(fabs(FrontAveFive-BackAveFive)>4)
                   {
                        CurveContinue+=1;
                        if(CurveContinue>=1)
                        {
                            MarkingCount=0;
                            for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
                            {
                                if((*gpsMap[MarkingIndex]).roadMode!=6)
                                {
                                if(MarkingCount<150)
                                {
                                     if((BackAveFive-FrontAveFive)<=3.5)
                                     {
                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
                                     }
                                     else if((BackAveFive-FrontAveFive)>3.5)
                                     {
                                           (*gpsMap[MarkingIndex]).roadMode=15;
                                     }

                                } //else if((MarkingCount>=100)&&(MarkingCount<150)){(*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米,1}
                                else if((MarkingCount>=150)&&(MarkingCount<320))
                                {
                                     (*gpsMap[MarkingIndex]).roadMode=5;   //低速，20米,5
                                }
                                else if((MarkingCount>=320)&&(MarkingCount<620))
                                {
                                     (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
                                }
                                else if(MarkingCount>=620)
                                {
                                     (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
                                }
                                }

                                MarkingCount++;
                            }
                            MarkedIndex=CurrentIndex;
                        }
                   }
                   else if(fabs(FrontAveFive-BackAveFive)<=4)
                   {
                        CurveContinue=0;
                   }
        }
        FrontAveFive=0;
        BackAveFive=0;
    }

    if(MarkedIndex<endIndex)
    {
        MarkingCount=0;
        for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
        {
            if((*gpsMap[MarkingIndex]).roadMode!=6)
            {
            if(MarkingCount<100)
            {
                if((BackAveFive-FrontAveFive)<3)
                {
                      (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
                }
                else if((BackAveFive-FrontAveFive)>3)
                {
                      (*gpsMap[MarkingIndex]).roadMode=15;
                }
            }
            else if((MarkingCount>=100)&&(MarkingCount<150))
            {
                 (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米
            }
            else if((MarkingCount>=150)&&(MarkingCount<320))
            {
                 (*gpsMap[MarkingIndex]).roadMode=5;   //低速，30米
            }
            else if((MarkingCount>=320)&&(MarkingCount<620))
            {
                 (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
            }
            else if(MarkingCount>=620)
            {
                 (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
            }
             }
            MarkingCount++;
        }
    }


    return 1;
}



//首次找点

int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
{
    int index = -1;
    //	DecideGps00().minDis = iv::MaxValue;
    float minDis = 10;
    double maxAng = iv::MaxValue;

    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
    int endIndex = gpsMap.size() - 1;

    for (int j = startIndex; j < endIndex; j++)
    {
        int i = (j + gpsMap.size()) % gpsMap.size();
        double tmpdis = GetDistance(rp, (*gpsMap[i]));

        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
                )
        {
            index = i;
            minDis = tmpdis;
            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
        }
    }

//    DecideGps00().maxAngle=maxAng;
//    DecideGps00().minDis=minDis;
    return index;
}

//search pathpoint
int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
{
    int index = -1;
    float minDis = 10;
    double maxAng = iv::MaxValue;
    int map_size=gpsMap.size();
    int preDistance=max(100,(int)(rp.speed*10));
        preDistance=min(500,preDistance);

    int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));     // startIndex = 0 则每一次都是遍历整条地图路线
    int endIndex = min((int)(lastIndex + preDistance ),(int)(lastIndex+map_size));


    for (int j = startIndex; j < endIndex; j++)
    {
        int i = (j + map_size) % map_size;
        double tmpdis = GetDistance(rp, (*gpsMap[i]));

        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
            )
        {
            index = i;
            minDis = tmpdis;
            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
            maxAng = min(maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
        }
    }


//    DecideGps00().maxAngle=maxAng;
//    DecideGps00().minDis=minDis;
    return index;
}


double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
{
    double sum_x = 0;
    double sum_y = 0;

    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
    {
        sum_x += farTrace[i].x;
        sum_y += abs(farTrace[i].y);
    }
    double average_y = sum_y / min(5, (int)farTrace.size());
    double average_x = sum_x / min(5, (int)farTrace.size());


    return atan(average_x / average_y) / PI * 180;
}



double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
{
    double sum_x = 0;
    double sum_y = 0;

    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
    {
        sum_x += farTrace[i].x;
        sum_y += abs(farTrace[i].y);
    }
    double average_y = sum_y / min(5, (int)farTrace.size());
    double average_x = sum_x / min(5, (int)farTrace.size());


    return atan(average_x + avoidX / average_y) / PI * 180;
}






double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {

    double ang = 0;
    double EPos = 0, EAng = 0;

    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;

    if(transferPieriod&& !transferPieriod2){
        DEang = 200;
        DEPos = 150;
    }

    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离

    double PreviewDistance;//预瞄距离
    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
    if(changeRoad ||transferPieriod){
        PreviewDistance=PreviewDistance+avoidX;
    }
    if(realSpeed <15){
        PreviewDistance = max(4.0, realSpeed *0.4) ;
    }

    if (gpsTrace[0].v1 == 1)
    {
        KEang = 14; KEPos = 10;
    }
    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
    {
        KEang = 14; KEPos = 10;
    }
    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
    {
        KEang = 14; KEPos = 10;
    }
    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
    {
        KEang = 18; KEPos = 50; PreviewDistance = 3;
    }
    else if (gpsTrace[0].v1 == 7)
    {
        KEang = 20; KEPos = 50; PreviewDistance = 4;
    }


    if (realSpeed > 40)	KEang = 10; KEPos = 8;
    if (realSpeed > 50) KEang = 5;

    double sumdis = 0;
    int gpsIndex = 0;
    std::vector<Point2D> farTrace;






    for (int i = 1; i < gpsTrace.size() - 1; i++)
    {
        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
        if (sumdis > PreviewDistance)
        {
            gpsIndex = i;
            break;
        }
    }




    EPos = gpsTrace[gpsIndex].x;

    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
        farTrace.push_back(gpsTrace[gpsIndex]);
    }
    if (farTrace.size() == 0) {
        EAng = 0;
    }
    else {
        EAng = getAveDef(farTrace);
    }

    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);

    lastEA = EAng;
    lastEP = EPos;

    if (ang > angleLimit) {
        ang = angleLimit;
    }
    else if (ang < -angleLimit) {
        ang = -angleLimit;
    }
    if (lastAng != iv::MaxValue) {
        ang = 0.2 * lastAng + 0.8 * ang;
        //ODS("lastAng：%d\n", lastAng);
    }
    lastAng = ang;
    return ang;
}




int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
{
    int index = 1;
    double sumdis = 0;
    while (index < gpsTrace.size() && sumdis < realSpeed)
        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);

    if (index == gpsTrace.size())
        return index - 1;

    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
        index--;
    return index;
}

iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {

    iv::Point2D obsPoint(-1, -1);
    vector<Point2D> gpsTraceLeft;
    vector<Point2D> gpsTraceRight;
    float xiuzheng=0;
    if(!ServiceCarStatus.useMobileEye){
        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
    }

    ServiceCarStatus.obsTraceLeft.clear();
    ServiceCarStatus.obsTraceRight.clear();

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

        gpsTraceLeft.push_back(ptLeft);
        gpsTraceRight.push_back(ptRight);


        TracePoint obsptleft(ptLeft.x,ptLeft.y);
        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);

        TracePoint obsptright(ptRight.x,ptRight.y);
        ServiceCarStatus.obsTraceLeft.push_back(obsptright);


    }


    bool isRemove = false;

    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
    {

        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
        {
            int count = 0;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
                int dx = (ptx + gridwide*(double)centerx)/gridwide;
                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            j++;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
                int dx = (ptx + gridwide*(double)centerx)/gridwide;
                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            if (count >= 2)
            {
                obsPoint.x = gpsTrace[j].x;
                obsPoint.y = gpsTrace[j].y;

                isRemove = true;
                //		DecideGps00().lidarDistance = obsPoint.y;
                return obsPoint;
            }
        }
    }
    //	DecideGps00().lidarDistance = obsPoint.y;
    return obsPoint;
}

//1220
iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {

    iv::Point2D obsPoint(-1, -1);
    vector<Point2D> gpsTraceLeft;
    vector<Point2D> gpsTraceRight;
    float xiuzheng=0;
    if(!ServiceCarStatus.useMobileEye){
        xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
    }
    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-0.3) / 2 * cos(anglevalue + PI / 2),
                       carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));

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

        gpsTraceLeft.push_back(ptLeft);
        gpsTraceRight.push_back(ptRight);

    }


    bool isRemove = false;

    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
    {

        if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
        {
            int count = 0;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
                int dx = (ptx + gridwide*(double)centerx)/gridwide;

                dx=grx-dx;//1227

                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            j++;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
                int dx = (ptx + gridwide*(double)centerx)/gridwide;

                dx=grx-dx;//1227
                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            if (count >= 2)
            {
                obsPoint.x = gpsTrace[j].x;
                obsPoint.y = gpsTrace[j].y;

                isRemove = true;
                //		DecideGps00().lidarDistance = obsPoint.y;
                return obsPoint;
            }
        }
    }
    //	DecideGps00().lidarDistance = obsPoint.y;
    return obsPoint;
}


iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr,double & lidarDistanceAvoid) {

    iv::Point2D obsPoint(-1, -1);
    vector<Point2D> gpsTraceLeft;
    vector<Point2D> gpsTraceRight;
    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);

        //1127 fanwei xiuzheng
        float buchang=0;
        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
                       carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));

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

        gpsTraceLeft.push_back(ptLeft);
        gpsTraceRight.push_back(ptRight);

    }


    bool isRemove = false;

    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
    {

        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
        {
            int count = 0;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            j++;

            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
            {
                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;

                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;

                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
                {
                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
                    {
                        count++; obsPoint.x = ptx; obsPoint.y = pty;
                    }
                }
            }

            if (count >= 2)
            {
                obsPoint.x = gpsTrace[j].x;
                obsPoint.y = gpsTrace[j].y;

                isRemove = true;
 //               DecideGps00().lidarDistanceAvoid = obsPoint.y;
                lidarDistanceAvoid = obsPoint.y;
                return obsPoint;
            }
        }
    }
    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;

    return obsPoint;
}




//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
//	bool isRemove = false;
//
//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
//	{
//
//		for (int i = 0; i < esrRadars.size(); i++)
//			if ((esrRadars[i].nomal_y) != 0)
//			{
//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
//				double yyy = esrRadars[i].nomal_y;
//
//				if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
//				{
//
//					if (lastEsrID == (esrRadars[i]).esr_ID)
//					{
//						lastEsrCount++;
//					}
//					else
//					{
//						lastEsrCount = 0;
//					}
//
//					if (lastEsrCount >= 3)
//					{
//						return i;
//					}
//
//					lastEsrID = (esrRadars[i]).esr_ID;
//				}
//			}
//	}
//	return -1;
//}




int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint,const double xiuzhengCs) {
    bool isRemove = false;

    float xiuzheng=0;
    if(!ServiceCarStatus.useMobileEye){
        xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
    }

//    float fxiuzhengCs = DecideGps00().xiuzhengCs;
    float fxiuzhengCs = xiuzhengCs;
    int nsize = gpsTrace.size();
    for (int j = 1; j < nsize - 1 && !isRemove; j++)
    {

        for (int i = 0; i < 64; i++)
            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
            {
                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;

                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/

//优化
//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
//                    *esrPathpoint = j;
//                    return i;
//                }


                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                {
                    return i;


                    if (lastEsrID == i)
                    {
                        lastEsrCount++;
                    }
                    else
                    {
                        lastEsrCount = 0;
                    }

                    if(yyy>50 ){
                        if (lastEsrCount >=200)
                        {
                            return i;
                        }
                    }
                    else if (lastEsrCount >= 1)
                    {
                        return i;
                    }

                    lastEsrID = i;
                }
            }
    }
    return -1;
}




int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,const double xiuzhengCs) {
    bool isRemove = false;

    float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
    {

        for (int i = 0; i < 64; i++)
            if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
            {
                double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
                double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);

                if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
                    xxx=0-xxx;
                }

                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/


//                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)

                {

                    if (lastEsrID == i)
                    {
                        lastEsrCount++;
                    }
                    else
                    {
                        lastEsrCount = 0;
                    }

                    if(yyy>50 ){
                        if (lastEsrCount >=200)
                        {
                            return i;
                        }
                    }
                    else if (lastEsrCount >= 1)
                    {
                        return i;
                    }

                    lastEsrID = i;
                }
            }
    }
    return -1;
}





//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
//    bool isRemove = false;

//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
//    {

//        for (int i = 0; i < 64; i++)
//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
//            {
//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;

//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/


//                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
//                {

//                    if (lastEsrID == i)
//                    {
//                        lastEsrCount++;
//                    }
//                    else
//                    {
//                        lastEsrCount = 0;
//                    }

//                    if(yyy>50 ){
//                        if (lastEsrCount >=200)
//                        {
//                            return i;
//                        }
//                    }
//                    else if (lastEsrCount >= 3)
//                    {
//                        return i;
//                    }

//                    lastEsrID = i;
//                }
//            }
//    }
//    return -1;
//}



int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
    bool isRemove = false;

    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
    {

        for (int i = 0; i < 64; i++)
            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
            {
                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;

                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
                {

                    if (lastEsrIDAvoid == i)
                    {
                        lastEsrCountAvoid++;
                    }
                    else
                    {
                        lastEsrCountAvoid = 0;
                    }

                    if (lastEsrCountAvoid >= 6)
                    {
                        return i;
                    }

                    lastEsrIDAvoid = i;
                }
            }
    }
    return -1;
}







//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
//	double obsSpeed = 0 - realSecSpeed;
//	double minDis = iv::MaxValue;
//	for (int i = 0; i < esrRadars.size(); i++)
//		if ((esrRadars[i].nomal_y) != 0)
//		{
//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
//			double yyy = esrRadars[i].nomal_y;
//
//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
//			{
//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
//				if (tmpDis < minDis)
//				{
//					minDis = tmpDis;
//					obsSpeed = esrRadars[i].speed_y;
//				}
//			}
//		}
//
//	return obsSpeed;
//
//
//}




double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
    double obsSpeed = 0 - realSecSpeed;
    double minDis = iv::MaxValue;
    for (int i = 0; i < 64; i++)
        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
        {
            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;

            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
            {
                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
                if (tmpDis < minDis)
                {
                    minDis = tmpDis;
                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
                }
            }
        }

    return obsSpeed;
}




double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX
                                                    ,const bool readyParkMode,const int gpsLineParkIndex) {

    double ang = 0;
    double EPos = 0, EAng = 0;

    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;

    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离


    if (gpsTrace[0].v1 == 1)
    {
        KEang = 10; KEPos = 8;
        if (realSpeed > 60) KEang = 5;
    }
    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
    {
        KEang = 14; KEPos = 10;
    }
    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
    {
        KEang = 14; KEPos = 10;
    }
    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
    {
        KEang = 18; KEPos = 50; PreviewDistance = 3;
    }
    else if (gpsTrace[0].v1 == 7)
    {
        KEang = 20; KEPos = 50; PreviewDistance = 4;
    }


    double sumdis = 0;
    int gpsIndex = 0;
    std::vector<Point2D> farTrace;






    for (int i = 1; i < gpsTrace.size() - 1; i++)
    {
        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
        if (sumdis > PreviewDistance)
        {
            gpsIndex = i;
            break;
        }
    }

//    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
//    {
//        gpsIndex = DecideGps00().gpsLineParkIndex;
//    }

    if ((readyParkMode) && (gpsIndex + 10>gpsLineParkIndex))
    {
        gpsIndex = gpsLineParkIndex;
    }



    EPos = gpsTrace[gpsIndex].x + avoidX;

    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
        farTrace.push_back(gpsTrace[gpsIndex]);
    }
    if (farTrace.size() == 0) {
        EAng = 0;
    }
    else {
        EAng = getAvoidAveDef(farTrace, avoidX);
    }

    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);

    lastEA = EAng;
    lastEP = EPos;

    if (ang > angleLimit) {
        ang = angleLimit;
    }
    else if (ang < -angleLimit) {
        ang = -angleLimit;
    }
    if (lastAng != iv::MaxValue) {
        ang = 0.2 * lastAng + 0.8 * ang;
        //ODS("lastAng：%d\n", lastAng);
    }
    lastAng = ang;

    return ang;
}


std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {

    vector<vector<iv::GPSData>> maps;
    vector<iv::GPSData> gpsMapLineBeside;
    int sizeN = gpsMapLine.size();
    for (int i = 1; i < sizeN; i++)
    {
        iv::GPSData gpsData(new GPS_INS);
        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
        double lng = ServiceCarStatus.location->ins_heading_angle;


        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);

        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));

        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
        gpsData->gps_x = x0 + k1 * avoidX;
        gpsData->gps_y = y0 + k2 * avoidX;
        gpsMapLineBeside.push_back(gpsData);

    }
    return gpsMapLineBeside;

}



//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {

//    double ang = 0;
//    double EPos = 0, EAng = 0;

// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;

// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离

//    double PreviewDistance;//预瞄距离
//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);





////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
////        if (realSpeed > 50) KEang = 5;





//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
//double a = ServiceCarStatus.Lane.curvature;
//double b = ServiceCarStatus.Lane.heading;
//double c = (c1+c2)*0.5;
//double x= PreviewDistance;
//double y;


//y=a*x*x+b*x+c;

//   // EPos = y;
//EPos=c;


//  //  EAng=atan(2*a*x+b) / PI * 180;
//    EAng=ServiceCarStatus.Lane.yaw;
//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);

//        lastEA = EAng;
//        lastEP = EPos;


//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
//            std::cout << "\nEAng:%f\n" << EAng << std::endl;


//        if (ang > angleLimit) {
//            ang = angleLimit;
//        }
//        else if (ang < -angleLimit) {
//            ang = -angleLimit;
//        }
//        if (lastAng != iv::MaxValue) {
//            ang = 0.2 * lastAng + 0.8 * ang;
//            //ODS("lastAng：%d\n", lastAng);
//        }
//        lastAng = ang;
//        return ang;
//    }



double IEPos = 0, IEang = 0;


double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {

    double ang = 0;
    double EPos = 0, EAng = 0;
    double Curve=0;
    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
    double KCurve=120;
    double KIEPos = 0, KIEang = 0;
    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离

    double PreviewDistance;//预瞄距离

    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
    int conf =min(confL,confR);

    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);





    if (realSpeed > 40)	KEang = 10; KEPos = 8;
    if (realSpeed > 50) KEang = 5;

    KEPos = 20;
    KEang = 200;
    //KEang = 15;

    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
    double a = ServiceCarStatus.Lane.curvature;
    double b = ServiceCarStatus.Lane.heading;
    double c = (c1+c2)*0.5;
    double yaw= ServiceCarStatus.Lane.yaw;

    double x= PreviewDistance;
    double y;


    y=c-(a*x*x+b*x);
    double difa=0-(atan(2*a*x+b) / PI * 180);
    Curve=0-a;

    //EAng=difa;
    //EPos=y;
    EAng= 0-b;
    EPos = c;
    DEang = 10;
    DEPos = 20;
    //DEang = 20;
    //DEPos = 10;

    IEang = EAng+0.7*IEang;
    IEPos = EPos+0.7*IEPos;
    KIEang = 0;
    //KIEang = 0.5;
    KIEPos =2;



    if(abs(confL)>=2&&abs(confR)>=2){
        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
    }else{
        ang=lastAng;
    }
    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;

    lastEA = EAng;
    lastEP = EPos;

    if (ang > angleLimit) {
        ang = angleLimit;
    }
    else if (ang < -angleLimit) {
        ang = -angleLimit;
    }
    if (lastAng != iv::MaxValue) {
        ang = 0.2 * lastAng + 0.8 * ang;
        //ODS("lastAng：%d\n", lastAng);
    }
    lastAng = ang;
    return ang;
}

int iv::decition::Compute00::bocheCompute(GPS_INS nowGps,GPS_INS aimGps,std::vector<GPS_INS> & xvectorPoint,double & dBocheAngle,double & fDirectRearDis,double l)
{
    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);

    double x_1 = pt.x;
    double y_1 = pt.y;
    double angle_1 = getQieXianAngle(nowGps,aimGps);
    double x_2 = 0.0, y_2 = 0.0;
    double steering_angle;
    double r =6;
    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
    double g_1 = tan(angle_1);
    double car_pos[3] = { x_1,y_1,g_1 };
    double parking_pos[2] = { x_2,y_2 };
    double g_3;
    double t[4][2];
    double p[4];
    double  s1, s2; //切点与车起始位置的距离
    double  min;
    int  min_i;

    //g_3 = 0 - 0.5775;
    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
    //交点
    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
    y_3 = y_1 - g_1 * x_1;
    //圆心1
    x_o_1 = r;
    y_o_1 = g_3 * r + y_3;
    //圆形1的切点1
    x_t_1 = 0.0;
    y_t_1 = g_3 * r + y_3;
    //圆形1的切点2
    if (g_1 == 0)
    {
        x_t_2 = r;
        y_t_2 = y_1 - g_1 * x_1;
    }
    else
    {
        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;

    }
    //圆心2
    x_o_2 = 0 - r;
    y_o_2 = y_3 - g_3 * r;
    //圆形2的切点1
    x_t_3 = 0;
    y_t_3 = y_3 - g_3 * r;
    //圆形2的切点2
    if (g_1 == 0)
    {
        x_t_4 = 0 - r;
        y_t_4 = y_1 - g_1 * x_1;
    }
    else
    {
        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;

    }
    t[0][0] = x_t_1;
    t[0][1] = y_t_1;
    t[1][0] = x_t_2;
    t[1][1] = y_t_2;
    t[2][0] = x_t_3;
    t[2][1] = y_t_3;
    t[3][0] = x_t_4;
    t[3][1] = y_t_4;
    for (int i = 0; i < 4; i++)
    {

        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);

    }
    min = p[0];
    min_i = 0;
    for (int i = 1; i < 4; i++)
    {

        if (p[i] < min)
        {
            min = p[i]; min_i = i;
        }
    }
    if (min_i < 2)
    {
        x_o = x_o_1;
        y_o = y_o_1;
        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
        if (s1 < s2)
        {
            x_t_n = x_t_1;
            y_t_n = y_t_1;
            x_t_f = x_t_2;
            y_t_f = y_t_2;
        }
        else
        {
            x_t_n = x_t_2;
            y_t_n = y_t_2;
            x_t_f = x_t_1;
            y_t_f = y_t_1;

        }
    }
    else
    {
        x_o = x_o_2;
        y_o = y_o_2;
        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);

        if (s1 < s2)
        {

            x_t_n = x_t_3;
            y_t_n = y_t_3;
            x_t_f = x_t_4;
            y_t_f = y_t_4;
        }
        else
        {
            x_t_n = x_t_4;
            y_t_n = y_t_4;
            x_t_f = x_t_3;
            y_t_f = y_t_3;
        }




    }
    steering_angle = atan2(l, r);

    if (x_t_n < 0)
    {
        steering_angle = 0 - steering_angle;
    }

    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
    bocheAngle = steering_angle*180/PI;




    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
    //        return 1;
    //    }
    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
    double disA = GetDistance(aimGps,nowGps);
    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){


        dBocheAngle = bocheAngle;
        fDirectRearDis = sqrt(pow(pt.x - x_t_n,2)+ pow(pt.y - y_t_n,2));
        xvectorPoint.push_back(nearTpoint);
        xvectorPoint.push_back(farTpoint);
        cout << "近切点：x_t_n=" << x_t_n << endl;
        cout << "近切点：y_t_n=" << y_t_n << endl;
        cout << "远切点：x_t_f=" << x_t_f << endl;
        cout << "远切点：y_t_f=" << y_t_f << endl;
        cout << "航向角：" << steering_angle << endl;


        return 1;
    }

    return 0;
}


double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {

    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);



    double x_1 = pt.x;
    double y_1 = pt.y;
    double angle_1 = getQieXianAngle(nowGps,aimGps);
    double x_2 = 0.0, y_2 = 0.0;
    double steering_angle;
    double l = 2.950;
    double r =6;
    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
    double g_1 = tan(angle_1);
    double car_pos[3] = { x_1,y_1,g_1 };
    double parking_pos[2] = { x_2,y_2 };
    double g_3;
    double t[4][2];
    double p[4];
    double  s1, s2; //切点与车起始位置的距离
    double  min;
    int  min_i;

    //g_3 = 0 - 0.5775;
    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
    //交点
    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
    y_3 = y_1 - g_1 * x_1;
    //圆心1
    x_o_1 = r;
    y_o_1 = g_3 * r + y_3;
    //圆形1的切点1
    x_t_1 = 0.0;
    y_t_1 = g_3 * r + y_3;
    //圆形1的切点2
    if (g_1 == 0)
    {
        x_t_2 = r;
        y_t_2 = y_1 - g_1 * x_1;
    }
    else
    {
        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;

    }
    //圆心2
    x_o_2 = 0 - r;
    y_o_2 = y_3 - g_3 * r;
    //圆形2的切点1
    x_t_3 = 0;
    y_t_3 = y_3 - g_3 * r;
    //圆形2的切点2
    if (g_1 == 0)
    {
        x_t_4 = 0 - r;
        y_t_4 = y_1 - g_1 * x_1;
    }
    else
    {
        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;

    }
    t[0][0] = x_t_1;
    t[0][1] = y_t_1;
    t[1][0] = x_t_2;
    t[1][1] = y_t_2;
    t[2][0] = x_t_3;
    t[2][1] = y_t_3;
    t[3][0] = x_t_4;
    t[3][1] = y_t_4;
    for (int i = 0; i < 4; i++)
    {

        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);

    }
    min = p[0];
    min_i = 0;
    for (int i = 1; i < 4; i++)
    {

        if (p[i] < min)
        {
            min = p[i]; min_i = i;
        }
    }
    if (min_i < 2)
    {
        x_o = x_o_1;
        y_o = y_o_1;
        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
        if (s1 < s2)
        {
            x_t_n = x_t_1;
            y_t_n = y_t_1;
            x_t_f = x_t_2;
            y_t_f = y_t_2;
        }
        else
        {
            x_t_n = x_t_2;
            y_t_n = y_t_2;
            x_t_f = x_t_1;
            y_t_f = y_t_1;

        }
    }
    else
    {
        x_o = x_o_2;
        y_o = y_o_2;
        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);

        if (s1 < s2)
        {

            x_t_n = x_t_3;
            y_t_n = y_t_3;
            x_t_f = x_t_4;
            y_t_f = y_t_4;
        }
        else
        {
            x_t_n = x_t_4;
            y_t_n = y_t_4;
            x_t_f = x_t_3;
            y_t_f = y_t_3;
        }




    }
    steering_angle = atan2(l, r);

    if (x_t_n < 0)
    {
        steering_angle = 0 - steering_angle;
    }

    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
    bocheAngle = steering_angle*180/PI;

    cout << "近切点：x_t_n=" << x_t_n << endl;
    cout << "近切点：y_t_n=" << y_t_n << endl;
    cout << "远切点：x_t_f=" << x_t_f << endl;
    cout << "远切点：y_t_f=" << y_t_f << endl;
    cout << "航向角：" << steering_angle << endl;


    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
    //        return 1;
    //    }
    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
    double disA = GetDistance(aimGps,nowGps);
    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
        return 1;
    }

    return 0;

}



//返回垂直平分线的斜率
double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
    double angl, x_3, angle_3;
    if (tan(angle_1 == 0))
    {
        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
        {
            angle_3 = 0 - 1;
        }
        else
        {
            angle_3 = 1;
        }
    }
    else
    {
        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
        angl = tan(angle_1);//车所在直线的斜率
        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
        {
            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
            {
                if (angl < 0)
                {
                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
                }
                else
                {
                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
                }



            }

        }
        else//第二象限
        {
            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
            {
                if (angl < 0)
                {
                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
                }
                else
                {
                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
                }

            }
        }
    }

    return angle_3;

}



double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
    double heading = nowGps.ins_heading_angle *PI/180;
    double x1 = nowGps.gps_x;
    double y1 = nowGps.gps_y;
    if (heading<=PI*0.5)
    {
        heading = 0.5*PI - heading;
    }
    else if (heading>PI*0.5 && heading<=PI*1.5) {
        heading = 1.5*PI - heading;
    }
    else if (heading>PI*1.5) {
        heading = 2.5*PI - heading;
    }
    double k1 = tan(heading);
    double x = x1+10;
    double y = k1 * x + y1 - (k1 * x1);
    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
    double angle = atan(abs(xielv));
    if (xielv<0)
    {
        angle = PI - angle;
    }
    return angle;

}


/*
  chuizhicheweiboche
  */


int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps,GPS_INS aimGps,std::vector<GPS_INS> & xvectorPoint,double & dBocheAngle,double & fDirectRearDis,double l)
{
    double x_0 = 0, y_0 = 0.5;
    double x_1, y_1;//车起点坐标
    double ange1;//车航向角弧度
    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
    double real_rad;;//另一条直线的航向角弧度
    double angle_3;//垂直平分线弧度
    double x_3, y_3;//垂直平分线交点
    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
    double x_o_1, y_o_1;//圆形1坐标
    double x_o_2, y_o_2;//圆形2坐标
    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
    double min_rad;
    double R_M; //后轴中点的转弯半径
    double steering_angle;

    xvectorPoint.clear();


    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
    x_1=pt.x;
    y_1=pt.y;
    ange1=getQieXianAngle(nowGps,aimGps);

    min_rad_zhuanxiang(&R_M , &min_rad);
    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
    steering_angle = atan2(l, R_M);
    x_4 = 0.5;
    y_4 = 0;
    //for (int i = 0; i < 4; i++)
    //{
    //for (int j = 0; j < 2; j++)
    //{
    //	cout << t[i][j] << endl;
    //}
    //}
    //cout << "min_rad:" << min_rad<< endl;
    //cout << "jiaodian：x=" << x_3 << endl;
    //cout << "jiaodian：y=" << y_3 << endl;
    // cout << "R-M：" << R_M << endl;




    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
    dBocheAngle = steering_angle*180/PI;

    double disA = GetDistance(aimGps,nowGps);

    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
        cout << "x_0:" << x_0 << endl;
        cout << "y_0:" << y_0 << endl;
        cout << "x_2:" << x_2 << endl;
        cout << "y_2:" << y_2 << endl;
        cout << "近切点：x_t_n="<< x_t_n << endl;
        cout << "近切点：y_t_n=" << y_t_n << endl;
        cout << "远切点：x_t_f=" << x_t_f << endl;
        cout << "远切点：y_t_f=" << y_t_f << endl;
        xvectorPoint.push_back(dTpoint0);xvectorPoint.push_back(dTpoint1);xvectorPoint.push_back(dTpoint2);xvectorPoint.push_back(dTpoint3);
        fDirectRearDis = sqrt(pow(pt.x - x_t_n,2) + pow(pt.y - y_t_n,2));
        return 1;
    }
    return 0;
}

int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
{



    double l=2.95;//轴距
    double x_0 = 0, y_0 = 0.5;
    double x_1, y_1;//车起点坐标
    double ange1;//车航向角弧度
    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
    double real_rad;;//另一条直线的航向角弧度
    double angle_3;//垂直平分线弧度
    double x_3, y_3;//垂直平分线交点
    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
    double x_o_1, y_o_1;//圆形1坐标
    double x_o_2, y_o_2;//圆形2坐标
    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
    double min_rad;
    double R_M; //后轴中点的转弯半径
    double steering_angle;




    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
    x_1=pt.x;
    y_1=pt.y;
    ange1=getQieXianAngle(nowGps,aimGps);

    min_rad_zhuanxiang(&R_M , &min_rad);
    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
    steering_angle = atan2(l, R_M);
    x_4 = 0.5;
    y_4 = 0;
    //for (int i = 0; i < 4; i++)
    //{
    //for (int j = 0; j < 2; j++)
    //{
    //	cout << t[i][j] << endl;
    //}
    //}
    //cout << "min_rad:" << min_rad<< endl;
    //cout << "jiaodian：x=" << x_3 << endl;
    //cout << "jiaodian：y=" << y_3 << endl;
    // cout << "R-M：" << R_M << endl;
    cout << "x_0:" << x_0 << endl;
    cout << "y_0:" << y_0 << endl;
    cout << "x_2:" << x_2 << endl;
    cout << "y_2:" << y_2 << endl;
    cout << "近切点：x_t_n="<< x_t_n << endl;
    cout << "近切点：y_t_n=" << y_t_n << endl;
    cout << "远切点：x_t_f=" << x_t_f << endl;
    cout << "远切点：y_t_f=" << y_t_f << endl;
    //cout << "航向角：" << steering_angle << endl;
    //cout << "圆心1横坐标=" << x_o_1 << endl;
    //cout << "圆心1纵坐标=" << y_o_1 << endl;
    //cout << "圆心2横坐标=" << x_o_2 << endl;
    //cout << "圆心2纵坐标=" << y_o_2 << endl;
    //cout << "平分线弧度=" << angle_3 << endl;
    //cout << " min_rad=" << min_rad << endl;
    //cout << " real_rad=" << real_rad << endl;
    //   system("PAUSE");



    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
    dBocheAngle = steering_angle*180/PI;

    double disA = GetDistance(aimGps,nowGps);

    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
        return 1;
    }
    return 0;

}


double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
    double L_c = 4.749;//车长
    double rad_1;
    double rad_2;
    double L_k = 1.931;//车宽
    double L = 2.95;//轴距
    double L_f =1.2 ;//前悬
    double L_r =0.7 ;//后悬
    double R_min =6.5 ;//最小转弯半径
    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
    return 0;
}


double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {

    if (x_1 > 0 && y_1 > 0)
    {
        *real_rad = PI*0.5 - min_rad;
        *x_2 = R_M - R_M*cos(min_rad);
        *y_2 = R_M*sin(min_rad) + 0.5;
    }
    else
    {
        *real_rad = PI*0.5 + min_rad;
        *x_2 = R_M*cos(min_rad) - R_M;
        *y_2 = R_M*sin(min_rad) + 0.5;
    }
    return 0;

}


double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
    double b1, b2;
    double k1, k2;
    if (ange1!=(PI*0.5))
    {
        k1 = tan(ange1);
        b1 = y_1 - k1*x_1;
        k2 = tan(real_rad);
        b2 = y_2 - k2*x_2;
        *x_3 = (b2 - b1) / (k1 - k2);
        *y_3 = k2*(*x_3) + b2;
    }
    else
    {
        k2 = tan(real_rad);
        b2 = y_2 - k2*x_2;
        *x_3 = x_1;
        *y_3 = k2*(*x_3) + b2;
    }
    return 0;
}


double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
    double k1, k2;
    double  angle_j;
    k2 = tan(real_rad);
    if (ange1 != (PI*0.5))
    {
        k1 = tan(ange1);
        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角

        if (x_1 > 0 && y_1 > 0)
        {
            *angle_3 = angle_j*0.5 - min_rad + PI;
        }
        else
        {
            *angle_3 = min_rad - angle_j*0.5;
        }
    }
    else
    {
        angle_j = min_rad;//两直线夹角
        if (x_1 > 0 && y_1 > 0)
        {
            *angle_3 = angle_j*0.5 - min_rad + PI;
        }
        else
        {
            *angle_3 = min_rad - angle_j*0.5;
        }
    }
    return 0;
}


double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
    double b2, b3, k2, k3;
    b2 = y_2 - tan(real_rad)*x_2;
    b3 = y_3 - tan(angle_3)*x_3;
    k2 = tan(real_rad);
    k3 = tan(angle_3);
    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
    *y_o_1 = k3*(*x_o_1) + b3;

    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
    *y_o_2 = k3*(*x_o_2) + b3;
    return 0;
}


double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
{
    double x_o, y_o;
    double b2, b3, k1, k2, k3;

    //double car_pos[3] = { x_1,y_1,k1 };
    double parking_pos[2] = { x_2,y_2 };
    //double t[4][2];
    double p[4];
    double  s1, s2; //切点与车起始位置的距离
    double  min;
    int  min_i;
    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
    double t[4][2];
    k1 = tan(ange1);
    b2 = y_2 - tan(real_rad)*x_2;
    b3 = y_3 - tan(real_rad)*x_3;
    k2 = tan(real_rad);//另一条直线斜率
    k3 = tan(angle_3);//垂直平分线斜率
    //圆心1和2切点*********************************************
    if (x_1 > 0 && y_1 > 0)//第一象限
    {
        if (ange1 == (PI*0.5))
        {
            x_t_1 = x_1;
            y_t_1 = y_o_1;
            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;

            x_t_3 = x_1;
            y_t_3 = y_o_2;
            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;


        }
        else
        {
            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;

            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;

        }
    }
    else
    {
        if (ange1 == 0)
        {
            x_t_1 = 0 - x_1;
            y_t_1 = y_o_1;
            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;

            x_t_3 = 0 - x_1;
            y_t_3 = y_o_2;
            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
        }
        else
        {
            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;

            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;

        }

    }

    //圆心1和2切点*********************************************

    t[0][0] = x_t_1;
    t[0][1] = y_t_1;
    t[1][0] = x_t_2;
    t[1][1] = y_t_2;
    t[2][0] = x_t_3;
    t[2][1] = y_t_3;
    t[3][0] = x_t_4;
    t[3][1] = y_t_4;
    for (int i = 0; i < 4; i++)
    {

        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);

    }
    min = p[0];
    min_i = 0;
    for (int i = 1; i < 4; i++)
    {

        if (p[i] < min)
        {
            min = p[i]; min_i = i;
        }
    }
    if (min_i < 2)
    {
        x_o = x_o_1;
        y_o = y_o_1;
        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
        if (s1 < s2)
        {
            *x_t_n = x_t_1;
            *y_t_n = y_t_1;
            *x_t_f = x_t_2;
            *y_t_f = y_t_2;
        }
        else
        {
            *x_t_n = x_t_2;
            *y_t_n = y_t_2;
            *x_t_f = x_t_1;
            *y_t_f = y_t_1;

        }
    }
    else
    {
        x_o = x_o_2;
        y_o = y_o_2;
        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);

        if (s1 < s2)
        {

            *x_t_n = x_t_3;
            *y_t_n = y_t_3;
            *x_t_f = x_t_4;
            *y_t_f = y_t_4;
        }
        else
        {
            *x_t_n = x_t_4;
            *y_t_n = y_t_4;
            *x_t_f = x_t_3;
            *y_t_f = y_t_3;
        }



    }

    return 0;

}


int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
{
    int index = -1;
    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
    int endIndex = gpsMap.size() - 1;
    float minDis=20;

    for (int j = startIndex; j < endIndex; j++)
    {
        int i = (j + gpsMap.size()) % gpsMap.size();
        double tmpdis = GetDistance(rp, (*gpsMap[i]));

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


double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
    double obsSpeed = 0 - realSecSpeed;
    double minDis = iv::MaxValue;
    FrenetPoint esr_obs_F_point;
    for (int i = 0; i < 64; i++)
        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
        {
            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;

            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
            {
                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
                if (tmpDis < minDis)
                {
                    minDis = tmpDis;
//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);

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

    return obsSpeed;
}


int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps
                                                 ,const double xiuzhengCs){
    double minDistance = numeric_limits<double>::max();
    int minDis_index=-1;

    for(int i=0; i<64; ++i){
        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;

            //将毫米波障碍物位置转换到frenet坐标系下
//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);

            //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
            //minDistance、minDis_index用来统计最近的障碍物信息。
 //           if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
            if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+xiuzhengCs)){
                if(esrObsPoint.s<minDistance){
                    minDistance = esrObsPoint.s;
                    minDis_index = i;
                }
            }
        }
    }
    return minDis_index;
}




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