#include <adc_adapter/ge3_adapter.h>
#include <constants.h>
#include <common/car_status.h>
#include <math.h>
#include <iostream>
#include <fstream>


using namespace std;


iv::decition::Ge3Adapter::Ge3Adapter(){
    adapter_id=0;
    adapter_name="ge3";
}
iv::decition::Ge3Adapter::~Ge3Adapter(){

}



iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                   float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){


    float controlSpeed=0;
    float controlBrake=0;
    float realSpeed = now_gps_ins.speed;
    float ttc = 0-(obsDistance/obsSpeed);
    if(ttc<0){
        ttc=15;
    }
    if (accAim < 0)
    {
        if(obsDistance<10 && obsDistance>0 ){
            if(ttc>3){
                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
            }else{
                controlBrake = (int)((0-accAim) * 60.0) + 1;  //(u * 14.0) + 1;
            }
        } else{
            if(ttc<5){
                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
            }else {
                controlBrake = (int)((0-accAim) * 20.0) + 1;  //(u * 14.0) + 1;
            }
        }
        controlSpeed = 0;
        if(obsDistance>50 && ttc>8 &&abs(realSpeed-dSpeed)<3){
            controlBrake=0;
            controlSpeed=max(lastTorque-10.0,0.0);
        }
    }
    else
    {
        controlBrake = 0;
        if(lastTorque==0){
            controlSpeed=100;
        }else{
            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
        }
    }



    if(ServiceCarStatus.bocheMode){
        if(dSpeed<6){
            controlSpeed=min(1.0f,controlSpeed);
            controlBrake=min(5.0f,controlBrake);
            if(abs(dSpeed-realSpeed)<2 && controlBrake>0){
                controlBrake=0;
            }
        }
    }


    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);

    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);

//    if(DecideGps00::minDecelerate<0){
//        controlBrake = max((0-DecideGps00::minDecelerate)*30,controlBrake);
//        controlSpeed=0;
//    }

    if(minDecelerate<0){
        controlBrake = max((0-minDecelerate)*30,controlBrake);
        controlSpeed=0;
    }

    controlBrake=min(controlBrake,100.0f);
    controlBrake=max(controlBrake,0.0f);

    (*decition)->brake = controlBrake;

    (*decition)->torque= controlSpeed;






    lastTorque=(*decition)->torque;


    (*decition)->grade=1;
    (*decition)->mode=1;
    (*decition)->speak=0;
    (*decition)->handBrake=0;
    (*decition)->bright=false;
    (*decition)->engine=0;

    if(ServiceCarStatus.bocheMode){
        (*decition)->dangWei=2;
    }else{
        (*decition)->dangWei=1;
    }


    (*decition)->wheel_angle=max((float)-500.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
    (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);

    (*decition)->air_enable=true;



    return *decition;
}



float iv::decition::Ge3Adapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){

    //刹车限制

    int BrakeIntMax = 0;
    if (realSpeed < 10.0 / 3.6) BrakeIntMax = 40;
    else if (realSpeed < 20.0 / 3.6) BrakeIntMax = 60;
    else BrakeIntMax = 100;

    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;

    if ((obsDistance>0 && obsDistance < 6) || dSpeed == 0)
    {
        controlBrake = max(30.0f, controlBrake);
    }
    if (obsDistance>0 && obsDistance<10&&obsDistance>0)
    {
        controlBrake = max(20.0f, controlBrake);
    }
    if (obsDistance<10&&obsDistance>0 &&ttc<8)
    {
        controlBrake = max(30.0f, controlBrake);
    }
    if (obsDistance<10&&obsDistance>0 &&ttc<5)
    {
        controlBrake = max(40.0f, controlBrake);
    }
    if (obsDistance<10&&obsDistance>0 &&ttc<1.6)
    {
        controlBrake = max(60.0f, controlBrake);
    }
    if(obsDistance<5 && obsDistance>0 ){
        controlBrake = max(60.0f, controlBrake);
    }
    if(obsDistance<5 && obsDistance>0 &&ttc<8){
        controlBrake = max(80.0f, controlBrake);
    }
    controlBrake=min(100.0f,controlBrake);
    return controlBrake;

}

float iv::decition::Ge3Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
    if(controlBrake>0){
        controlSpeed=0;
    }

    if(realSpeed<10){
        controlSpeed=min((float)400.0,controlSpeed);
    }else if(realSpeed<30){
        controlSpeed =min((float)600.0,controlSpeed);
    }
    if(controlSpeed>0){
        if(controlSpeed>lastTorque){
            controlSpeed=min(float(lastTorque+5.0),controlSpeed);
        }
    }
    if(dSpeed <= 3)
    {
        controlSpeed = min(controlSpeed,20.0f);
    }

    controlSpeed=min((float)1200.0,controlSpeed);
    controlSpeed=max((float)0.0,controlSpeed);
    return controlSpeed;
}