//#include <QCoreApplication>

#include <thread>
#include <mutex>
#include <iostream>

#include "cyber/cyber.h"
#include "cyber/time/rate.h"
#include "cyber/time/time.h"
#include "shenlanfd.h"
#include <canmsg.pb.h>
#include "adchassis.pb.h"
#include "canmsg.pb.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"

static bool gbthreadrun = true;
static std::thread * gpthreadsend;

static Shenlanfd * gpShenlanfd;


static std::shared_ptr<apollo::cyber::Writer<iv::can::canmsg>>
      can0send_writer_;
static std::shared_ptr<apollo::cyber::Reader<iv::can::canmsg>>
      can0recv_reader_;
static std::shared_ptr<apollo::cyber::Reader<iv::chassis>>
        chassis_reader_;
static std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
        controller_reader_;

static void ExecSend()
{
    static int nCount = 0;
    static int gnIndex = 0;
    nCount++;
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
    unsigned char byte_1C4[32];
    gpShenlanfd->Get1C4(byte_1C4);

    xraw.set_id(0x1C4);
    xraw.set_data(byte_1C4,32);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(32);
    iv::can::canraw * pxraw1C4 = xmsg.add_rawmsg();
    pxraw1C4->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);

    unsigned char byte_24E[64];
    gpShenlanfd->Get1C4(byte_24E);
    xraw.set_id(0x24E);
    xraw.set_data(byte_24E,64);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(64);
    if(nCount%2 == 1)
    {
        iv::can::canraw * pxraw24E = xmsg.add_rawmsg();
        pxraw24E->CopyFrom(xraw);
    }
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    unsigned char byte_25E[32];
    gpShenlanfd->Get1C4(byte_25E);
    xraw.set_id(0x25E);
    xraw.set_data(byte_25E,32);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(32);
//    if(nCount == 10)
    if(nCount%2 == 1) //25Ede zhouqi shi 20ms
    {
        iv::can::canraw * pxraw25E = xmsg.add_rawmsg();
        pxraw25E->CopyFrom(xraw);
//        nCount = 0;
    }
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    std::shared_ptr<iv::can::canmsg> canmsg_ptr;
    iv::can::canmsg * pcanmsg = new iv::can::canmsg();
    pcanmsg->CopyFrom(xmsg);
    canmsg_ptr.reset(pcanmsg);
    can0send_writer_->Write(canmsg_ptr);
}

void threadsend()
{
    int i;


    while(gbthreadrun)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        ExecSend();
    }

    std::cout<<"  threadsendrecv exit."<<std::endl;
}

void RecvCANMsg(const std::shared_ptr<iv::can::canmsg> &xmsg)
{
    
}

void RecvChassis(const std::shared_ptr<iv::chassis> &xchassis)
{
    gpShenlanfd->SetSpeed(xchassis->vel());
}

void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd)
{
    double facc = 0;
    double ftorque = 0;
    double fbrake = xcmd->brake();
    double fwheelangle = 0;

    facc = xcmd->acceleration();

        double fVehWeight = 1800;
        double fg = 9.8;
        double fRollForce = 50;
        const double fRatio = 2.5;
        double fNeed = fRollForce + fVehWeight*facc;

        ftorque = fNeed/fRatio;

        if(fbrake>0.0001)
        {
            fbrake = fbrake * (-5.0) /100.0;
            ftorque = 0;
        }

        if(ftorque<0)ftorque = 0;



//    facc = xctrlcmd.linear_acceleration();
    fwheelangle = xcmd->steering_target() * 430.0/100.0;
    if(fwheelangle>430)fwheelangle = 430;
    if(fwheelangle<-430)fwheelangle = -430;

    iv::dcs xdcs;
    xdcs.mfBrake = fbrake;
    xdcs.mfTorque = ftorque;
    xdcs.mfWheel = fwheelangle;
    xdcs.mblampleft = false; xdcs.mblampright = false;
    if(xcmd->has_signal())
    {
        if(xcmd->signal().has_turn_signal())
        {
            if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_LEFT)
            {
                xdcs.mblampleft = true;
            }
            if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_RIGHT)
            {
                xdcs.mblampright = true;
            }
        }
    }

    gpShenlanfd->SetDecision(xdcs);   
}


int main(int argc, char *argv[])
{
//    QCoreApplication a(argc, argv);

    apollo::cyber::Init("apollocontroller_shenlanfd");
    std::unique_ptr<apollo::cyber::Node> pilot_node  = apollo::cyber::CreateNode("apollocontroller_shenlanfd");
    can0send_writer_ = pilot_node->CreateWriter<iv::can::canmsg>("/adc/cansend0");

    can0recv_reader_ = pilot_node->CreateReader<iv::can::canmsg>(
        "/adc/canrecv0",
      [](const std::shared_ptr<iv::can::canmsg> &xmsg) {
            RecvCANMsg(xmsg);
      });

    chassis_reader_ = pilot_node->CreateReader<iv::chassis>(
        "/adc/chassis",
        [](const std::shared_ptr<iv::chassis> &xchassis){
            RecvChassis(xchassis);
        }
    );

    controller_reader_ = pilot_node->CreateReader<apollo::control::ControlCommand>(
        "/aa",
        [](const std::shared_ptr<apollo::control::ControlCommand> &xcmd){
            RecvController(xcmd);
        }
    );



    gpShenlanfd = new Shenlanfd();
    gpthreadsend = new std::thread(threadsend);

    apollo::cyber::WaitForShutdown();

    gbthreadrun = false;
    gpthreadsend->join();
    delete gpShenlanfd;
    std::cout<<" Shut Down."<<std::endl;

    return 0;
}