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

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

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

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

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

CANPacker * gpPacker;


std::vector<SignalPackValue> mvectorADSEPS1;
std::vector<SignalPackValue> mvectorADSEPS3;
std::vector<SignalPackValue> mvectorADSONEBOX1;
std::vector<SignalPackValue> mvectorADSVCU1;



void set_EPS1_signal(std::string strsigname,double value){
    gpsterraes->set_EPS1_signal(strsigname,value);
}

void set_EPS3_signal(std::string strsigname,double value){
    gpsterraes->set_EPS3_signal(strsigname,value);
}

void set_ONEBOX1_signal(std::string strsigname,double value){
    gpsterraes->set_ONEBOX1_signal(strsigname,value);
}

void set_VCU1_signal(std::string strsigname,double value){
    gpsterraes->set_VCU1_signal(strsigname,value);
}

void ExecSend();

void executeDecition(const iv::brain::decition &decition)
{



}

void Activate()
{
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    iv::brain::decition xdecition;
    xdecition.set_brake(0.0);
    xdecition.set_torque(0.0);
    //    for(int j=0;j<100000;j++)
    //    {
    std::cout<<" run "<<std::endl;
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0.0);
        xdecition.set_angle_mode(0);
        xdecition.set_angle_active(0);
        xdecition.set_acc_active(0);
        xdecition.set_brake_active(0);
        //        xdecition.set_brake_type(0);
        xdecition.set_auto_mode(0);
        gnState = 0;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0.0);
        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);
        gnState = 1;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    //    }
}

void UnAcitvate()
{
    iv::brain::decition xdecition;

    xdecition.set_brake(0.0);
    xdecition.set_torque(0.0);
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0);
        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);
        gnState = 1;
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    for(int i = 0; i < 3; i++){
        xdecition.set_wheelangle(0);
        xdecition.set_angle_mode(0);
        xdecition.set_angle_active(0);
        xdecition.set_acc_active(0);
        xdecition.set_brake_active(0);
        //        xdecition.set_brake_type(0);
        gnState = 0;
        xdecition.set_auto_mode(0);
        executeDecition(xdecition);
        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);

    //   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 ExecSend()
{
    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(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;
    }
}

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

void SendEPS3()
{
    static int rollcouter = 0;
    gpsterraes->GetEPS3Data(ADS_EPS_3);
    ExecSend();
    rollcouter++;
    if(rollcouter>14)rollcouter = 0;
}

void SendEPS1()
{
    static int rollcouter = 0;
//    std::cout<<" roll count:: "<<rollcouter<<std::endl;
    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
    set_EPS1_signal("ADS_1_Resd1",0.0);
    gpsterraes->GetEPS1Data(ADS_EPS_1);
    ExecSend();
    rollcouter++;
    if(rollcouter>14)rollcouter = 0;
}

void testes()
{
    int i = 0;
    int rollcouter = 0;
    double fwheelang = 90.0;

    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
    set_EPS1_signal("ADS_1_Resd1",0.0);
    set_EPS1_signal("ADS_1_SteerAgReq",fwheelang);
    set_EPS1_signal("ADS_1_SteerAgVld",1.0);
    set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
    set_EPS1_signal("ADS_1_RollgCntr2",rollcouter);
    set_EPS1_signal("ADS_1_Resd2",0.0);
    set_EPS1_signal("ADS_1_SteerTqEna",1.0);
    set_EPS1_signal("ADS_1_LdwWarningCmd",0.0);
    set_EPS1_signal("ADS_1_LdwWarningCmdVld",2.0);
    set_EPS1_signal("ADS_1_SteerMaxTqReq",10.0);
    set_EPS1_signal("ADS_1_SteerMinTqReq",1.0);
    set_EPS1_signal("ADS_1_ADSHealthSts",1.0);
    set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
    set_EPS1_signal("CutOutMAC_2CB_S",1.0);

    set_EPS3_signal("ADS_3_RollgCntr1",rollcouter);
    set_EPS3_signal("ADS_3_Resd1",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgReq",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgVld",0.0);
    set_EPS3_signal("ADS_3_SteerParkAgEna",0.0);
    set_EPS3_signal("ADS_3_RollgCntr2",rollcouter);
    set_EPS3_signal("ADS_3_Resd2",0.0);
    set_EPS3_signal("ADS_3_ParkFcnMode",0.0);
    set_EPS3_signal("ADS_3_ADSParkHealthSts",0.0);

    for(i=0;i<10;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // for(i=0;i<10;i++)
    // {
    //     set_EPS3_signal("ADS_3_SteerParkAgEna",1.0);
    //     set_EPS3_signal("ADS_3_SteerParkAgReq",0.0);
    //     set_EPS3_signal("ADS_3_SteerParkAgVld",0.0);
    //     SendEPS3();
    //     std::this_thread::sleep_for(std::chrono::milliseconds(10));
    // }

    for(i=0;i<3000;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",2.0);
        set_EPS1_signal("ADS_1_SteerAgReq",fwheelang);
        set_EPS1_signal("ADS_1_SteerAgVld",1.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    for(i=0;i<10;i++)
    {
        set_EPS1_signal("ADS_1_SteerPilotAgEna",0.0);
        SendEPS1();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

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)
{
    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

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;

    gpsterraes = new sterraes();

    //    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);

    //    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();

    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

    testes();
    return 0;

    std::thread xthread(sendthread);

    gpsendthread = &xthread;

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

    return a.exec();
}