#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)
    {
        //_can_thread = std::thread(&CCanRadarSensor::Run, this);
        _run = true;
    } 
}

void CCanRadarSensor::Run()
{
    if (_run)
    {
    }
}
  
void CCanRadarSensor::Stop()
{
    if (_run)
    {
        _run = false;
        //_can_thread.join();
    }
}

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

            switch (Nl_Index)
            {
                case 1:
                _message->_Radardata.r0 = ((((date->data[1] * 32) + (date->data[2] >> 3)) * 0.2) - 500);
                break;
                    
                default:
                break;
            }
            std::cout << "radar :" << _message->_Radardata.r0 <<  std::endl;
                
        }
        
    }
}