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

uint16_t CCanRadarSensor::bcd2dec(uint8_t data0, uint8_t data1)
{

    unsigned char Dst[4];
    memset(Dst, 0, 4);
    uint16_t i = (((data0 & 0x00FF) << 8) | (data1 & 0x00FF));
    sprintf((char *)Dst, "%x", i);
    return strtol((const char *)Dst, NULL, 10);
    
}

// 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;
//         }
//     }
// }
#include <chrono>
void CCanRadarSensor::Notify(struct can_frame *date)
{

    

    //auto start = std::chrono::system_clock::now();

    if(_run)
    {
        if (date -> can_id == 0x61b){
            int c_l_1 = date->data[1];
            int c_l_2 = date->data[2];

            c_l_1 = c_l_1 * 32;
            c_l_2 = c_l_2 >> 3;

            int32_t dis_long2 = ((c_l_1 + c_l_2) * 0.2f - 500.0f) * 1000; // 毫米  纵向   
            _data.r0 = dis_long2;
            _count++;
        }

        else if (date -> can_id == 0x62b){
            int c_l_1 = date->data[1];
            int c_l_2 = date->data[2];

            c_l_1 = c_l_1 * 32;
            c_l_2 = c_l_2 >> 3;

            int32_t dis_long2 = ((c_l_1 + c_l_2) * 0.2f - 500.0f) * 1000; // 毫米  纵向
            _data.r1 = dis_long2;
            // _data.r2 = dis_long2;
             _count++;
        }

        else if (date -> can_id == 0x63b){
            int c_l_1 = date->data[1];
            int c_l_2 = date->data[2];

            c_l_1 = c_l_1 * 32;
            c_l_2 = c_l_2 >> 3;

            int32_t dis_long2 = ((c_l_1 + c_l_2) * 0.2f - 500.0f) * 1000; // 毫米  纵向
            _data.r2 = dis_long2;
            // _data.r4 = dis_long2;
             _count++;
        }

        else if (date -> can_id == 0x64b){
            int c_l_1 = date->data[1];
            int c_l_2 = date->data[2];

            c_l_1 = c_l_1 * 32;
            c_l_2 = c_l_2 >> 3;

            int32_t dis_long2 = ((c_l_1 + c_l_2) * 0.2f - 500.0f) * 1000; // 毫米  纵向
            _data.r3 = dis_long2;
            // _data.r6 = dis_long2;
             _count++;
        }

        else if (date-> can_id== 0x611)
        {
            _data.r4 = bcd2dec(date->data[0], date->data[1]);
            _data.r5 = bcd2dec(date->data[2], date->data[3]);
            _data.r6 = bcd2dec(date->data[4], date->data[5]);
            _data.r7 = bcd2dec(date->data[6], date->data[7]);
            _count += 4;

        } 

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

        //std::this_thread::sleep_for(std::chrono::milliseconds(20));  
        // 执行一些操作
        //auto end = std::chrono::system_clock::now();

        //std::chrono::duration<double> elapsed = end - start;
        //std::cout << "Elapsed time: " << elapsed.count() << " seconds" << std::endl;
    }
}