#include <QCoreApplication>

#include <QTime>
#include <QMutex>

#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 "remotectrl.pb.h"
#include "platform_feedback.pb.h"

QMutex gMutex;

void * gpacansend;
void * gpadecition;
void * gparemote;
void * gpaPaltformFeedback;

std::string gstrmemdecition;
std::string gstrmemcansend;
std::string gstrmemremote; //Remote Ctrl
std::string gstrmemPaltformFeedback;

bool gbSendRun = true;

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

bool gbAllowRemote = false;   //Default, Not Allow Remote

bool gbAutoDriving = true; //if true, Auto Driving, false, remote controll

bool gbChassisEPS = true;

iv::brain::decition gdecition_remote;
iv::platformFeedback gPlatformFeedback;

qint64 gLastRemoteTime = 0;

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.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
    std::cout<<"limitspeed is "<<decition.speed()<<" brake is "<<decition.brake()<<std::endl;
    std::cout<<"drive_mode is "<<decition.mode()<<" elec_brake is "<<decition.handbrake()<<std::endl;
    std::cout<<"brake_light is "<<decition.brakelamp()<<" dangwei is "<<decition.gear()<<std::endl;
    gcontroller->inialize();
    gcontroller->control_torque(decition.torque());
    gcontroller->control_wheel(decition.wheelangle());
    gcontroller->control_brake(decition.brake());
    gcontroller->control_limit_speed(decition.speed());
    gcontroller->control_drive_mode(decition.mode());
    gcontroller->control_elec_brake(decition.handbrake());
    gcontroller->control_brake_light(decition.brakelamp());
    gcontroller->control_dangwei(decition.gear()); //0P 1D 2R 3N

    switch (decition.gear()) {
    case 0:
        gPlatformFeedback.set_shift(3);  //0N 1D 2R 3P
        break;
    case 1:
        gPlatformFeedback.set_shift(1);  //0N 1D 2R 3P
        break;
    case 2:
        gPlatformFeedback.set_shift(2);  //0N 1D 2R 3P
        break;
    case 3:
        gPlatformFeedback.set_shift(0);  //0N 1D 2R 3P
        break;
    default:
        break;
    }
    gPlatformFeedback.set_throttle(decition.torque());
    gPlatformFeedback.set_brake(decition.brake());
    gPlatformFeedback.set_steeringwheelangle(decition.wheelangle());
}



void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    qint64 oldtime;
    iv::brain::decition xdecition;

    iv::remotectrl xrc;

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

    if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
    {
        gbAutoDriving = true;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_AUTO);
    }
    else if(xrc.ntype() == iv::remotectrl_CtrlType_STOP)
    {
        gbAutoDriving = false;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_STOP);
        xdecition.set_torque(0.0);
        xdecition.set_brake(100.0);
        xdecition.set_wheelangle(0.0);
        xdecition.set_speed(5);
        xdecition.set_gear(1); //0P 1D 2R 3N
        xdecition.set_handbrake(0);
        xdecition.set_grade(1);
        xdecition.set_mode(1);
        xdecition.set_speak(0);
        xdecition.set_headlight(false);
        xdecition.set_engine(0);
        xdecition.set_taillight(false);
        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();
    }
    else
    {
        gbAutoDriving = false;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_REMOTE);
        xdecition.set_torque(xrc.acc());
        xdecition.set_brake(xrc.brake());
        xdecition.set_wheelangle(xrc.wheel()*5.5f);
        xdecition.set_speed(5);
        if(xrc.shift() > 0) //D shift
            xdecition.set_gear(1); //0P 1D 2R 3N
        else if(xrc.shift() < 0)
            xdecition.set_gear(2);
        else
            xdecition.set_gear(3);
        xdecition.set_handbrake(0);
        xdecition.set_grade(1);
        xdecition.set_mode(1);
        xdecition.set_speak(0);
        xdecition.set_headlight(false);
        xdecition.set_engine(0);
        xdecition.set_taillight(false);
        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();

    }

}


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;
    }

    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.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);
    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;
    }

    int ndatasize_pf = gPlatformFeedback.ByteSize();
    char * str_pf = new char[ndatasize_pf];
    std::shared_ptr<char> pstr;pstr.reset(str_pf);
    if(!gPlatformFeedback.SerializeToArray(str_pf,ndatasize_pf))
    {
        std::cout<<"MainWindow::on_horizontalSlider_valueChanged serialize error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(gpaPaltformFeedback,str_pf,ndatasize_pf);
}


void sendthread()
{
    iv::brain::decition xdecition;
    while(gbSendRun)
    {
        if(gbAutoDriving)
        {
            if(gnDecitionNum <= 0)
            {
                xdecition.CopyFrom(gdecition_def);
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition);
                gMutex.unlock();
                gnDecitionNum--;
            }
        }
        else
        {
            if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 1000)
            {
                xdecition.CopyFrom(gdecition_def);
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_remote);
                gMutex.unlock();
            }
        }
        executeDecition(xdecition);
        ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

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

     QString strpath = QCoreApplication::applicationDirPath();

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

    gdecition_def.set_brake(0);
    gdecition_def.set_torque(0);
    gdecition_def.set_speed(105);

    gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_AUTO);
    gPlatformFeedback.set_shift(3);
    gPlatformFeedback.set_throttle(0.0);
    gPlatformFeedback.set_brake(0.0);
    gPlatformFeedback.set_steeringwheelangle(0.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");
    gstrmemremote =  xp.GetParam("remotectrl","remotectrl");
    gstrmemPaltformFeedback = xp.GetParam("paltformFeedback","platformFeedback");

    std::string strremote = xp.GetParam("allowremote","true");
    if(strremote == "true")
    {
        gbAllowRemote = true;
    }


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


    if(gbAllowRemote)
    {
        gpaPaltformFeedback = iv::modulecomm::RegisterSend(gstrmemPaltformFeedback.data(),10000,1);
        gparemote = iv::modulecomm::RegisterRecv(gstrmemremote.data(),ListenRemotectrl);
    }

    std::thread xthread(sendthread);

    return a.exec();
}