#include "sdi_datarecv_consumer.h"

extern setupConfig_t setupConfig;
extern iv::msgunit shmSonar;
extern double gsoundVelocity;

SDI_DataRecv_Consumer::SDI_DataRecv_Consumer(Byte_Qvector_Producer_Consumer *pBuf)
{
    pBuffer = pBuf;


}

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

void SDI_DataRecv_Consumer::run()
{
    QVector<uint8_t> tempData;

    BYTES2UINT bytes2uint;
    bytes2uint.wordData = 0xFF00;

    while (!QThread::isInterruptionRequested())
    {
        tempData = pBuffer->Consume_Element();
        uint16_t tempPtr = tempData.size();

        bytes2uint.byteData[1] = tempData.at(tempPtr - 1);
        bytes2uint.byteData[0] = tempData.at(tempPtr - 2);
        SDI_payload_size = bytes2uint.wordData;

        SDI_sensor_CNT = tempData.at(8);
        bytes2uint.byteData[0] = tempData.at(4);
        bytes2uint.byteData[1] = tempData.at(5);
        SDI_TX_Mask = bytes2uint.wordData;

        if(SDI_sensor_CNT == 4 && SDI_payload_size >= 0x0061)
        {
            if(SDI_TX_Mask == 0x0041)
            {
                if(tempData.at(9)==0x00&&tempData.at(39)==0x01&&tempData.at(61)==0x06&&tempData.at(91)==0x07)
                {
                    Clear_Sensor_Data();

                    uint8_t tempEchosCNT = tempData.at(10); // sensor 0
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_0.append(bytes2uint.wordData);
                        sensorMagnitude_0.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(40); // sensor 1
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+41);
                        bytes2uint.byteData[1] = tempData.at(4*i+42);
                        sensorTOF_1.append(bytes2uint.wordData);
                        sensorMagnitude_1.append(tempData.at(4*i+43));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 6
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_6.append(bytes2uint.wordData);
                        sensorMagnitude_6.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(92); // sensor 7
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+93);
                        bytes2uint.byteData[1] = tempData.at(4*i+94);
                        sensorTOF_7.append(bytes2uint.wordData);
                        sensorMagnitude_7.append(tempData.at(4*i+95));
                    }
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else if(SDI_TX_Mask == 0x0820)
            {
                if(tempData.at(9)==0x04&&tempData.at(31)==0x05&&tempData.at(61)==0x0A&&tempData.at(83)==0x0B)
                {
                    uint8_t tempEchosCNT = tempData.at(10); // sensor 4
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_4.append(bytes2uint.wordData);
                        sensorMagnitude_4.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(32); // sensor 5
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+33);
                        bytes2uint.byteData[1] = tempData.at(4*i+34);
                        sensorTOF_5.append(bytes2uint.wordData);
                        sensorMagnitude_5.append(tempData.at(4*i+35));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 10
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_10.append(bytes2uint.wordData);
                        sensorMagnitude_10.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(84); // sensor 11
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+85);
                        bytes2uint.byteData[1] = tempData.at(4*i+86);
                        sensorTOF_11.append(bytes2uint.wordData);
                        sensorMagnitude_11.append(tempData.at(4*i+87));
                    }

                    this->ResultFilterAndPublish();
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else
            {
                std::cout<<"TX Mask error. Sensor may broken?"<<std::endl;
            }
        }
        else if(SDI_sensor_CNT == 6 && SDI_payload_size >= 0x008D)
        {
            if(SDI_TX_Mask == 0x0082)
            {
                if(tempData.at(9)==0x00&&tempData.at(31)==0x01&&tempData.at(61)==0x02&&tempData.at(83)==0x06&&tempData.at(105)==0x07&&tempData.at(135)==0x08)
                {
                    uint8_t tempEchosCNT = tempData.at(10); // sensor 0
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_0.append(bytes2uint.wordData);
                        sensorMagnitude_0.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(32); // sensor 1
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+33);
                        bytes2uint.byteData[1] = tempData.at(4*i+34);
                        sensorTOF_1.append(bytes2uint.wordData);
                        sensorMagnitude_1.append(tempData.at(4*i+35));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 2
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_2.append(bytes2uint.wordData);
                        sensorMagnitude_2.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(84); // sensor 6
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+85);
                        bytes2uint.byteData[1] = tempData.at(4*i+86);
                        sensorTOF_6.append(bytes2uint.wordData);
                        sensorMagnitude_6.append(tempData.at(4*i+87));
                    }

                    tempEchosCNT = tempData.at(106); // sensor 7
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+107);
                        bytes2uint.byteData[1] = tempData.at(4*i+108);
                        sensorTOF_7.append(bytes2uint.wordData);
                        sensorMagnitude_7.append(tempData.at(4*i+109));
                    }

                    tempEchosCNT = tempData.at(136); // sensor 8
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+137);
                        bytes2uint.byteData[1] = tempData.at(4*i+138);
                        sensorTOF_8.append(bytes2uint.wordData);
                        sensorMagnitude_8.append(tempData.at(4*i+139));
                    }
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else if(SDI_TX_Mask == 0x0104)
            {
                if(tempData.at(9)==0x01&&tempData.at(31)==0x02&&tempData.at(61)==0x03&&tempData.at(83)==0x07&&tempData.at(105)==0x08&&tempData.at(135)==0x09)
                {
                    uint8_t tempEchosCNT = tempData.at(10); // sensor 1
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_1.append(bytes2uint.wordData);
                        sensorMagnitude_1.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(32); // sensor 2
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+33);
                        bytes2uint.byteData[1] = tempData.at(4*i+34);
                        sensorTOF_2.append(bytes2uint.wordData);
                        sensorMagnitude_2.append(tempData.at(4*i+35));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 3
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_3.append(bytes2uint.wordData);
                        sensorMagnitude_3.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(84); // sensor 7
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+85);
                        bytes2uint.byteData[1] = tempData.at(4*i+86);
                        sensorTOF_7.append(bytes2uint.wordData);
                        sensorMagnitude_7.append(tempData.at(4*i+87));
                    }

                    tempEchosCNT = tempData.at(106); // sensor 8
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+107);
                        bytes2uint.byteData[1] = tempData.at(4*i+108);
                        sensorTOF_8.append(bytes2uint.wordData);
                        sensorMagnitude_8.append(tempData.at(4*i+109));
                    }

                    tempEchosCNT = tempData.at(136); // sensor 9
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+137);
                        bytes2uint.byteData[1] = tempData.at(4*i+138);
                        sensorTOF_9.append(bytes2uint.wordData);
                        sensorMagnitude_9.append(tempData.at(4*i+139));
                    }
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else if(SDI_TX_Mask == 0x0208)
            {
                if(tempData.at(9)==0x02&&tempData.at(31)==0x03&&tempData.at(61)==0x04&&tempData.at(83)==0x08&&tempData.at(105)==0x09&&tempData.at(135)==0x0A)
                {
                    uint8_t tempEchosCNT = tempData.at(10); // sensor 2
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_2.append(bytes2uint.wordData);
                        sensorMagnitude_2.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(32); // sensor 3
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+33);
                        bytes2uint.byteData[1] = tempData.at(4*i+34);
                        sensorTOF_3.append(bytes2uint.wordData);
                        sensorMagnitude_3.append(tempData.at(4*i+35));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 4
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_4.append(bytes2uint.wordData);
                        sensorMagnitude_4.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(84); // sensor 8
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+85);
                        bytes2uint.byteData[1] = tempData.at(4*i+86);
                        sensorTOF_8.append(bytes2uint.wordData);
                        sensorMagnitude_8.append(tempData.at(4*i+87));
                    }

                    tempEchosCNT = tempData.at(106); // sensor 9
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+107);
                        bytes2uint.byteData[1] = tempData.at(4*i+108);
                        sensorTOF_9.append(bytes2uint.wordData);
                        sensorMagnitude_9.append(tempData.at(4*i+109));
                    }

                    tempEchosCNT = tempData.at(136); // sensor 10
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+137);
                        bytes2uint.byteData[1] = tempData.at(4*i+138);
                        sensorTOF_10.append(bytes2uint.wordData);
                        sensorMagnitude_10.append(tempData.at(4*i+139));
                    }
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else if(SDI_TX_Mask == 0x0410)
            {
                if(tempData.at(9)==0x03&&tempData.at(31)==0x04&&tempData.at(61)==0x05&&tempData.at(83)==0x09&&tempData.at(105)==0x0A&&tempData.at(135)==0x0B)
                {
                    uint8_t tempEchosCNT = tempData.at(10); // sensor 3
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+11);
                        bytes2uint.byteData[1] = tempData.at(4*i+12);
                        sensorTOF_3.append(bytes2uint.wordData);
                        sensorMagnitude_3.append(tempData.at(4*i+13));
                    }

                    tempEchosCNT = tempData.at(32); // sensor 4
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+33);
                        bytes2uint.byteData[1] = tempData.at(4*i+34);
                        sensorTOF_4.append(bytes2uint.wordData);
                        sensorMagnitude_4.append(tempData.at(4*i+35));
                    }

                    tempEchosCNT = tempData.at(62); // sensor 5
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+63);
                        bytes2uint.byteData[1] = tempData.at(4*i+64);
                        sensorTOF_5.append(bytes2uint.wordData);
                        sensorMagnitude_5.append(tempData.at(4*i+65));
                    }

                    tempEchosCNT = tempData.at(84); // sensor 9
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+85);
                        bytes2uint.byteData[1] = tempData.at(4*i+86);
                        sensorTOF_9.append(bytes2uint.wordData);
                        sensorMagnitude_9.append(tempData.at(4*i+87));
                    }

                    tempEchosCNT = tempData.at(106); // sensor 10
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+107);
                        bytes2uint.byteData[1] = tempData.at(4*i+108);
                        sensorTOF_10.append(bytes2uint.wordData);
                        sensorMagnitude_10.append(tempData.at(4*i+109));
                    }

                    tempEchosCNT = tempData.at(136); // sensor 11
                    for(unsigned int i=0;i<tempEchosCNT;i++)
                    {
                        bytes2uint.byteData[0] = tempData.at(4*i+137);
                        bytes2uint.byteData[1] = tempData.at(4*i+138);
                        sensorTOF_11.append(bytes2uint.wordData);
                        sensorMagnitude_11.append(tempData.at(4*i+139));
                    }
                }
                else
                {
                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
                }
            }
            else
            {
                std::cout<<"TX Mask error. Sensor may broken?"<<std::endl;
            }
        }
        else
        {
            std::cout<<"number of working sensor is "<<(int)SDI_sensor_CNT<<"payload size is "<<(int)SDI_payload_size<<" may not correct. default is 4 and 6."<<std::endl;
        }

    }
}

