#include "ivdetection_radar_delphi_esr.h"

#include <math.h>

namespace iv {


ivdetection_radar_delphi_esr::ivdetection_radar_delphi_esr(std::string strxmlpath)
{
    iv::xmlparam::Xmlparam xp(strxmlpath);

    std::string strcanmsgname = xp.GetParam("canrecv","canrecv1");
    std::string strradarmsgname = xp.GetParam("radar","radar");
    setcanmsgname(strcanmsgname);
    setradarmsgname(strradarmsgname);
}

int ivdetection_radar_delphi_esr::DecodeCANMsg(radar::radarobjectarray &xradar, can::canraw *prawmsg)
{
    int nrtn = 0;
    int32_t range, rate, angle;
    static int ncount = 0;
    static qint64 nlastsend = 0;

    ncount++;

    iv::can::canraw canmsg;
    canmsg.CopyFrom(*prawmsg);

    if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53f))
    {
        int  data[8];
        unsigned char cdata[8];
        memcpy(cdata,canmsg.data().data(),8);

        int radar_ID_index = canmsg.id() - 0x500;

        int j;
        for(j=0;j<8;j++)data[j] = cdata[j];

        angle = ((data[1] & 0x1F) << 5) + ((data[2] & 0xF8) / 8);
        range = ((data[2] & 0x07) << 8) + data[3];
        rate = ((data[6] & 0x3F) << 8) | data[7];
        if (angle & 0x200) {
            angle = angle | 0xFFFFFC00;
        }
        if (rate & 0x2000) {
            rate = rate | 0xFFFFC000;
        }
        //           qDebug("range = %d angle = %d ",range,angle);
        //If angle and range both are 0, it is an invalid data.
        if (angle != 0 || range != 0) {
            iv::radar::radarobject * pobj = xradar.add_obj();
            pobj->set_bvalid(true);

            pobj->set_x(range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI));
            pobj->set_y(range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI));
            pobj->set_vel(rate * 1.0 / 100.0);
            pobj->set_vx(pobj->vel() * sin(angle / 1800.0 * M_PI));
            pobj->set_vy(pobj->vel() * cos(angle / 1800.0 * M_PI));
        }
        else {
            iv::radar::radarobject * pobj = xradar.add_obj();
            pobj->set_bvalid(false);
        }


        if(canmsg.id() == 0x53f)
        {
            nrtn = 1;
        }

        if(ncount > 100)
        {
            nrtn = 1;
        }

        if((QDateTime::currentMSecsSinceEpoch() - nlastsend) > 100)
        {
            nrtn = 1;
        }
    }

    if(nrtn == 1)
    {
        ncount = 0;
        nlastsend = QDateTime::currentMSecsSinceEpoch();
    }
    //       qDebug("id is %08x",canmsg.id());

    return nrtn;
}

}