#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 <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;
//void executeDecition(const iv::decition::Decition decition)
//{

//    std::cout<<"acc is"<<decition->accelerator<<" ang is "<<decition->wheel_angle<<std::endl;

//    controller_->control_wheel(decition->wheel_angle);
//	controller_->control_accelerate(decition->accelerator);


//        if(!decition->leftlamp && !decition->rightlamp){
//             ServiceControlStatus.set_turnsignals_control(0,0);
//        }else if(decition->leftlamp){
//        ServiceControlStatus.set_turnsignals_control(decition->leftlamp,0);
//        }else if(decition->rightlamp){
//        ServiceControlStatus.set_turnsignals_control(0,decition->rightlamp);
//        }
//}
//	controller_->control_accelerate(decition->accelerator);

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

    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;

    gcontroller->control_acc_en(true);
    gcontroller->control_angle_enable(true);
    gcontroller->control_gear_en(true);
    gcontroller->control_speed_limit(30);

    gcontroller->control_wheel(decition.wheelangle());
    gcontroller->control_angle_speed(decition.wheelspeed());
    gcontroller->control_torque(decition.torque());

    gcontroller->control_brake(decition.brake());
    gcontroller->control_gear(decition.gear());

    gcontroller->control_handBrake(decition.handbrake());
    gcontroller->control_mode(decition.mode());
    //   gcontroller->control_mode(1);
    //  gcontroller->control_near_light(1);


    gcontroller->control_air_enable(decition.air_enable());
    gcontroller->control_air_on(decition.air_on());
    gcontroller->control_air_temp(decition.air_temp());
    gcontroller->control_air_mode(decition.air_mode());
    gcontroller->control_wind_level(decition.wind_level());
    gcontroller->control_roof_light(decition.roof_light());
    gcontroller->control_home_light(decition.home_light());
    gcontroller->control_air_worktime(decition.air_worktime());
    gcontroller->control_air_offtime(decition.air_offtime());

    if(decition.door())
        gcontroller->control_door(2);
    else
        gcontroller->control_door(3);






    gcontroller->cmd_checksum(0x10);
    gcontroller->cmd_checksum(0x11);
    gcontroller->cmd_checksum(0x12);

    //    qDebug("dangwei is %d mode is %d",decition.gear(),decition.mode());
}

void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *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)
{
    qint64 oldtime;
    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 %ld diff is %ld ",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(ServiceControlStatus.command_ID10);
    xraw.set_data(ServiceControlStatus.command10.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(ServiceControlStatus.command_ID11);
    xraw.set_data(ServiceControlStatus.command11.byte,8);
    int a = ServiceControlStatus.command11.byte[5]&0x0f;
    if(a != 0x04)
    {
        qDebug("not D.");
    }
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw1 = xmsg.add_rawmsg();
    pxraw1->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(ServiceControlStatus.command_ID12);
    xraw.set_data(ServiceControlStatus.command12.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw2 = xmsg.add_rawmsg();
    pxraw2->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()
{
    //    ServiceControlStatus.command.byte[0] = 0;
    //    ServiceControlStatus.command.byte[1] = 0;
    //    ServiceControlStatus.command.byte[2] = 0;
    //    ServiceControlStatus.command.byte[3] = 0;
    //    ServiceControlStatus.command.byte[4] = 0;
    //    ServiceControlStatus.command.byte[5] = 0;
    //    ServiceControlStatus.command.byte[6] = 0;
    //    ServiceControlStatus.command.byte[7] = 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_hapo");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

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

    gdecition_def.set_accelerator(-0.5);
    //    gdecition_def.set_brake(1.0);
    gdecition_def.set_brake(20);   //apollo_fu 20200417
    gdecition_def.set_torque(0);
    // gdecition_def.set_rightlamp(true);
    //  gdecition_def.set_doublespark(true);
    gdecition_def.set_wheelangle(0);
    gdecition_def.set_mode(1);
    gdecition_def.set_grade(1);
    gdecition_def.set_gear(1);
    gdecition_def.set_engine(0);
    //    gdecition_def.set_gear(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);

    std::thread xthread(sendthread);

    return a.exec();
}