#include <QCoreApplication>

#include <QDateTime>
#include <QMutex>

#include <iostream>
#include <thread>

#include "modulecomm.h"
#include "ivversion.h"
#include "ivbacktrace.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivexit.h"

#include "canmsg.pb.h"
#include "radarobjectarray.pb.h"
#include "chassis.pb.h"
#include "gpsimu.pb.h"

#include "decode_cfg.h"
#include "can_producer_consumer.h"
#include "canrecv_consumer.h"
#include "cansend_consumer.h"
#include "cansend_producer.h"

#include "iv_msgunit.h"

/*
 * Notice : the radar config setting function is not finished
 * Notice : the number of object is less than 64. so this version is used for object mode.
 * Notice : only can decode standard data CAN frame
*/

QCoreApplication * gApp;

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

setupConfig_t setupConfig;

iv::msgunit shmCANRecv;
iv::msgunit shmCANSend;
iv::msgunit shmRadar;
iv::msgunit shmGPSIMU;
iv::msgunit shmChassis;

CAN_Producer_Consumer CANRecv(4096);
CAN_Producer_Consumer CANSend(4096);

uint8_t chassisShift = 0; //1P 2R 3N 4D
double chassisVelocity = 0; //m/s
QMutex chassisDataLock;

double GPSIMUVelocity = 0; // m/s
double GPSIMUYawRate = 0; // deg/s
QMutex GPSIMUDataLock;

CANSend_Producer* can_send_producer;
CANSend_Consumer* can_send_consumer;
CANRecv_Consumer* can_recv_consumer;

void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)
    {
        std::cout<<"radar ListenCANMsg data empty."<<std::endl;
        return;
    }
    iv::can::canmsg xmsg;
    if(!xmsg.ParseFromArray(strdata,nSize))
    {
        givlog->error("radar Listencanmsg fail");
        gfault->SetFaultState(1, 0, "radar Listencanmsg error");
        std::cout<<"radar ListenCANMsg parse fail."<<std::endl;
        return;
    }

    CANRecv.Produce_Elements_From_CANMsg(xmsg);
}

void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)
    {
        std::cout<<"radar ListenGPSIMUMsg data empty."<<std::endl;
        return;
    }
    iv::gps::gpsimu xdata;
    if(!xdata.ParseFromArray(strdata,nSize))
    {
        std::cout<<" radar ListenGPSIMUMsg parse fail."<<std::endl;
        return;
    }
    GPSIMUDataLock.lock();
    GPSIMUVelocity = xdata.speed(); //m/s
    GPSIMUYawRate = xdata.gyro_z(); //deg/s
    GPSIMUDataLock.unlock();
}

void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)
    {
        std::cout<<"radar ListenChassisMsg data empty."<<std::endl;
        return;
    }
    iv::chassis xdata;
    if(!xdata.ParseFromArray(strdata,nSize))
    {
        std::cout<<" radar ListenChassisMsg parse fail."<<std::endl;
        return;
    }
    chassisDataLock.lock();
    chassisShift = xdata.shift(); //1P 2R 3N 4D
    chassisVelocity = xdata.vel()/3.6; //km/h to m/s
    chassisDataLock.unlock();
}

void ExitFunc()
{
    gApp->quit();
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
}

void signal_handler(int sig)
{
    if(sig == SIGINT)
    {
        ExitFunc();
    }
}

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

    gApp = &a;

    givlog = new iv::Ivlog("driver_radar_continental_ARS408_SRR308");
    gfault = new iv::Ivfault("driver_radar_continental_ARS408_SRR308");

//    iv::ivexit::RegIVExitCall(ExitFunc);
    signal(SIGINT,signal_handler);

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/driver_radar_continental_ARS408_SRR308.xml";
    else
        strpath = argv[1];

//    givlog->verbose("%s", strpath.data());
//    std::cout<<strpath.toStdString()<<std::endl;

    DecodeSetupCfgFromXML(setupConfig,strpath.toStdString());

    if(setupConfig.radarCfgApply!=0)
    {
        RadarConfiguration_ch0_t radarConfiguration;
        DecodeRadarCfgFromYAML(radarConfiguration,setupConfig.radarCfgYAML_Path);
    }

    strncpy(shmCANSend.mstrmsgname,setupConfig.strMemCANSend.data(),255);
    shmCANSend.mnBufferSize = 100000;
    shmCANSend.mnBufferCount = 3;
    shmCANSend.mpa = iv::modulecomm::RegisterSend(shmCANSend.mstrmsgname,shmCANSend.mnBufferSize,shmCANSend.mnBufferCount);

    strncpy(shmRadar.mstrmsgname,setupConfig.strMemRadar.data(),255);
    shmRadar.mnBufferSize = 100000;
    shmRadar.mnBufferCount = 1;
    shmRadar.mpa = iv::modulecomm::RegisterSend(shmRadar.mstrmsgname,shmRadar.mnBufferSize,shmRadar.mnBufferCount);

    strncpy(shmCANRecv.mstrmsgname,setupConfig.strMemCANRecv.data(),255);
    shmCANRecv.mnBufferSize = 100000;
    shmCANRecv.mnBufferCount = 3;
    shmCANRecv.mpa = iv::modulecomm::RegisterRecv(shmCANRecv.mstrmsgname,ListenCANMsg);

    strncpy(shmGPSIMU.mstrmsgname,setupConfig.strMemGPSIMU.data(),255);
    shmGPSIMU.mnBufferSize = 100000;
    shmGPSIMU.mnBufferCount = 3;
    shmGPSIMU.mpa = iv::modulecomm::RegisterRecv(shmGPSIMU.mstrmsgname,ListenGPSIMUMsg);

    strncpy(shmChassis.mstrmsgname,setupConfig.strMemChassis.data(),255);
    shmChassis.mnBufferSize = 100000;
    shmChassis.mnBufferCount = 3;
    shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisMsg);

    can_send_producer = new CANSend_Producer(&CANSend);
    can_send_producer->start();
    can_send_consumer = new CANSend_Consumer(&CANSend);
    can_send_consumer->start();
    can_recv_consumer = new CANRecv_Consumer(&CANRecv);
    can_recv_consumer->start();

    int rtn = a.exec();

    can_send_producer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
    can_send_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
    can_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.

    if(gfault != nullptr)delete gfault;
    if(givlog != nullptr)delete givlog;
    if(shmCANRecv.mpa != nullptr)iv::modulecomm::Unregister(shmCANRecv.mpa);
    if(shmCANSend.mpa != nullptr)iv::modulecomm::Unregister(shmCANSend.mpa);
    if(shmRadar.mpa != nullptr)iv::modulecomm::Unregister(shmRadar.mpa);
    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
    if(shmChassis.mpa != nullptr)iv::modulecomm::Unregister(shmChassis.mpa);

    return rtn;
}