void SDI_DataRecv_Consumer::Clear_Sensor_Data(void)
{
    sensorTOF_0.clear();
    sensorTOF_1.clear();
    sensorTOF_2.clear();
    sensorTOF_3.clear();
    sensorTOF_4.clear();
    sensorTOF_5.clear();
    sensorTOF_6.clear();
    sensorTOF_7.clear();
    sensorTOF_8.clear();
    sensorTOF_9.clear();
    sensorTOF_10.clear();
    sensorTOF_11.clear();

    sensorMagnitude_0.clear();
    sensorMagnitude_1.clear();
    sensorMagnitude_2.clear();
    sensorMagnitude_3.clear();
    sensorMagnitude_4.clear();
    sensorMagnitude_5.clear();
    sensorMagnitude_6.clear();
    sensorMagnitude_7.clear();
    sensorMagnitude_8.clear();
    sensorMagnitude_9.clear();
    sensorMagnitude_10.clear();
    sensorMagnitude_11.clear();
}

void SDI_DataRecv_Consumer::ResultFilterAndPublish(void)
{
    if(sensorTOF_0.size()!=0 && sensorMagnitude_0.size()!=0)
        this->BubbleSort(sensorTOF_0,sensorMagnitude_0);
    else
        return;

    if(sensorTOF_1.size()!=0 && sensorMagnitude_1.size()!=0)
        this->BubbleSort(sensorTOF_1,sensorMagnitude_1);
    else
        return;

    if(sensorTOF_2.size()!=0 && sensorMagnitude_2.size()!=0)
        this->BubbleSort(sensorTOF_2,sensorMagnitude_2);
    else
        return;

    if(sensorTOF_3.size()!=0 && sensorMagnitude_3.size()!=0)
        this->BubbleSort(sensorTOF_3,sensorMagnitude_3);
    else
        return;

    if(sensorTOF_4.size()!=0 && sensorMagnitude_4.size()!=0)
        this->BubbleSort(sensorTOF_4,sensorMagnitude_4);
    else
        return;

    if(sensorTOF_5.size()!=0 && sensorMagnitude_5.size()!=0)
        this->BubbleSort(sensorTOF_5,sensorMagnitude_5);
    else
        return;

    if(sensorTOF_6.size()!=0 && sensorMagnitude_6.size()!=0)
        this->BubbleSort(sensorTOF_6,sensorMagnitude_6);
    else
        return;

    if(sensorTOF_7.size()!=0 && sensorMagnitude_7.size()!=0)
        this->BubbleSort(sensorTOF_7,sensorMagnitude_7);
    else
        return;

    if(sensorTOF_8.size()!=0 && sensorMagnitude_8.size()!=0)
        this->BubbleSort(sensorTOF_8,sensorMagnitude_8);
    else
        return;

    if(sensorTOF_9.size()!=0 && sensorMagnitude_9.size()!=0)
        this->BubbleSort(sensorTOF_9,sensorMagnitude_9);
    else
        return;

    if(sensorTOF_10.size()!=0 && sensorMagnitude_10.size()!=0)
        this->BubbleSort(sensorTOF_10,sensorMagnitude_10);
    else
        return;

    if(sensorTOF_11.size()!=0 && sensorMagnitude_11.size()!=0)
        this->BubbleSort(sensorTOF_11,sensorMagnitude_11);
    else
        return;

    iv::ultrasonic::ultrasonic xmsg;

    for(int i=0;i<sensorTOF_0.size();i++)
    {
        if(sensorTOF_0.at(i)>0 && sensorMagnitude_0.at(i)>5)
        {
            xmsg.set_sigobjdist_flside((uint32_t)(gsoundVelocity * sensorTOF_0.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_ls(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_flside(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_ls(false);
        }
    }

    for(int i=0;i<sensorTOF_1.size();i++)
    {
        if(sensorTOF_1.at(i)>0 && sensorMagnitude_1.at(i)>5)
        {
            xmsg.set_sigobjdist_flcorner((uint32_t)(gsoundVelocity * sensorTOF_1.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_l(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_flcorner(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_l(false);
        }
    }

    for(int i=0;i<sensorTOF_2.size();i++)
    {
        if(sensorTOF_2.at(i)>0 && sensorMagnitude_2.at(i)>5)
        {
            xmsg.set_sigobjdist_flmiddle((uint32_t)(gsoundVelocity * sensorTOF_2.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_lm(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_flmiddle(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_lm(false);
        }
    }

    for(int i=0;i<sensorTOF_3.size();i++)
    {
        if(sensorTOF_3.at(i)>0 && sensorMagnitude_3.at(i)>5)
        {
            xmsg.set_sigobjdist_frmiddle((uint32_t)(gsoundVelocity * sensorTOF_3.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_rm(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_frmiddle(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_rm(false);
        }
    }

    for(int i=0;i<sensorTOF_4.size();i++)
    {
        if(sensorTOF_4.at(i)>0 && sensorMagnitude_4.at(i)>5)
        {
            xmsg.set_sigobjdist_frcorner((uint32_t)(gsoundVelocity * sensorTOF_4.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_r(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_frcorner(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_r(false);
        }
    }

    for(int i=0;i<sensorTOF_5.size();i++)
    {
        if(sensorTOF_5.at(i)>0 && sensorMagnitude_5.at(i)>5)
        {
            xmsg.set_sigobjdist_frside((uint32_t)(gsoundVelocity * sensorTOF_5.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_front_rs(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_frside(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_front_rs(false);
        }
    }

    for(int i=0;i<sensorTOF_6.size();i++)
    {
        if(sensorTOF_6.at(i)>0 && sensorMagnitude_6.at(i)>5)
        {
            xmsg.set_sigobjdist_rrside((uint32_t)(gsoundVelocity * sensorTOF_6.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_rs(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rrside(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_rs(false);
        }
    }

    for(int i=0;i<sensorTOF_7.size();i++)
    {
        if(sensorTOF_7.at(i)>0 && sensorMagnitude_7.at(i)>5)
        {
            xmsg.set_sigobjdist_rrcorner((uint32_t)(gsoundVelocity * sensorTOF_7.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_r(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rrcorner(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_r(false);
        }
    }

    for(int i=0;i<sensorTOF_8.size();i++)
    {
        if(sensorTOF_8.at(i)>0 && sensorMagnitude_8.at(i)>5)
        {
            xmsg.set_sigobjdist_rrmiddle((uint32_t)(gsoundVelocity * sensorTOF_8.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_rm(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rrmiddle(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_rm(false);
        }
    }

    for(int i=0;i<sensorTOF_9.size();i++)
    {
        if(sensorTOF_9.at(i)>0 && sensorMagnitude_9.at(i)>5)
        {
            xmsg.set_sigobjdist_rlmiddle((uint32_t)(gsoundVelocity * sensorTOF_9.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_lm(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rlmiddle(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_lm(false);
        }
    }

    for(int i=0;i<sensorTOF_10.size();i++)
    {
        if(sensorTOF_10.at(i)>0 && sensorMagnitude_10.at(i)>5)
        {
            xmsg.set_sigobjdist_rlcorner((uint32_t)(gsoundVelocity * sensorTOF_10.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_l(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rlcorner(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_l(false);
        }
    }

    for(int i=0;i<sensorTOF_11.size();i++)
    {
        if(sensorTOF_11.at(i)>0 && sensorMagnitude_11.at(i)>5)
        {
            xmsg.set_sigobjdist_rlside((uint32_t)(gsoundVelocity * sensorTOF_11.at(i) * 51.2 / 2000.0)); // mm
            xmsg.set_sigsensor_rear_ls(true);
            break;
        }
        else
        {
            xmsg.set_sigobjdist_rlside(DIST_ERROR); // 50000 mm
            xmsg.set_sigsensor_rear_ls(false);
        }
    }

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

void SDI_DataRecv_Consumer::BubbleSort(QVector<uint16_t> &sensorTOF, QVector<uint16_t> &sensorMagnitude)
{
    if(sensorTOF.size()<2||sensorMagnitude.size()<2)return;
    //notice vector size must >= 2
    int i = 0,j = 0;
    uint16_t tempTOF = 0;
    uint8_t tempMagnitude = 0;
    if(sensorTOF.size() == sensorMagnitude.size())
    {
        for(i=0;i<sensorTOF.size()-1;i++)
        {
            for(j=0;j<sensorTOF.size()-1-i;j++)
            {
                if(sensorTOF.at(j)>sensorTOF.at(j+1))
                {
                    tempTOF = sensorTOF.at(j);
                    sensorTOF[j] = sensorTOF.at(j+1);
                    sensorTOF[j+1] = tempTOF;
                    tempMagnitude = sensorMagnitude.at(j);
                    sensorMagnitude[j] = sensorMagnitude.at(j+1);
                    sensorMagnitude[j+1] = tempMagnitude;
                }
            }
        }
    }
}