#include <stdint.h>
#include <chrono>

#include "radar_ip.h"
#include "message_queue.h"

#pragma pack(1)
struct Radar_Frame_Nl_Revice
{

    int8_t DLC_Nl_radar;
    int32_t Addr_Nl_radar;
    uint8_t RegData_Nl_radar1[8];
};

CRadarSensor::CRadarSensor(CMessageQueue* q):_message(q)
{
    _run = false;
}

void CRadarSensor::Notify(int8_t * buffer,int32_t size)
{
    int i = 0;

    int8_t source[13];

    if(_run)
    {
        for(i = 0 ; i < size  ; i += 13)
        {
            memset(source,0,13);
            memcpy(source,buffer + i,13);

            Radar_Frame_Nl_Revice * p = (Radar_Frame_Nl_Revice *)source;

            if ((htonl(p->Addr_Nl_radar) & 0x60B) == 0x60B)
            {
                int Nl_Index = (int)(((htonl(p->Addr_Nl_radar)) & 0x00f0) >> 4);

                switch (Nl_Index)
                {
                    case 1:
                        _data.r0 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;
                    
                    case 2:
                        _data.r1 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 3:
                        _data.r2 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 4:
                        _data.r3 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 5:
                        _data.r4 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 6:
                        _data.r5 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 7:
                        _data.r6 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    case 8:
                        _data.r7 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
                        break;

                    default:
                        break;
                }
            }
        }
         std::cout << "radar :" << _data.r0 << " " << _data.r1 << " " << _data.r2 << " " << _data.r3 << " "  
                << _data.r4 << " " << _data.r5 << " " << _data.r6 << " " << _data.r7 << " "<< std::endl;

        _count++;

        if (_count > 8)
        {
            _message->WriteRadarData(_data);
            _count = 0;
        }
    }
}

void CRadarSensor::Start()
{
    if(!_run)
    {
        _run = true;
        _count = 0;
    } 
}

void CRadarSensor::Run()
{
    if (_run)
    {
    }
}
  
void CRadarSensor::SetSensorSocket(SensorSocket<CRadarSensor>* can)
{
    _socket = can;
}
void CRadarSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _count = 0;
    }
}