#include <QCoreApplication>

#include <QDateTime>
#include <QMutex>

#include <iostream>

#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.
*/

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;


//------------------------------------

iv::radar::radarobjectarray gobj;
int gntemp = 0;

static int gnNotSend = 10;

static bool gbinit = false;

iv::radar::radarobjectarray mradararray;

void ShareResult()
{
    char * str = new char[mradararray.ByteSize()];
    int nsize = mradararray.ByteSize();
    if(mradararray.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,str,nsize);
    }

    givlog->verbose("obj size is %d ",mradararray.obj_size());

//    qDebug("share time is %d ",gTime.elapsed());
    delete str;
}

void ProcRadarMsg(iv::can::canraw xmsg)
{
    if(gbinit == false)gnNotSend = 10;

    if(gnNotSend > 0)
    {
        givlog->verbose("send valid data.");
        iv::can::canraw x;
        x.set_id(0x200);
        x.set_bext(false);
        x.set_bremote(false);
        x.set_len(8);
        unsigned char strdata[8];
        strdata[0] = 0x08;
        strdata[4] = 0x08;
        x.set_data(strdata,8);
        iv::can::canmsg xsend;
        iv::can::canraw * praw = xsend.add_rawmsg();
        praw->CopyFrom(x);
        int nsize = xsend.ByteSize();
        char * str = new char[nsize];
        if(xsend.SerializeToArray(str,nsize))
        {
            iv::modulecomm::ModuleSendMsg(shmRadar.mpa,str,nsize);
        }
        else
        {

        }
        delete str;
        gnNotSend--;
    }
    int i;
    if(xmsg.id() == 0x60A)
    {
        gbinit = true;
        mradararray.set_mstime(QDateTime::currentMSecsSinceEpoch());
        ShareResult();
        mradararray.clear_obj();
    }
    if(xmsg.id() == 0x60B)
    {
        if(xmsg.len()<8)
        {
            std::cout<<"ProcRadarMsg can message length is small "<<xmsg.len()<<std::endl;
            return;
        }
        char data[8];
        memcpy(data,xmsg.data().data(),8);
        unsigned char id;
        id = data[0];
        float y;
        float x;
        float vx,vy;

        unsigned char a,b;
        a = data[1];
        b = data[2];
        b = b&0xf8;
        y = a;
        y = y*256 + b;
        y = y/8;
        y = y*0.2 - 500;
        a = data[2];b = data[3];
        a = a&0x07;
        x = a;
        x = x*256 + b;
        x = x*0.2 -204.6;
        a = data[4];b = data[5];
        b = b&0xc0;b = b/64;
        vy = a; vy = vy*4 + b;
        vy = vy *0.25 - 128;
        a = data[5];b = data[6];
        a = a&0x3f;b = b&0xe0;b = b/32;
        vx = a; vx = vx*8 + b;
        vx = vx *0.25 - 64;

        iv::radar::radarobject xobj;
        xobj.set_bvalid(true);
        xobj.set_x(x*(-1));
        xobj.set_y(y);
        xobj.set_vx(vx*(-1));
        xobj.set_vy(vy);
        xobj.set_vel(vy);

        iv::radar::radarobject * pxobj = mradararray.add_obj();
        pxobj->CopyFrom(xobj);
    }

}

void DecodeRadar(iv::can::canmsg xmsgvetor)
{

    int i;

    for(i=0;i<xmsgvetor.rawmsg_size();i++)
    {
        ProcRadarMsg(xmsgvetor.rawmsg(i));
    }

}

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(1000));
}

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;

    decode_setup_cfg_from_xml(setupConfig,strpath.toStdString());

    if(setupConfig.radarCfgApply!=0)
    {
        RadarConfiguration_ch0_t radarConfiguration;
        decode_radar_cfg_from_yaml(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);

//    CANSend_Producer can_send_producer(&CANSend);
//    can_send_producer.start();
//    CANSend_Consumer can_send_consumer(&CANSend);
//    can_send_consumer.start();

    int rtn = a.exec();

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