#include <adc_controller/pid_controller.h>
#include <common/constants.h>
#include <common/car_status.h>
#include <math.h>
#include <iostream>
#include <fstream>


using namespace std;


iv::decition::PIDController::PIDController(){
    controller_id = 0;
    controller_name="pid";
}
iv::decition::PIDController::~PIDController(){

}



/**
 * @brief getControlDecition
 * pid方式计算横向方向盘和纵向加速度
 *
 * @param now_gps_ins          实时gps信息
 * @param path                 目标路径
 * @param dSpeed               决策速度
 * @param ObsDistacne          障碍物距离
 * @param ObsSpeed             障碍物速度
 * @param computeAngle         是否要计算方向盘角度
 * @param computeAcc           是否要计算纵向加速度
 * @param decition             决策信息结构体的指针
 * @return                     iv::decition::Decition
 */

iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path, float dSpeed, float obsDistacne, float obsSpeed,
                                                                       bool computeAngle, bool computeAcc, Decition *decition){

    float realSpeed= now_gps_ins.speed;
    int roadMode = now_gps_ins.roadMode;
    if(computeAngle){
        (*decition)->wheel_angle=getPidAngle( realSpeed,  path,roadMode);
    }
    if(computeAcc){
        (*decition)->accelerator=getPidAcc( now_gps_ins, dSpeed,  obsDistacne,  obsSpeed);
    }
    return *decition;
}




float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
    double ang = 0;
    double EPos = 0, EAng = 0;

    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0,IEpos=0,IEang=0;

    bool turning=false;
    if(roadMode==14||roadMode==15||roadMode==16||roadMode==17){
        turning=true;
    }

    double PreviewDistance;//预瞄距离

    if(ServiceCarStatus.msysparam.mvehtype=="ge3" || ServiceCarStatus.msysparam.mvehtype=="vv7"){
        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
        if(turning){
           realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
        }else{
          realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
        }
    } else{
        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
    }

    //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
    //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
    //        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);

    //    }
    //   if(realSpeed <15){
    //    PreviewDistance = max(4.0, realSpeed *0.4) ;
    //   }
    //    if(realSpeed <10){
    //        PreviewDistance = max(3.0, realSpeed *0.3) ;
    //    }
    double sumdis = 0;
    int gpsIndex = 0;
    std::vector<Point2D> farTrace;

    int nsize = trace.size();
    for (int i = 1; i < nsize-1; i++)
    {
        sumdis += GetDistance(trace[i - 1], trace[i]);
        if (sumdis > PreviewDistance)
        {
            gpsIndex = i;
            break;
        }
    }

    EPos = trace[gpsIndex].x;

    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), trace.size()); i++) {
        farTrace.push_back(trace[gpsIndex]);
    }
    if (farTrace.size() == 0) {
        EAng = 0;
    }
    else {
        EAng = getAveDef(farTrace);
    }
    if(abs(realSpeed)<3){
        eAngSum=0;
        ePosSum=0;
    }else{
        eAngSum=eAngSum*0.8+EAng;
        ePosSum=ePosSum*0.8+EPos;
    }

    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)
            +IEang*eAngSum + IEpos*ePosSum;

    lastEA = EAng;
    lastEP = EPos;

    //    if (ang > angleLimit) {
    //        ang = angleLimit;
    //    }
    //    else if (ang < -angleLimit) {
    //        ang = -angleLimit;
    //    }
    if (lastAng >-3000&&lastAng<3000) {
        ang = 0.2 * lastAng + 0.8 * ang;
    }
    if(ang >-3000&&ang<3000){
        lastAng = ang;
    }else{
        ang=lastAng;
    }
    return ang;

}



float iv::decition::PIDController::getAveDef(std::vector<Point2D> farTrace){
    double sum_x = 0;
    double sum_y = 0;
    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
    {
        sum_x += farTrace[i].x;
        sum_y += abs(farTrace[i].y);
    }
    double average_y = sum_y / min(5, (int)farTrace.size());
    double average_x = sum_x / min(5, (int)farTrace.size());
    return atan(average_x / average_y) / M_PI * 180;
}



