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

void * gpacansend;
void * gpadecition;

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

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

QMutex gMutexDecision;


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<<"acc is "<<decition.accelerator()<<" ang is "<<decition.wheelangle()<<std::endl;
//    std::cout<<"brake is "<<decition.brake()<<std::endl;
    gcontroller->inialize();
    gcontroller->control_wheel(decition.wheelangle());
    gcontroller->control_accelerate(decition.accelerator());
 //   gcontroller->control_braking(decition.brake());    //vv7不用brake

    gcontroller->control_accmode(true);
    gcontroller->control_turnmode(true);
    gcontroller->control_drivemode(1);

#if 0
    optional bool air_ac = 37;
    optional bool air_circle = 38;
    optional bool air_auto = 39;
    optional bool air_off = 40;
    optional uint32 window_fl = 41;
    optional uint32 window_fr = 42;
    optional uint32 window_rl = 43;
    optional uint32 window_rr = 44;
#endif
    if(decition.has_air_ac())
        gcontroller->control_air_AC_ctrl(decition.air_ac());
    if(decition.has_air_auto())
        gcontroller->control_air_auto_ctrl(decition.air_auto());
    if(decition.has_air_circle())
        gcontroller->control_air_inloop_ctrl(decition.air_circle());
    if(decition.has_air_off())
        gcontroller->control_air_close(decition.air_off());

    if(decition.has_window_fl())
        gcontroller->control_air_lf_win(decition.window_fl());
    if(decition.has_window_fr())
        gcontroller->control_air_rf_win(decition.window_fr());
    if(decition.has_window_rl())
        gcontroller->control_air_lr_win(decition.window_rl());
    if(decition.has_window_rr())
        gcontroller->control_air_rr_win(decition.window_rr());

    bool leftlampSig = false;
    bool rightlampSig = false;
    if(decition.has_leftlamp())
        leftlampSig = decition.leftlamp();
    if(decition.has_rightlamp())
        rightlampSig = decition.rightlamp();
    gcontroller->control_turnsignals(leftlampSig,rightlampSig);

//    if(decition.has_engine())
//        gcontroller->control_engine(decition.engine());
    if(decition.has_speak())
        gcontroller->control_speaker(decition.speak());

}


void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    iv::brain::decition xdecition;

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

    gMutexDecision.lock();
    gdecition.CopyFrom(xdecition);
    gMutexDecision.unlock();
    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
        {
            gMutexDecision.lock();
            xdecition.CopyFrom(gdecition);
            gMutexDecision.unlock();
            gnDecitionNum--;
        }
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_tju_vv7");
    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_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();
}