#include "ivdetection_radar.h"

#include <iostream>

namespace iv {

ivdetection_radar::ivdetection_radar()
{
}

void ivdetection_radar::setcanmsgname(const std::string str)
{
    mstrcanmsgname = str;
}

void ivdetection_radar::setradarmsgname(const std::string str)
{
    mstrradarmsgname = str;
}

void ivdetection_radar::modulerun()
{
    iv::can::canmsg xmsg;
    ModuleFun funcan = std::bind(&iv::ivdetection_radar::UpdateCANMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpmsgcan = iv::modulecomm::RegisterRecvPlus(mstrcanmsgname.data(),funcan);

    mpmsgradar = iv::modulecomm::RegisterSend(mstrradarmsgname.data(),100000,3);

    int nnothavedata = 0;
    iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
    while(mbrun)
    {
        mWaitMutex.lock();
        mwc.wait(&mWaitMutex,100);
        mWaitMutex.unlock();
        if(mbUpdateCANMsg)
        {
            mMutex.lock();
            xmsg.CopyFrom(mCANMsg);
            mbUpdateCANMsg = false;
            mMutex.unlock();
            nnothavedata = 0;
            int nRtn;
            int i;
            for(i=0;i<xmsg.rawmsg_size();i++)
            {
                nRtn = DecodeCANMsg(*pradar,xmsg.mutable_rawmsg(i));
                if(nRtn == 1)
                {
                    ShareRadarMsg(mpmsgradar,pradar);
                    delete pradar;
                    pradar = new iv::radar::radarobjectarray;
                }
            }
            //Decode
        }
        else
        {
            nnothavedata++;
        }

        if(nnothavedata == 100)
        {

        }
        if(nnothavedata == 1000)
        {

        }


    }


    iv::modulecomm::Unregister(mpmsgradar);
    iv::modulecomm::Unregister(mpmsgcan);

}

void ivdetection_radar::UpdateCANMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void )&index;
    (void )&dt;
    (void )strmemname;

    if(nSize<1)return;
    iv::can::canmsg xmsg;
    if(false == xmsg.ParseFromArray(strdata,nSize))
    {
        std::cout<<"esr Listencan0 fail."<<std::endl;
        return;
    }

    mMutex.lock();
    mCANMsg.CopyFrom(xmsg);
    mbUpdateCANMsg = true;
    mMutex.unlock();

    mwc.wakeAll();
}

void ivdetection_radar::ShareRadarMsg(void *pa, radar::radarobjectarray *pradar)
{
    static qint64 oldrecvtime = 0;
    char * str = new char[pradar->ByteSize()];
    int nsize = pradar->ByteSize();
    if(pradar->SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(pa,str,nsize);
    }
    else
    {
        std::cout<<"ivdetection_radar::ShareRadarMsg SerializeToArray error."<<std::endl;
    }


    if(((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)&&(oldrecvtime != 0))
    {
//        givlog->warn("radar interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
    }

    oldrecvtime = QDateTime::currentMSecsSinceEpoch();

//    givlog->verbose("share time is %d ",gTime.elapsed());
//    qDebug("share time is %d ",gTime.elapsed());

    delete str;
}

}