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

QTime doorOpenTime;

using namespace std;

iv::decition::HapoAdapter::HapoAdapter(){
    adapter_id=5;
    adapter_name="hapo";
}
iv::decition::HapoAdapter::~HapoAdapter(){

}

#include "ivlog.h"
extern iv::Ivlog * givlog;

iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
    accAim=min(0.7f,accAim);

    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<6){
            controlBrake= u*1.5;
        }
         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around
                 && dSpeed>0 && lastBrake==0){
            controlBrake=0;
            controlSpeed=0;
        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around){
            controlBrake=min(controlBrake,0.5f);
            controlSpeed=0;
        }

       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>5
                  && !turn_around ){
            controlBrake=min(controlBrake,0.3f);
            controlSpeed=0;
        }
        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>5
                && dSpeed>0 && !turn_around ){
            controlBrake=min(controlBrake,0.5f);
            controlSpeed=0;
        }
         else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>=15
                 && dSpeed>0 && !turn_around ){
             controlBrake=0;
             controlSpeed= 2.0* (realSpeed/15.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,2.0f);
             controlSpeed =max( controlSpeed,1.0f);

        }else{
            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//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(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
//            if(controlSpeed>0){
//                controlSpeed = ServiceCarStatus.torque_st;
//            }
//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
//            if(controlSpeed>0 ){
//                controlSpeed = ServiceCarStatus.torque_st;
//            }
//        }


        if(controlSpeed>ServiceCarStatus.torque_st){
            controlSpeed = min(ServiceCarStatus.torque_st+2.0f,controlSpeed);
        }else if(controlSpeed<ServiceCarStatus.torque_st){
            controlSpeed = ServiceCarStatus.torque_st-1.0f;
        }




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

       if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
          controlSpeed=2.4;
        }
        //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
//        }

    }
//0125
    if(controlSpeed>0){
        controlSpeed=max(controlSpeed,2.4f);
    }

    //0227 10m nei xianzhi shache
    if(obsDistance<6 &&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*10;
      (*decition)->brake = controlBrake*6;

    (*decition)->torque=controlSpeed;
      lastBrake= (*decition)->brake;





    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)->air_enable=false;
    (*decition)->driveMode=1;
    (*decition)->brakeType=0;
    (*decition)->angSpeed=60;
    (*decition)->topLight=0;


    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
        (*decition)->dangWei=2;
        (*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)->handBrake=false;
    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;

    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
     (*decition)->wheel_angle=min((float)870.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;

     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
     {
          (*decition)->mode=0;
     }


     if(ServiceCarStatus.station_control_door==0)
     {
         (*decition)->door=false;      //CLOSE
         doorOpenTime.start();
         givlog->debug("DOOR","STATUS is: %d",5);

     }else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
     {
         (*decition)->door=true;       //OPEN
         (*decition)->brake =6;
         (*decition)->torque=0;
         if(doorOpenTime.elapsed()>6000)
         {
             ServiceCarStatus.station_control_door_option=true;
             givlog->debug("DOOR","STATUS is: %d",8);
         }
     }

givlog->debug("DOOR","door is: %d",(*decition)->door);
givlog->debug("DOOR","station_control_door is: %d",ServiceCarStatus.station_control_door);

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


   (*decition)->accelerator=  (*decition)->torque ;


givlog->debug("obs_distance","dSpeed: %f, realSpeed: %f, brake: %f",
              dSpeed, realSpeed,(*decition)->brake);

    return *decition;
}



float iv::decition::HapoAdapter::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 {
        BrakeIntMax = 5;
    }
    if(obsDistance>0 && obsDistance<10){
        BrakeIntMax=max(BrakeIntMax,3);
    }

    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;




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

}

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

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