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

using namespace std;


iv::decition::ZhongcheAdapter::ZhongcheAdapter(){
    adapter_id= 3;
    adapter_name="zhongche";
}
iv::decition::ZhongcheAdapter::~ZhongcheAdapter(){

}


iv::decition::Decition iv::decition::ZhongcheAdapter::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;
    }





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

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


    if(ttc>10 && (obsDistance>10||obsDistance<0)){





//        if(DecideGps00::minDecelerate<0){
//            (*decition)->torque = 0;
//            (*decition)->brake = max((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
//             dSpeed=10.0;
//        }else{
//            (*decition)->torque=90;
//            (*decition)->brake=0;
//        }

        if(minDecelerate<0){
            (*decition)->torque = 0;
            (*decition)->brake = max((0-minDecelerate)*90, (*decition)->brake);
             dSpeed=10.0;
        }else{
            (*decition)->torque=90;
            (*decition)->brake=0;
        }


        dSpeed=max(0.0f,dSpeed);
        dSpeed=min(10.0f, dSpeed);
        (*decition)->speed=(int)dSpeed*10+5;

        (*decition)->torque=min((float)100.0,(*decition)->torque);
        (*decition)->torque=max((float)0.0,(*decition)->torque);
        (*decition)->brake=min((float)100.0,(*decition)->brake);
        (*decition)->brake=max((float)0.0,(*decition)->brake);

        lastTorque=(*decition)->torque;

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

        (*decition)->wheel_angle=max((float)-550.0,(*decition)->wheel_angle);
         (*decition)->wheel_angle=min((float)550.0,(*decition)->wheel_angle);
        return *decition;
    }



    if (accAim < 0)
    {
        if(obsDistance<10 && obsDistance>0 &&ttc>3){
            controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
        } else{
            if(ttc<3){
                controlBrake = (int)(u * 60.0) + 1;  //(u * 14.0) + 1;
            }
            else if(ttc<5){
                controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
            }else {
                controlBrake = (int)(u * 20.0) + 1;  //(u * 14.0) + 1;
            }
        }
          (*decition)->brake = controlBrake;
        (*decition)->torque=0;

    }
    else
    {
        controlBrake = 0;
        (*decition)->brake=0;

        if(lastTorque==0){
             (*decition)->torque = u*(-30)+((0-u)-ServiceCarStatus.mfCalAcc)*1+
                    now_gps_ins.ins_pitch_angle*10;
        }else{
             (*decition)->torque = lastTorque+((0-u)-ServiceCarStatus.mfCalAcc)*10;
        }
        if(realSpeed<10){
             (*decition)->torque=min((float)40.0,(*decition)->torque);
        }else if(realSpeed<30){
             (*decition)->torque=min((float)60.0,(*decition)->torque);
        }

         (*decition)->torque=min((float)99.0,(*decition)->torque);
         (*decition)->torque=max((float)0.0,(*decition)->torque);

    }

    //0227 10m nei xianzhi shache
    if(obsDistance<10 &&obsDistance>0){
        (*decition)->torque=0;
        (*decition)->brake=max((float)30.0,(*decition)->brake);
    }


//    if(DecideGps00::minDecelerate<0){
//        dSpeed=10;
//        (*decition)->torque = 0;
//        (*decition)->brake = min((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
//    }

    if(minDecelerate<0){
        dSpeed=10;
        (*decition)->torque = 0;
        (*decition)->brake = min((0-minDecelerate)*90, (*decition)->brake);
    }


    (*decition)->torque=min((float)100.0,(*decition)->torque);
    (*decition)->torque=max((float)0.0,(*decition)->torque);
    (*decition)->brake=min((float)100.0,(*decition)->brake);
    (*decition)->brake=max((float)0.0,(*decition)->brake);

    lastTorque=(*decition)->torque;

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

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



    dSpeed=max(0.0f,dSpeed);
    dSpeed=min(10.0f, dSpeed);
    (*decition)->speed=(int)dSpeed*10+5;


std::cout<<"==========================================="<<std::endl;
     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
    <<std::endl;
std::cout<<"========================================="<<std::endl;

    return *decition;
}