//#include <QCoreApplication>

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

#include "cyber/cyber.h"
#include "cyber/time/rate.h"
#include "cyber/time/time.h"
#include "adchassis.h"
#include <canmsg.pb.h>



static Adchassis * gpChassis;

static chassisfun gchassisfun;


static std::shared_ptr<apollo::cyber::Writer<iv::chassis>>
      chassis_writer_;
static std::shared_ptr<apollo::cyber::Reader<iv::can::canmsg>>
      can0recv_reader_;



void RecvCANMsg(const std::shared_ptr<iv::can::canmsg> &xmsg)
{
    gpChassis->ProcCANMsg(*xmsg);
}

void ChassisCallBack(iv::chassis * pchassis)
{
    iv::chassis* raw_ptr = new iv::chassis();
    raw_ptr->CopyFrom(*pchassis);
    std::shared_ptr<iv::chassis> chassis_ptr(raw_ptr);

    chassis_writer_->Write(chassis_ptr);
}

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

    apollo::cyber::Init("apollochassis");
    std::unique_ptr<apollo::cyber::Node> pilot_node  = apollo::cyber::CreateNode("apollochassis");

    gchassisfun = std::bind(&ChassisCallBack,std::placeholders::_1);

    chassis_writer_ = pilot_node->CreateWriter<iv::chassis>("/adc/chassis");

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



    std::string strvehtype = std::string("SHENLAN_CANFD");
    gpChassis = new Adchassis(&gchassisfun,strvehtype);

    apollo::cyber::WaitForShutdown();

    delete gpChassis;

    return 0;
}