#include <QCoreApplication>

#include <QTime>

#include <QMutex>

#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"

#include "canmsg.pb.h"
#include "decition.pb.h"
#include "chassis.pb.h"

#include "torquebrake.h"

#include <thread>
#include <math.h>

#ifdef Q_OS_LINUX
#include <signal.h>
#endif

#include "candbc.h"
#include "sterraes.h"
#include "dbcsigpacker.h"

std::string gstrdbcpath;
bool gbUseOutDBC;

sterraes * gpsterraes;

static void * gpacansend;
static void * gpadecition;
static void * gpachassis;

static std::string gstrmemdecition;
static std::string gstrmemcansend;
static std::string gstrmemchassis;
static bool gbSendRun = true;

static bool gbChassisEPS = false;

static iv::brain::decition gdecition_def;
static iv::brain::decition gdecition;

static QTime gTime;
static int gnLastSendTime = 0;
static int gnLastRecvDecTime = -1000;
static int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;
static int gnIndex = 0;

static bool gbHaveVehSpd = false;
static double gfVehSpd = 0.0;

static bool gbHaveWheelAngle = false;
static double gfWheelAngle = 0.0;

static double gfWheelSpeedLim = 300; //300 degrees per second


static QMutex gMutex;

static std::thread * gpsendthread = NULL;

unsigned char ADS_EPS_1[24];// 0x195/405
unsigned char ADS_EPS_3[24]; // 0x1BC/444
unsigned char ADS_ONEBOX_1[24];// 0x159/345
unsigned char ADS_VCU_1[24];   // 0x167/359
unsigned char ADS_ONEBOX_2[24];
unsigned char ADS_ONEBOX_3[24];
unsigned char ADS_COM2[8];
unsigned char ADS_COM3[24];

static int gnState = 0; //0 not act  1 act


double gfWheelReq = 0.0;
double gfTorqueReq = 0.0;
double gfBrakeReq = 0.0;


static bool gbSendBrake = false;


static double kp = 0.5;
static double ki = 0.0;
static double kd =  0.0;
static double MAXACC = 3.0;
static int64_t gnLastAccUptime = 0;
static bool gbUsePID = true;
static bool gbPrintPIDOut = true;
static double TorqueRatio = 1.4;

static double gfVehAcc = 0.0;

void SetMsgSignal(std::string strmsgname,std::string strsigname,double fvalue){
    gpsterraes->SetMsgSignal(strmsgname,strsigname,fvalue);
}


void ExecSend();

double acctotorque(double facc)
{
    double fVehWeight = 1800;
    double fRollForce = 50;
    const double fRatio = 2.5;
    double fNeed = fRollForce + fVehWeight*facc;
    double ftorque = fNeed/fRatio;
    return ftorque;
}

double  PIDTorque(double torque)
{
    if(gbUsePID == false)return torque;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();

    if((nnow - gnLastAccUptime)>1e9)return torque;

    std::vector<int64_t> xvectorintegtime;
    std::vector<double> xvectorintegerr;

    static double error_previous = 0.0;
    static double integral = 0.0;
    double fVehWeight = 1800;
    double fRollForce = 50;
    const double fRatio = 2.5;
    double accAim = (torque * fRatio - fRollForce)/fVehWeight;

    static bool bPrintTitle = false;


    double error = accAim - gfVehAcc;
    double dt = 0.01; //10ms
    double diff = (error - error_previous)/dt;
//    integral = integral + error * dt * ki;
    xvectorintegtime.push_back(nnow);
    xvectorintegerr.push_back(error);
    unsigned int i;
    unsigned int nsize = 0;//static_cast<unsigned int>(xvectorintegerr.size());
    while(xvectorintegerr.size()>0)
    {
        if(abs(nnow - xvectorintegtime[0])>1e9){
            xvectorintegerr.erase(xvectorintegerr.begin());xvectorintegtime.erase(xvectorintegtime.begin());
        }
        else {
            break;
        }
    }
    integral = 0;
    nsize = static_cast<unsigned int>(xvectorintegerr.size());
    for(i=0;i<nsize;i++)integral = xvectorintegerr[i]*dt*ki;
    double output = error * kp + integral + diff * kd;

    double fAccAjust = accAim + output;
    if(fAccAjust>MAXACC)fAccAjust = MAXACC;
    if(fAccAjust<=0)fAccAjust = 0.0;

    double ftorqueAjust = acctotorque(fAccAjust);

    error_previous = error;

    if(bPrintTitle == false)
    {
        std::cout<<"Time\t"<<"Speed(km/h)\t"<<"RealAcc(m/s2)\t"<<"AimAcc(m/s2)\t"<<"AjustAcc(m/s2)\t"<<"Error(m/s2)\t"<<"RawTorque\t"<<"AjustTorque"<<std::endl;
        bPrintTitle = true;
    }

    if(gbPrintPIDOut)
    {
        int64_t timesecond = nnow/1e9;
        int64_t timepart = nnow - timesecond * 1e9;
        double fnow = timepart; fnow = fnow/1e9;
        fnow = fnow + timesecond;
        std::cout<<fnow<<"\t"<<gfVehSpd<<"\t"<<gfVehAcc<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
    }

    return ftorqueAjust;
}

