#include <QCoreApplication>

#include <QTime>

#include <QMutex>

#include "control/control_status.h"
#include "control/controller.h"

#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>

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

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

bool gbChassisEPS = false;

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

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

boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器

static QMutex gMutex;


// signal: @ACC_LatAngReq
#define ECU_144_ACC_LatAngReq_CovFactor (0.1)
// conversion value to CAN signal
#define ECU_144_ACC_LatAngReq_toS(x) ((int16_t)((x) / 0.1 + 7200))

// signal: @ACC_MotorTorqueMaxLimitRequest
#define ECU_144_ACC_MotorTorqueMaxLimitRequest_CovFactor (0.02)
// conversion value to CAN signal
#define ECU_144_ACC_MotorTorqueMaxLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))

// signal: @ACC_MotorTorqueMinLimitRequest
#define ECU_144_ACC_MotorTorqueMinLimitRequest_CovFactor (0.02)
// conversion value to CAN signal
#define ECU_144_ACC_MotorTorqueMinLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))

typedef struct
{
    int16_t ACC_LatAngReq;
    uint8_t ADS_Reqmode;
    int16_t ACC_MotorTorqueMaxLimitRequest;
    uint8_t ACC_LatAngReqActive;
    int16_t ACC_MotorTorqueMinLimitRequest;
    uint8_t ACC_ADCReqType;
} ECU_144_t;

// signal: @ACC_AccTrqReq
#define ECU_24B_ACC_AccTrqReq_CovFactor (1)
// conversion value to CAN signal
#define ECU_24B_ACC_AccTrqReq_toS(x) ((int16_t)((x) + 16384))

// signal: @ACC_ACCTargetAcceleration
#define ECU_24B_ACC_ACCTargetAcceleration_CovFactor (0.05)
// conversion value to CAN signal
#define ECU_24B_ACC_ACCTargetAcceleration_toS(x) ((int16_t)((x) / 0.05 + 200))

// signal: @ACC_AEBTargetDeceleration
#define ECU_24B_ACC_AEBTargetDeceleration_CovFactor (0.0005)
// conversion value to CAN signal
#define ECU_24B_ACC_AEBTargetDeceleration_toS(x) ((int32_t)((x) / 0.0005 + 32000))

typedef struct
{
    int16_t ACC_AccTrqReq;
    uint8_t ACC_AccTrqReqActive;
    int16_t ACC_ACCTargetAcceleration;
    int32_t ACC_AEBTargetDeceleration;
    uint8_t ACC_AEBVehilceHoldReq;
    uint8_t ADCReqMode;
    uint8_t ACC_AEBActive;
    uint8_t ACC_Driveoff_Request;
    uint8_t ACC_DecToStop;
    uint8_t ACC_CDDActive;
    uint8_t ACC_ACCMode;
} ECU_24B_t;

typedef struct
{
    uint8_t ADS_UDLCTurnLightReq;
} ECU_36E_t;

unsigned char byte_144[8];
unsigned char byte_24B[8];
unsigned char byte_36E[8];

ECU_144_t* _m144;
ECU_24B_t* _m24B;
ECU_36E_t* _m36E;


