#include "canrecv_consumer.h"

#include "math.h"

extern setupConfig_t setupConfig;
extern iv::msgunit shmSonar;

CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
{
    pBuffer = pBuf;

    QObject::connect(this,&CANRecv_Consumer::UltrasonicData_Ready,this,&CANRecv_Consumer::UltrasonicData_Ready_Slot);

    objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
    sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
    objDist_Send.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
    sensorStatus_Send.fill(false,NUM_OF_SENSOR_MAX);
}

CANRecv_Consumer::~CANRecv_Consumer()
{
    requestInterruption();
    while(this->isFinished() == false);
}

void CANRecv_Consumer::Clear_CAN_PrivateTempVariable(void)
{
    CAN_ID = 0x000u;
    CAN_DLC = 0;
    for(int i=0;i<8;i++) CAN_data[i] = 0x00;
}

uint32_t CANRecv_Consumer::Trans_From_CANRaw(const iv::can::canraw &xraw) //only for standard data frame, return can id
{
    this->Clear_CAN_PrivateTempVariable();
    CAN_ID = xraw.id();
    CAN_DLC = xraw.len();
    char tempData[8];
    int tempLen = (xraw.data().size() > 8) ? 8 : xraw.data().size();
    strncpy(tempData,xraw.data().data(),tempLen);
    for(int i=0;i<tempLen;i++)
    {
        CAN_data[i] = (uint8_t)tempData[i];
    }

    return CAN_ID;
}

void CANRecv_Consumer::run()
{
    iv::can::canraw tempCANRaw;
    uint8_t tempSensorID = 0;
    uint32_t tempLastResult = DIST_ERROR;
    uint16_t tempDist;
    decodeTimer.start();

    while (!QThread::isInterruptionRequested())
    {
        tempCANRaw.CopyFrom(pBuffer->Consume_Element(1));
        if(decodeEnableFlag == true)
        {
            if(decodeTimer.elapsed() >= 50)
            {
                decodeEnableFlag = false;
                objDist[decodeSensorID] = DIST_ERROR;
                sensorStatus[decodeSensorID] = false;
                tempSensorID = decodeSensorID + 1;

                if(tempSensorID >= NUM_OF_SENSOR_MAX)
                {
                    sonarSend_Lock.lock();
                    tempLastResult =  objDist_Send[0];
                    sonarSend_Lock.unlock();
                }
                else
                {
                    sonarSend_Lock.lock();
                    tempLastResult =  objDist_Send[tempSensorID];
                    sonarSend_Lock.unlock();
                }

                if(tempSensorID >= NUM_OF_SENSOR_MAX)
                {
                    //copy and send
                    sonarSend_Lock.lock();
                    objDist_Send.clear();
                    sensorStatus_Send.clear();
                    objDist_Send.swap(objDist);
                    sensorStatus_Send.swap(sensorStatus);
                    sonarSend_Lock.unlock();
                    emit UltrasonicData_Ready();
                    //clear
                    objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
                    sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
                    tempSensorID = 0;
                }
//                if(tempLastResult <= 2000)
//                    emit Enable_Ask(true,tempSensorID,2);
//                else if(tempLastResult <= 3800)
//                    emit Enable_Ask(true,tempSensorID,1);
//                else
//                    emit Enable_Ask(true,tempSensorID,0);
                decodeTimer.restart();
            }
            else
            {
                this->Trans_From_CANRaw(tempCANRaw);
                if(CAN_ID == setupConfig.sonarCAN_ID && CAN_DLC == 2)
                {
                    tempDist = (CAN_data[0] << 8) | CAN_data[1];
                    if(tempDist <= 0x1650 && tempDist >= 0xD0)
                    {
                        decodeEnableFlag = false;
                        objDist[decodeSensorID] = tempDist;
                        sensorStatus[decodeSensorID] = true;
                        tempSensorID = decodeSensorID + 1;

                        if(tempSensorID >= NUM_OF_SENSOR_MAX)
                        {
                            sonarSend_Lock.lock();
                            tempLastResult =  objDist_Send[0];
                            sonarSend_Lock.unlock();
                        }
                        else
                        {
                            sonarSend_Lock.lock();
                            tempLastResult =  objDist_Send[tempSensorID];
                            sonarSend_Lock.unlock();
                        }

                        if(tempSensorID >= NUM_OF_SENSOR_MAX)
                        {
                            //copy and send
                            sonarSend_Lock.lock();
                            objDist_Send.clear();
                            sensorStatus_Send.clear();
                            objDist_Send.swap(objDist);
                            sensorStatus_Send.swap(sensorStatus);
                            sonarSend_Lock.unlock();
                            emit UltrasonicData_Ready();
                            //clear
                            objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
                            sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
                            tempSensorID = 0;
                        }
//                        if(tempLastResult <= 2000)
//                            emit Enable_Ask(true,tempSensorID,2);
//                        else if(tempLastResult <= 3800)
//                            emit Enable_Ask(true,tempSensorID,1);
//                        else
//                            emit Enable_Ask(true,tempSensorID,0);
                        decodeTimer.restart();
                    }
                }
            }
        }
    }
}