float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistance, float obsSpeed){

    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;

    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
    {
            static int obs_line=0;
            if(obsDistance<6 && obsDistance>0){
                dSpeed=0;
                obs_line=1;
            }
            if(obs_line==1)
            {
                if(obsDistance<8 && obsDistance>0){
                    dSpeed=0;
                }else{
                    obs_line=0;
                }
            }
    }else if(obsDistance<10 && obsDistance>0){
        dSpeed=0;
    }

    float dSecSpeed = dSpeed / 3.6;
    float realSpeed=gpsIns.speed;
    float secSpeed =realSpeed/3.6;
    double vt = dSecSpeed;
    double vs = secSpeed;
    if (abs(vs) < 0.05) vs = 0;
    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
    double vl = vs + obsSpeed;
    double vh = vs;
    double dmax = 150;

    //车距滤波
    if (obsDistance < 0||obsDistance>100) {
        obsDistance = 500;
        obsSpeed=0;
    }


    if (obsDistance > 150) vl = 25; //25m/s


    //TTC计算
    double ds = 0.2566 * vl * vl + 5;
    double ttcr = (vh - vl) / obsDistance;
    double ttc = 15;
    if (15 * (vh - vl) > obsDistance)
        ttc = obsDistance / (vh - vl);


    ServiceCarStatus.mfttc = ttc;


    if (obsDistance <= dmax)
    {
        if (ttc > 10)
        {
            if (obsDistance > ds + 5)
            {
                double dds = min(30.0, obsDistance - (ds + 5));
                vt = vt * dds / 30 + vl * (1 - dds / 30);
            }
            else
            {
                vt = min(vt, vl);
            }
        }
        else
        {
            vt = min(vt, vl);
        }
    }else{
        vt=dSecSpeed;
    }

    vt=min((float)vt,dSecSpeed);
    std::cout << "\nvt:%f\n" << vt << std::endl;

    //计算加速度
    float u =computeU(obsDistance,ds,vs,vt,vh,vl,ttc,ttcr);

    //u 限值
    u=limiteU(u,ttc);

    lastVt = vt;
    lastU = u;
    float acc=0-u;
    return acc;
}


float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr){

    double Id = 0;
    double ed = ds - obsDistance;
    if (obsDistance > 150) ed = 0;
    double ev = vs - vt;
    double u = 0;
    if (ttc>10)
    {
        double kp = 0.5;        //double kp = 0.5;
        double kd = 0.5;       //kd = 0.5
        //      double k11 = 0.025;
        //      double k12 = 0.000125;
        double dev = (ev - lastEv) / 0.1;
        Iv = 0.925 * Iv + ev;
        Id = 0.85 * Id + ed;
        if (abs(vh) < 2.0&& abs(vl) < 2.0)
        {
            Iv = 0.0; Id = 0.0;
        }
        //u = kp * ev + kd * dev + k11 * Iv + k12 * Id;
        u = kp * ev + kd * dev;
    }
    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
    {
        //AEB控制
        Id = 0; Iv = 0;
        u = 0;
        if (ttc < 0.8) u = 7;
        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
        else
        {
            u = (10 - ttc) * (10 - ttc) / 23.52;
        }
    }
    else
    {
        //制动控制
        Id = 0; Iv = 0;
        if (obsDistance > 1.25 * ds)
        {
            double rev_ed = 1 / ed;
            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
        }
        else
        {
            if (abs(vl) > 2.0)
                u = 0;
            else
                u = max(lastU, 1.0f);
        }
    }
    if (abs(vl) < 1.0 && abs(vh) < 1.0
            && obsDistance < 2 * ds)
    {
        u = 0.5;
    }
    lastEv = ev;
    return u;
}


float iv::decition::PIDController::limiteU(float u,float ttc){
    if(ttc<3 && u<-0.2){
        u=-0.2;
    }
    else
    {
        if (u < -1.5) u = -1.5;
    }
    if (u >= 0 && lastU <= 0)
    {
        if (u > 0.5) u = 0.5;
    }
    else if (u >= 0 && lastU >= 0)
    {
        if (u > lastU + 0.5) u = lastU + 0.5;
    }
    else if (u <= 0 && lastU >= 0)
    {
        if (u < -0.04) u = -0.04;
    }
    else if (u <= 0 && lastU <= 0)
    {
        if (u < lastU - 0.04) u = lastU - 0.04;
    }
    return u;
}