#include "freertos_action.h"

void send_uavcan_proximity(uavcan_proximity_t *data);

void task_usart_rev_action(void)
{
    usart_data_t data = {0};
   // uint8_t data_uart;
    osStatus_t os_status;
    
    for(;;)
    {
        if (usart_rev_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(usart_rev_queueHandle, &data, NULL, osWaitForever);
            if (os_status == osOK)
            {   
               parse_ar1271_data(&data);               
            }
        }
        else
        {
            osDelay(2);
        }
    }
}

void task_uavcan_rev_action(void)
{
    uavcan_rev_t uavcan_rev_data;
    osStatus_t os_status;
    
    /* Infinite loop */
    for (;;)
    {
        if (uavcan_rev_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(uavcan_rev_queueHandle, &uavcan_rev_data, NULL, osWaitForever);
            if (os_status == osOK)
            {
                handle_uavcan_rx_frame(&uavcan_rev_data);              
            }
        }
        else
        {
            osDelay(2);
        }
    }
}

void task_uavcan_send_action(void)
{
    uavcan_send_data_t data = {0};
    float depth_data=0.0f;
    osStatus_t status;
    
    for(;;)
    {
        if (uavcan_send_queueHandle != NULL)
        {
            status = osMessageQueueGet(uavcan_send_queueHandle, &data, NULL, 5);
            if (status == osOK)
            {
                send_uavcan_data(&data);
            }
        }
        send_canard();      
    }
}

void task_default_action(void)
{
    init_allocation();
    uint32_t last_send_node_status_tick =0; 
    uint32_t last_send_proximity_tick =0;
    /* Infinite loop */
    for (;;)
    {
        if(last_send_node_status_tick+1000<HAL_GetTick())
        {
            last_send_node_status_tick = HAL_GetTick();
            if (uavcan_send_queueHandle != NULL)
            {
                uavcan_send_data_t send_data ={0};
                send_data.id = UAVCAN_NODE_STATUS_DATA_TYPE_ID;
                uavcan_node_status_t data ={0};
                memcpy(&send_data.content.node_state,&data,sizeof(uavcan_node_status_t));
                osMessageQueuePut(uavcan_send_queueHandle,&send_data,0,10);
            }
        }
        osDelay(200);
    }
}

void task_usart_send(void)
{
    osDelay(5000);
    request_QOAR1271_data();
    active_uasrt_irq(1);
    active_uasrt_irq(2);
    for (;;)
    {
        osDelay(1000);
    }
}

void task_proximity(void)
{
    proximity_data_t proximity1 ={0};
    proximity_data_t proximity2 ={0};
    osStatus_t os_p1 =osError;
    osStatus_t os_p2 =osError;
    for (;;)
    {
        if (proximity1_queueHandle != NULL && proximity2_queueHandle != NULL)
        {
            os_p1 = osMessageQueueGet(proximity1_queueHandle, &proximity1, NULL, 100);
            os_p2 = osMessageQueueGet(proximity2_queueHandle, &proximity2, NULL, 100);
            if((os_p1 == osOK) || (os_p2 == osOK)){
                uavcan_proximity_t data = {0};
                data.d0 = proximity1.d0;
                data.d45 = proximity1.d45;
                data.d90 = proximity1.d90;
                data.d135 = proximity1.d135;
                data.d180 = proximity1.d180;
                data.d225 = proximity1.d225;
                data.d270 = proximity1.d270;
                data.d315 = proximity1.d315;
                send_uavcan_proximity(&data);
            }
            // if ((os_p1 == osOK) && (os_p2 == osOK))
            // {
            //     uavcan_proximity_t data = {0};
            //     data.d0 = min(proximity1.d0, proximity2.d0);
            //     data.d315 = proximity1.d315;
            //     data.d45 = proximity2.d45;
            //     send_uavcan_proximity(&data);
            // }
            // else
            // {
            //     if (os_p1 == osOK)  //左边的雷达是反的
            //     {
            //         uavcan_proximity_t data = {0};
            //         data.d0 = proximity1.d0;
            //         data.d315 = proximity1.d315;
            //         data.d45 = proximity1.d45;
            //         send_uavcan_proximity(&data);
            //     }
            //     else if (os_p2 == osOK)
            //     {
            //         uavcan_proximity_t data = {0};
            //         data.d0 = proximity2.d0;
            //         data.d315 = proximity2.d315;
            //         data.d45 = proximity2.d45;
            //         send_uavcan_proximity(&data);
            //     }
            // }
        }
    }
}

uint16_t min(uint16_t one, uint16_t two)
{
    return one < two ? one : two;
}

void send_uavcan_proximity(uavcan_proximity_t *data)
{
    uavcan_send_data_t msg = {0};
    if (uavcan_send_queueHandle != NULL)
    {
        msg.id = ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID;
        memcpy(&msg.content.proximity, data, sizeof(uavcan_proximity_t));
        osMessageQueuePut(uavcan_send_queueHandle, &msg, NULL, 10U);
    }
}