#ifdef TESTBRAKE
int testnum = 0;
#endif

int testwheel = 0;

#ifdef PIDTEST
int pidtestnstep = 0;
double fpidtestacc = 1.0;
double fpidtestmaxvel = 15.0;
#endif

void executeDecition(const iv::brain::decition &decition)
{
    double fwheel = decition.wheelangle()*0.9;
    if(fwheel<-430)fwheel = 430;
    if(fwheel>380)fwheel = 380;

    double ftorque = decition.torque() * TorqueRatio;
    double fbrake = decition.brake();

#ifdef PIDTEST
    if(gfVehSpd>=fpidtestmaxvel)
    {
        pidtestnstep = 1;
    }

    if(pidtestnstep == 1)
    {
        ftorque = 0;
        fbrake = -1.0;
    }

    if(pidtestnstep == 0)
    {
        ftorque = PIDTorque(acctotorque(fpidtestacc));
        fbrake = 0;
    }
#else
    ftorque = PIDTorque(ftorque);
#endif



    SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",fwheel);
    SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
    SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
    SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
    SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
    SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
    SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);

#ifdef TESTBRAKE
    if((testnum < 1000) || (testnum > 1500))
    {
#endif
    if(fbrake<(-0.0001))
    {
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTq",0.0);

        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",decition.brake());
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",1.0);

        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",0.0);

        if(gbSendBrake == false)   //if brake is unavailable
        {

            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);

            SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
            SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);

            if(fabs(gfVehSpd)>0.1)
            {
                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Warning: can't brake."<<std::endl;
            }
        }
 //       ADS_1_PilotParkCtrlRepMod = 1.0;
 //       std::cout<<" send brake "<<std::endl;
    }

    else
    {
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTq",ftorque);
        if(fabs(gfVehSpd) < 0.1)
        {
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",1.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",1.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",1.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",1.0);
        }
        else
        {
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
            SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);
        }

        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);

        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);

    }

#ifdef TESTBRAKE
   }

   else
   {

       if(fabs(gfVehSpd) < 0.1)
       {
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",1.0);
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",1.0);
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",1.0);
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",1.0);
       }
       else
       {
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
           SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);

       }
       SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
       SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
       SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
       SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);

       SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTq",300);


   }

   testnum++;
   if(testnum > 1500)testnum = 0;
#endif



//    std::cout<<" send dec. "<<std::endl;

//    ADS_1_SteerAgReq = 90.0;//decition.wheelangle();

}

void Activate()
{
    //    for(int j=0;j<100000;j++)
    //    {
    std::cout<<" activate "<<std::endl;
    for(int i = 0; i < 3; i++){

        SetMsgSignal("ADS_VCU_1","ADS_1_DrvCtrlReq",1.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_CtrlReqMod",1.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTqVld",1.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTqEnable",1.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTq",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_ADCCAvl",1.0);

        SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",gfWheelReq);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",1.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",2.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
        SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
        SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);

        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);

        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
        SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);

        SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",0.0);

        SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
        SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",3.0);



  //      std::cout<<"activate."<<std::endl;

        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    //    }
}