void executeDecition(const iv::brain::decition decition)
{
    //    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
    _m144->ACC_LatAngReq = ECU_144_ACC_LatAngReq_toS(decition.wheelangle());
    _m144->ADS_Reqmode = 2;
    _m144->ACC_MotorTorqueMaxLimitRequest = ECU_144_ACC_MotorTorqueMaxLimitRequest_toS(10);
    _m144->ACC_LatAngReqActive = 1;
    _m144->ACC_MotorTorqueMinLimitRequest = ECU_144_ACC_MotorTorqueMinLimitRequest_toS(-10);
    _m144->ACC_ADCReqType = decition.brake_type();//ADC请求类型（制动时为1，其余情况为0）

    byte_144[0] |= ((_m144->ACC_LatAngReq >> 9) & (0x1FU)) | ((_m144->ADS_Reqmode & (0x07U)) << 5);
    byte_144[1] |= ((_m144->ACC_LatAngReq >> 1) & (0xFFU));
    byte_144[2] |= ((_m144->ACC_LatAngReq & (0x01U)) << 7) | ((_m144->ACC_MotorTorqueMaxLimitRequest >> 5) & (0x3FU)) | ((_m144->ACC_LatAngReqActive & (0x01U)) << 6);
    byte_144[3] |= ((_m144->ACC_MotorTorqueMaxLimitRequest & (0x1FU)) << 3) | ((_m144->ACC_MotorTorqueMinLimitRequest >> 8) & (0x07U));
    byte_144[4] |= (_m144->ACC_MotorTorqueMinLimitRequest & (0xFFU));
    byte_144[5] |= ((_m144->ACC_ADCReqType & (0x03U)) << 6);

    /*制动过程用的减速度，加速用扭矩*/
    _m24B->ACC_AccTrqReq = decition.niuju_y();
    _m24B->ACC_AccTrqReqActive = decition.acc_active();
    _m24B->ACC_ACCTargetAcceleration = decition.accelerator();
//    _m24B->ACC_AEBTargetDeceleration = 0;
//    _m24B->ACC_AEBVehilceHoldReq = 0;
//    _m24B->ADCReqMode = 0;
//    _m24B->ACC_AEBActive = 0;
//    _m24B->ACC_Driveoff_Request = 0;
//    _m24B->ACC_DecToStop = 0;
    _m24B->ACC_CDDActive = decition.brake_active();
    _m24B->ACC_ACCMode = decition.auto_mode();

    byte_24B[0] |= ((_m24B->ACC_AccTrqReq >> 7) & (0xFFU));
    byte_24B[1] |= ((_m24B->ACC_AccTrqReq & (0x7FU)) << 1) | (_m24B->ACC_AccTrqReqActive & (0x01U));
    byte_24B[2] |= ((_m24B->ACC_ACCTargetAcceleration >> 3) & (0x1FU));
    byte_24B[3] |= ((_m24B->ACC_ACCTargetAcceleration & (0x07U)) << 5) | ((_m24B->ACC_AEBTargetDeceleration >> 15) & (0x01U));
    byte_24B[4] |= ((_m24B->ACC_AEBTargetDeceleration >> 7) & (0xFFU));
    byte_24B[5] |= ((_m24B->ACC_AEBTargetDeceleration & (0x7FU)) << 1) | (_m24B->ACC_AEBVehilceHoldReq & (0x01U));
    byte_24B[6] |= ((_m24B->ADCReqMode & (0x03U)) << 1) | ((_m24B->ACC_AEBActive & (0x01U)) << 3) | ((_m24B->ACC_Driveoff_Request & (0x01U)) << 5) | ((_m24B->ACC_DecToStop & (0x01U)) << 6) | ((_m24B->ACC_CDDActive & (0x01U)) << 7);
    byte_24B[7] |= (_m24B->ACC_ACCMode & (0x07U));

    if(decition.leftlamp() == true && decition.rightlamp() == false)
        _m36E->ADS_UDLCTurnLightReq = 3;
    else if(decition.leftlamp() == false && decition.rightlamp() == true)
        _m36E->ADS_UDLCTurnLightReq = 4;
    else
        _m36E->ADS_UDLCTurnLightReq = 0;

    byte_36E[0] |= ((_m36E->ADS_UDLCTurnLightReq & (0x07U)) << 5);

}

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.epsmode() == 0)
    {
        gbChassisEPS = 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");
//    }
    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 ExecSend()
{
    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(0x144);
    xraw.set_data(byte_144,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw144 = xmsg.add_rawmsg();
    pxraw144->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(0x24B);
    xraw.set_data(byte_144,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw24B = xmsg.add_rawmsg();
    pxraw24B->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(0x36E);
    xraw.set_data(byte_144,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw36E = xmsg.add_rawmsg();
    pxraw36E->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;
    }
}

void initial()
{
    for (uint8_t i = 0; i < 8; i++)
    {
        byte_144[i] = 0;
        byte_24B[i] = 0;
        byte_36E[i] = 0;
    }

}

void sendthread()
{
    initial();
    iv::brain::decition xdecition;
    while(gbSendRun)
    {
        if(gnDecitionNum <= 0)
        {
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            gMutex.lock();
            xdecition.CopyFrom(gdecition);
            gMutex.unlock();
            gnDecitionNum--;
        }
        executeDecition(xdecition);
        if(gbChassisEPS == false) ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_changan_shenlan");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

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

    gdecition_def.set_accelerator(-0.5);
    gdecition_def.set_rightlamp(true);
    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(0);
    gdecition_def.set_brake_type(0);
    gdecition_def.set_auto_mode(0);

    gTime.start();

    gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());

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

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

    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

    std::thread xthread(sendthread);

    return a.exec();
}