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


using namespace std;


iv::decition::YuHeSenAdapter::YuHeSenAdapter(){
    adapter_id= 7;
    adapter_name="yuhesen";
}
iv::decition::YuHeSenAdapter::~YuHeSenAdapter(){

}



iv::decition::Decition iv::decition::YuHeSenAdapter::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=accAim;
    float controlBrake=0;
    float u = 0- accAim;
    float realSpeed = now_gps_ins.speed;
    float ttc = 0-(obsDistance/obsSpeed);
    bool turn_around=false;
    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
        turn_around=true;
    }

    if(ttc<0){
        ttc=15;
    }
    if (accAim < 0)
    {
        //0110
        if(changingDangWei){
            controlSpeed=min(-0.5f,controlSpeed);
        }
//        else if((obsDistance<0 ||obsDistance>50)&&(abs(dSpeed-realSpeed)<3)){
//             controlSpeed=max(-0.2f,controlSpeed);
//        }
    }
    else
    {
        controlSpeed=min(1.0f,controlSpeed);

    }

    //0227 10m nei xianzhi shache
    if(obsDistance<10 &&obsDistance>0){
        controlSpeed=min(controlSpeed,-0.8f);
    }


//    if(DecideGps00::minDecelerate<0){
//        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
//    }

    if(minDecelerate<0){
        controlSpeed = min(minDecelerate, controlSpeed);
    }
    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);

    (*decition)->accelerator=controlSpeed;

    if((*decition)->leftlamp){
        (*decition)->directLight=1;
    }else if((*decition)->rightlamp){
         (*decition)->directLight=2;
    }else{
        (*decition)->directLight=0;
    }


    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;

    (*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
     (*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);


        (*decition)->brake=0;




std::cout<<"==========================================="<<std::endl;
     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
    <<std::endl;
std::cout<<"========================================="<<std::endl;
(*decition)->speed = dSpeed/3.6;
(*decition)->wheel_angle=(*decition)->wheel_angle*0.1;
(*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
(*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
(*decition)->mode=1;
(*decition)->dangWei=1;
if((*decition)->brake>0){
    (*decition)->speed=0;
}
    return *decition;
}



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

    //刹车限制

    //刹车限制

    float vs =realSpeed*3.6;
    int BrakeIntMax = 10;
    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;

    //`
    if(ttc>10){
        BrakeIntMax = 2;
    }else if(ttc>6){
        BrakeIntMax = 3;
    }else if(ttc>3){
        BrakeIntMax = 4;
    }else if(ttc>1.6){
        BrakeIntMax = 5;
    }else if(ttc>0.8){
        BrakeIntMax = 8;
    }else{
        BrakeIntMax = 10;
    }
    if(obsDistance>0 && obsDistance<10){
        BrakeIntMax=max(BrakeIntMax,3);
    }

    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;




    controlBrake=min(10.0f,controlBrake);
    controlBrake=max(0.0f,controlBrake);
    return controlBrake;

}

float iv::decition::YuHeSenAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){

    controlSpeed=min((float)5.0,controlSpeed);
    controlSpeed=max((float)-7.0,controlSpeed);
    return controlSpeed;
}