void UnAcitvate()
{

    for(int i = 0; i < 3; i++){

        SetMsgSignal("ADS_VCU_1","ADS_1_DrvCtrlReq",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_CtrlReqMod",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTqVld",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTqEnable",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_DrvTarTq",0.0);
        SetMsgSignal("ADS_VCU_1","ADS_1_ADCCAvl",0.0);

        SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",0.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",0.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",0.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
        SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
        SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
        SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);


        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
        SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);

        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
        SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);

        SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);

        SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",1.0);

        SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
        SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",0.0);


        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::chassis xchassis;
    //    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }

    if(xchassis.has_epsmode())
    {
        if(xchassis.epsmode() == 0)
        {
            gbChassisEPS = true;
        }
    }

    if(xchassis.has_vel())
    {
        gfVehSpd = xchassis.vel();
        gbHaveVehSpd = true;
        //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
    }

    if(xchassis.has_angle_feedback())
    {
        gfWheelAngle = xchassis.angle_feedback();
        gbHaveWheelAngle = true;
    }
}


void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    static qint64 oldtime = QDateTime::currentMSecsSinceEpoch();
    iv::brain::decition xdecition;

    if(!xdecition.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenDecition parse error."<<std::endl;
        return;
    }



    //    if(xdecition.gear() != 4)
    //    {
    //        qDebug("not D");
    //    }
    xdecition.set_angle_mode(1);
    xdecition.set_angle_active(1);
    xdecition.set_acc_active(1);
    xdecition.set_brake_active(1);
    //    xdecition.set_brake_type(1);
    xdecition.set_auto_mode(3);


//    std::cout<<"decition acc: "<<xdecition.accelerator()<<std::endl;

    //   xdecition.set_wheelangle(45.0);

    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %lld diff is %lld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
    oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
    gbChassisEPS = false;

}


void PrepareMsg()
{
    static int rollcouter = 0;
    //    std::cout<<" roll count:: "<<rollcouter<<std::endl;
    SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr1",rollcouter);
    SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr2",rollcouter);
    // set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
    // set_EPS1_signal("CutOutMAC_2CB_S",1.0);
    gpsterraes->GetEPS1Data(ADS_EPS_1);

    SetMsgSignal("ADS_VCU_1","ADS_1_RollgCntr1",rollcouter);
    SetMsgSignal("ADS_VCU_1","ADS_1_RollgCntr2",rollcouter);
    gpsterraes->GetVCU1Data(ADS_VCU_1);

    SetMsgSignal("ADS_ONEBOX_2","FCM_2_RollgCntr1",rollcouter);
    gpsterraes->GetONEBOX2Data(ADS_ONEBOX_2);

    SetMsgSignal("ADS_ONEBOX_3","FCM_3_RollgCntr1",rollcouter);
    gpsterraes->GetONEBOX3Data(ADS_ONEBOX_3);

    SetMsgSignal("ADS_COM_3","ADS_3_RollgCntr1",rollcouter);
    gpsterraes->GetADSCOM3Data(ADS_COM3);

    SetMsgSignal("ADS_COM_2","ADS_2_RollgCntr1",rollcouter);
    gpsterraes->GetADSCOM2Data(ADS_COM2);

    rollcouter++;
    if(rollcouter>=14)rollcouter = 0;
}

