#include "freertos_action.h"

extern uint32_t last_rev_usart1_tick;
extern uint32_t last_rev_usart2_tick;

void task_usart_rev_action(void)
{
    usart_data_t data = {0};
   // uint8_t data_uart;
    osStatus_t os_status;
    active_uasrt_irq(1);
    active_uasrt_irq(2);
    for(;;)
    {
        
        if (usart_rev_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(usart_rev_queueHandle, &data, NULL, osWaitForever);
            if (os_status == osOK)
            {
                if (data.len < 6)
                {
                  deal_send_data(&data);
                }
                else
                {
                    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(;;)
    {
        allocation_update();
        if (uavcan_send_queueHandle != NULL)
        {
            status = osMessageQueueGet(uavcan_send_queueHandle, &data, NULL, 5);
            if (status == osOK)
            {
                send_uavcan_data(&data);
            }
        }
        send_canard();   
         check_need_to_jump_boot();   
    }
}

void task_default_action(void)
{
    
    uavcan_init_by_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())
        {
            uavcan_cleanup_stale_transfers(HAL_GetTick() * 1000);
            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)
{
    osStatus os_status;
    usart_data_t data = {0};
    for (;;)
    {
        if (usart_send_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(usart_send_queueHandle, &data, NULL, 10);
            if (os_status == osOK)
            {
                if (data.usart_instance == USART_INSTANCE_U1)
                {
                    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&data.data, data.len);
                }
                else
                {
                    HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&data.data, data.len);
                }
            }
        }

        if ((last_rev_usart1_tick + 3000) < HAL_GetTick())
        {
            request_qoar1271_by_step(1, USART_INSTANCE_U1);
        }

        if ((last_rev_usart2_tick + 3000) < HAL_GetTick())
        {
            request_qoar1271_by_step(1, USART_INSTANCE_U2);
        }
    }
}

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