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


using namespace std;


iv::decition::QingYuanAdapter::QingYuanAdapter(){
    adapter_id=1;
    adapter_name="qingyuan";
}
iv::decition::QingYuanAdapter::~QingYuanAdapter(){

}



iv::decition::Decition iv::decition::QingYuanAdapter::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 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)
    {
        controlSpeed=0;
        controlBrake=u; //102
        if(obsDistance>0 && obsDistance<10){
            controlBrake= u*1.5;
        }
        if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
            controlBrake=0;
            controlSpeed=max(0.0,ServiceCarStatus.torque_st-10.0);
        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
            controlBrake=0;
            controlSpeed=0;
        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
            controlBrake=min(controlBrake,0.5f);
            controlSpeed=0;
        }

        else if(abs(dSpeed-realSpeed)>=3 &&abs(dSpeed-realSpeed)<5&&(obsDistance>40 || obsDistance<0)&&ttc>8
                && dSpeed>0 && !turn_around ){
            controlBrake=0;
            controlSpeed=0;
        }else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
                 && dSpeed>0 && !turn_around ){
            controlBrake=min(controlBrake,0.3f);
            controlSpeed=0;
        }
        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
                && dSpeed>0 && !turn_around ){
            controlBrake=min(controlBrake,0.5f);
            controlSpeed=0;
        }


        //0110
        if(changingDangWei){
            controlBrake=max(0.5f,controlBrake);
        }

        //斜坡刹车加大 lsn 0217
        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
            controlBrake=max(2.0f,controlBrake);
        }
        //斜坡刹车加大 end





    }
    else
    {
        controlBrake = 0;

        if(ServiceCarStatus.torque_st==0){
            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
            controlSpeed =min( controlSpeed,1.0f);

        }else{
            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*10;//1115    *10
        }

        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
        //            if(realSpeed<5  ){
        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
        //            }else if(realSpeed<10){
        //                controlSpeed=min((float)25.0,controlSpeed);  //40
        //            }
        //        }


        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
        if(controlSpeed>ServiceCarStatus.torque_st){
            controlSpeed = ServiceCarStatus.torque_st+1.0;
        }else if(controlSpeed<ServiceCarStatus.torque_st){
            controlSpeed = ServiceCarStatus.torque_st-1.0;
        }






        //Seizing
        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1&&realSpeed<0.5){
            seizingCount++;
        }else{
            seizingCount=0;
        }
        if(seizingCount>=10){
            controlSpeed=ServiceCarStatus.torque_st+2;
        }
        //seizing end




        // 斜坡加大油门   0217 lsn

        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
                && abs(realSpeed)<1.0){
            controlSpeed=max((float)20.0,controlSpeed);
            controlSpeed=min((float)40.0,controlSpeed);
        }
        // 斜坡加大油门  end


        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
                && abs(realSpeed)<10.0){
            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
        }

    }

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


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

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

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

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

    (*decition)->brake = controlBrake;

    (*decition)->torque=controlSpeed;






    lastTorque=(*decition)->torque;




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

    (*decition)->dangweiEnable=true;
    (*decition)->angleEnable=true;
    (*decition)->speedEnable=true;
    (*decition)-> brakeEnable=true;
    (*decition)->driveMode=1;
    (*decition)->brakeType=0;
    (*decition)->angSpeed=60;
    (*decition)->topLight=0;


    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
        (*decition)->dangWei=3;
        (*decition)->backLight=1;
    }
    //1220
    else{
        (*decition)->dangWei=4;
        (*decition)->backLight=0;
    }



    if((*decition)->brake>0){
        (*decition)->brakeLight=1;
    }else{
        (*decition)->brakeLight=0;
    }


    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)-570.0,(*decition)->wheel_angle);
     (*decition)->wheel_angle=min((float)570.0,(*decition)->wheel_angle);




        //dangweiTrasfer
//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
//                 decition_gps->dangWei=2;
//                 decition_gps->torque=0;
//             }
     lastDangWei= (*decition)->dangWei;

            (*decition)->topLight=0; //1116
            (*decition)->nearLight=0;
             (*decition)->farLight=0;



       //(*decition)->door=3;

std::cout<<"==========================================="<<std::endl;
     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
             <<"  decideTorque:"<<(*decition)->torque
    <<std::endl;
std::cout<<"========================================="<<std::endl;

    return *decition;
}



float iv::decition::QingYuanAdapter::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::QingYuanAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){

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