void ExecSend()
{
    static int nrd = 0;
    PrepareMsg();
//    gpsterraes->GetEPS1Data(ADS_EPS_1);
    static int nCount = 0;
    nCount++;
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
    //    unsigned char * strp = (unsigned char *)&(ServiceControlStatus.command10.byte[0]);
    //    qDebug("%02x %02x %02x %02x %02x %02x %02x %02x",strp[0],strp[1],strp[2],strp[3],strp[4],strp[5],strp[6],strp[7]);

    xraw.set_id(0x195);
    xraw.set_data(ADS_EPS_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw195 = xmsg.add_rawmsg();
    pxraw195->CopyFrom(xraw);
    //    qDebug(" 0x144: %02X %02X %02X %02X %02X %02X %02X %02X",byte_144[0],byte_144[1],byte_144[2],byte_144[3],
    //            byte_144[4],byte_144[5],byte_144[6],byte_144[7]);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(0x1BC);
    xraw.set_data(ADS_EPS_3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    xmsg.set_channel(0);
 //   iv::can::canraw * pxraw1BC = xmsg.add_rawmsg();
//    pxraw1BC->CopyFrom(xraw);

    xraw.set_id(0x159);
    xraw.set_data(ADS_ONEBOX_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    // iv::can::canraw * pxraw159 = xmsg.add_rawmsg();
    // pxraw159->CopyFrom(xraw);

    xraw.set_id(0x145);
    xraw.set_data(ADS_ONEBOX_2,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw145 = xmsg.add_rawmsg();
    pxraw145->CopyFrom(xraw);

    if(nrd == 0)
    {
    xraw.set_id(0x31A);
    xraw.set_data(ADS_COM3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw31A = xmsg.add_rawmsg();
    pxraw31A->CopyFrom(xraw);

    xraw.set_id(0x314);
    xraw.set_data(ADS_COM2,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw314 = xmsg.add_rawmsg();
    pxraw314->CopyFrom(xraw);
    }



    xraw.set_id(0x14A);
    xraw.set_data(ADS_ONEBOX_3,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw14A = xmsg.add_rawmsg();
    pxraw14A->CopyFrom(xraw);

    xraw.set_id(0x167);
    xraw.set_data(ADS_VCU_1,24);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(24);
    iv::can::canraw * pxraw167 = xmsg.add_rawmsg();
    pxraw167->CopyFrom(xraw);


    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    gnIndex++;
    xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
    int ndatasize = xmsg.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser;
    pstrser.reset(strser);
    if(xmsg.SerializeToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }

    nrd++;
    if(nrd == 5)nrd = 0;
}

void initial()
{
    for (uint8_t i = 0; i < 24; i++) //CAN  to  canfd
    {
        //byte_36E[i] = 0;
    }
}


void sendthread()
{
    initial();
    iv::brain::decition xdecition;

    UnAcitvate();
    //   UnAcitvate();

    int nstate = 0; //0 Un 1 Activate
    //    Activate();
    while(gbSendRun)
    {
        if(gnDecitionNum <= 0)
        {
            if(nstate == 1)
            {
                UnAcitvate();
                nstate = 0;
            }
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            if(nstate == 0)
            {
                Activate();
                nstate = 1;
            }
            gMutex.lock();
            xdecition.CopyFrom(gdecition);
            gMutex.unlock();
            gnDecitionNum--;
        }

#ifdef TORQUEBRAKETEST
        if(gnothavetb < 10)
        {
            iv::controller::torquebrake xtb;
            gMutextb.lock();
            xtb.CopyFrom(gtb);
            gMutextb.unlock();
            if(xtb.enable())
            {

                xdecition.set_torque(xtb.torque());
                xdecition.set_brake(xtb.brake());

                std::cout<<" use xtb. torque: "<<xtb.torque()<<" brake: "<<xtb.brake()<<std::endl;

                //                gcontroller->control_torque(xtb.torque());
                //                gcontroller->control_brake(xtb.brake());
                //            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
            }
            else
            {
                //            qDebug("torquebrake not enable.");
            }
            gnothavetb++;
        }

#endif
        executeDecition(xdecition);
        if(gbChassisEPS == false) ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    UnAcitvate();
}

#ifdef Q_OS_LINUX
void sig_int(int signo)
{
    (void)signo;
    gbSendRun = false;
    gpsendthread->join();

    iv::modulecomm::Unregister(gpacansend);
    iv::modulecomm::Unregister(gpachassis);
    iv::modulecomm::Unregister(gpadecition);

    std::cout<<" controller exit."<<std::endl;
    exit(0);
}
#endif


// void processStream(std::istream& stream) {

//     std::string line;

//     while (std::getline(stream, line)) {

//         std::cout << "Read line: " << line << std::endl;

//     }

// }



void LoadXML(std::string strxmlpath){
    iv::xmlparam::Xmlparam xp(strxmlpath);
    gbUseOutDBC = xp.GetParam("UseOutDBC",false);
    gstrdbcpath = xp.GetParam("dbcpath","./ADCC_CH.dbc");
}

void Listencanrecv0()
{
    void * pblambda = iv::modulecomm::RegisterRecv("canrecv0",[](const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
        (void)strdata;
        (void)nSize;
        (void)index;
        (void)dt;
        (void)strmemname;
        iv::can::canmsg xmsg;
        if(false == xmsg.ParseFromArray(strdata,nSize))
        {
            std::cout<<"controller Listencan0 fail."<<std::endl;
            return;
        }

        int i;
        for(i=0;i<xmsg.rawmsg_size();i++)
        {
            iv::can::canraw * praw = xmsg.mutable_rawmsg(i);
            if(praw->id() == 0x14f)
            {
                unsigned char byte[24];
                if(praw->len() == 24)
                {
                    memcpy(byte,praw->data().data(),24);
                    unsigned int value;
                    value = byte[2]&0x01;
                    if(value == 0)
                    {
                         gbSendBrake = true;//std::cout<<" brake available."<<std::endl;
                    }
                    else
                    {
                        gbSendBrake = false;//std::cout<<" warning: no brake unavailable."<<std::endl;
                    }

                }
            }

            if(praw->id() == 0x21)
            {
                unsigned char xdata[8];
                if(praw->len() == 8)
                {
                    memcpy(xdata,praw->data().data(),8);
                    unsigned int value;
                    value = xdata[2]&0xff;
                    value = value<<8;
                    value =value + (xdata[3]&0xff);
                    double facc = value;
                    facc = facc * 0.001 - 2.0;
                    gfVehAcc = facc*9.8;
                    gnLastAccUptime = std::chrono::system_clock::now().time_since_epoch().count();
  //                  std::cout<<" acc : "<<gfVehAcc<<std::endl;
                }
            }
        }
    });

    (void)pblambda;


}


#include <QFile>
#include <QTextStream>
#include <strings.h>
#include <streambuf>
#include <sstream>

#include <iostream>
#include <istream>
#include <fstream>
int main(int argc, char *argv[])
{

 //    std::istringstream iss("First line.\nSecond line.\n");



    // // 将 iss 传递给 processStream，这是合法的

    // processStream(iss);

    RegisterIVBackTrace();
    showversion("controller_changan_shenlan");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

    if(argc < 2)
        strpath = strpath + "/controller_chery_sterra_es_fcm.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    LoadXML(strpath.toStdString());

    if(gbUseOutDBC == false)
    {
        QFile xFile;
        xFile.setFileName(":/FCM.dbc");
        if(xFile.open(QIODevice::ReadOnly))
        {
            std::cout<<" open qrc dbc file success. "<<std::endl;
            QTextStream in(&xFile);

            QString content = in.readAll(); // 读取文件全部内容到QString

            xFile.close();



            // 将QString转换为std::string，然后传递给std::istringstream

            std::istringstream iss(content.toStdString());
            gpsterraes = new sterraes(std::string(":/ADCC_CH.dbc"),iss);

            dbcsigpacker * ppac = new dbcsigpacker(std::string(":/ADCC_CH.dbc"),iss);
            ppac->SetMsgSignal("ADS_EPS_1","hi",1);
        }
        else
            std::cout<<" open qrc dbc file fail. "<<std::endl;

    }
    else
    {
        gpsterraes = new sterraes(gstrdbcpath);
    }


    //    gdecition_def.set_accelerator(-0.5);
    gdecition_def.set_brake(0);
    gdecition_def.set_rightlamp(false);
    gdecition_def.set_leftlamp(false);
    gdecition_def.set_wheelangle(0);

    gdecition_def.set_angle_mode(0);
    gdecition_def.set_angle_active(0);
    gdecition_def.set_acc_active(0);
    //    gdecition_def.set_brake_active(1);
    gdecition_def.set_brake_type(0);
    gdecition_def.set_auto_mode(0);


    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    gstrmemcansend = xp.GetParam("cansend","cansend0");
    gstrmemdecition = xp.GetParam("dection","deciton");
    gstrmemchassis = xp.GetParam("chassismsgname","chassis");

    kp = xp.GetParam("kp",0.5);
    ki = xp.GetParam("ki",0.3);
    kd = xp.GetParam("kd",0.01);
    gbPrintPIDOut = xp.GetParam("PringtPIDOut",false);
    TorqueRatio = xp.GetParam("TorqueRatio",1.4);


    gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
    gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);

#ifdef TORQUEBRAKETEST
    EnableTorqueBrakeTest();
#endif

//    testes();
//    return 0;

    Listencanrecv0();

    std::thread xthread(sendthread);

    gpsendthread = &xthread;

#ifdef Q_OS_LINUX
    signal(SIGINT, sig_int);
    signal(SIGTERM,sig_int);
#endif

    return a.exec();
}