#include "radar_can.h"
#include "message_queue.h"

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

void CCanRadarSensor::SetCanBusSensor(SensorCanBus<CCanRadarSensor>* can)
{
    _canbus = can;
}

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

void CCanRadarSensor::Run()
{
    if (_run)
    {
    }
}
  
void CCanRadarSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _count = 0;
    }
}

void CCanRadarSensor::Notify(struct can_frame *date)
{
    if(_run)
    {
       if ((htonl(date->can_id) & 0x60B) == 0x60B)
        {
            int Nl_Index = (int)(((htonl(date->can_id)) & 0x00f0) >> 4);

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

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

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

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

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

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

                case 8:
                    _data.r7 = ((((date->data[1] * 32) + (date->data[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;
        }
    }
}