#include <QCoreApplication>

#include <QTime>

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

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

#include "canmsg.pb.h"
#include "decition.pb.h"
#include <thread>

void * gpacansend;
void * gpadecition;

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

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;	//实际车辆控制器




void executeDecition(const iv::brain::decition decition)
{
    std::cout<<"***************************"<<std::endl;
    std::cout<<"acc is "<<decition.accelerator()<<std::endl;
    std::cout<<"ang is "<<decition.wheelangle()<<std::endl;
    std::cout<<"brake is "<<decition.brake()<<std::endl;
    std::cout<<"limitspeed is "<<decition.speed()<<std::endl;
    std::cout<<"dangwei is "<<decition.gear()<<std::endl;
    std::cout<<std::endl;
    gcontroller->inialize();
    gcontroller->control_torque(decition.accelerator());
    gcontroller->control_wheel(decition.wheelangle());
    gcontroller->control_braking(decition.brake());
    gcontroller->control_speed_limit(decition.speed());

    gcontroller->control_dangWei(decition.gear());
    //gcontroller->control_zhuche(int P_zhuche);       //++

    gcontroller->control_drivemode(true);
    gcontroller->control_brakemode(true);
    gcontroller->control_torquemode(true);
    gcontroller->control_dangweimode(true);  //++
    gcontroller->control_turnmode(true);

    gcontroller->control_turnLight(decition.leftlamp(), decition.rightlamp());  //++
    gcontroller->control_nearLight(0);  //++
    gcontroller->control_beep(decition.speak());            //++
    gcontroller->control_brakeLight(decition.brakelamp());//++
}


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;

    iv::brain::decition xdecition;

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

    gdecition.CopyFrom(xdecition);
    gnDecitionNum = gnDecitionNumMax;



}

void ExecSend()
{
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
    xraw.set_id(ServiceControlStatus.command_ID);
    xraw.set_data(ServiceControlStatus.command.byte,8);
//    qDebug("%02x %02x",ServiceControlStatus.command.byte[2],ServiceControlStatus.command.byte[3]);
    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);
    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.SerializePartialToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }
}


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

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

     QString strpath = QCoreApplication::applicationDirPath();

    if(argc < 2)
        strpath = strpath + "/controller_jinlong_peisong.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_torque(0);
    gdecition_def.set_rightlamp(true);
    gdecition_def.set_doublespark(true);
    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");

    const char * strx = gstrmemcansend.data();

    gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,3);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);

    std::thread xthread(sendthread);

    return a.exec();
}