void CANRecv_Consumer::Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID)
{
    decodeEnableFlag = enableFlag;
    decodeSensorID = sensorID;
    decodeTimer.restart();
}

void CANRecv_Consumer::UltrasonicData_Ready_Slot(void)
{
    iv::ultrasonic::ultrasonic xmsg;
    uint64_t tempLastTime = QDateTime::currentMSecsSinceEpoch();
    std::cout<<"interval time is "<<tempLastTime - gLastTime<<std::endl;
    gLastTime = tempLastTime;
    sonarSend_Lock.lock();
    xmsg.set_sigobjdist_flside(objDist_Send[0]);
    xmsg.set_sigobjdist_flcorner(objDist_Send[1]);
    xmsg.set_sigobjdist_flmiddle(objDist_Send[2]);
    xmsg.set_sigobjdist_frmiddle(objDist_Send[3]);
    xmsg.set_sigobjdist_frcorner(objDist_Send[4]);
    xmsg.set_sigobjdist_frside(objDist_Send[5]);
    xmsg.set_sigobjdist_rrside(objDist_Send[6]);
    xmsg.set_sigobjdist_rrcorner(objDist_Send[7]);
    xmsg.set_sigobjdist_rrmiddle(objDist_Send[8]);
    xmsg.set_sigobjdist_rlmiddle(objDist_Send[9]);
    xmsg.set_sigobjdist_rlcorner(objDist_Send[10]);
    xmsg.set_sigobjdist_rlside(objDist_Send[11]);

    xmsg.set_sigsensor_front_ls(sensorStatus_Send[0]);
    xmsg.set_sigsensor_front_l(sensorStatus_Send[1]);
    xmsg.set_sigsensor_front_lm(sensorStatus_Send[2]);
    xmsg.set_sigsensor_front_rm(sensorStatus_Send[3]);
    xmsg.set_sigsensor_front_r(sensorStatus_Send[4]);
    xmsg.set_sigsensor_front_rs(sensorStatus_Send[5]);
    xmsg.set_sigsensor_rear_rs(sensorStatus_Send[6]);
    xmsg.set_sigsensor_rear_r(sensorStatus_Send[7]);
    xmsg.set_sigsensor_rear_rm(sensorStatus_Send[8]);
    xmsg.set_sigsensor_rear_lm(sensorStatus_Send[9]);
    xmsg.set_sigsensor_rear_l(sensorStatus_Send[10]);
    xmsg.set_sigsensor_rear_ls(sensorStatus_Send[11]);
    sonarSend_Lock.unlock();

    xmsg.set_timestamp(QDateTime::currentMSecsSinceEpoch());
    int ndatasize = xmsg.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser;
    pstrser.reset(strser);
    if(xmsg.SerializePartialToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(shmSonar.mpa,strser,ndatasize);
    }
    else
    {
        std::cout<<"ultrasonic data serialize error."<<std::endl;
    }
}