/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2026 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp_can.h"
#include "CAN_receive.h"
#include "chassis_control.h"
#include "bsp_rc.h"
#include "remote_control.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
typedef StaticQueue_t osStaticMessageQDef_t;
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for CAN_Receive_Tas */
osThreadId_t CAN_Receive_TasHandle;
const osThreadAttr_t CAN_Receive_Tas_attributes = {
  .name = "CAN_Receive_Tas",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityHigh,
};
/* Definitions for Chassis_Control */
osThreadId_t Chassis_ControlHandle;
const osThreadAttr_t Chassis_Control_attributes = {
  .name = "Chassis_Control",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Command_Task */
osThreadId_t Command_TaskHandle;
const osThreadAttr_t Command_Task_attributes = {
  .name = "Command_Task",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for chassis_speed_queue */
osMessageQueueId_t chassis_speed_queueHandle;
uint8_t chassis_speed_queueBuffer[ 5 * sizeof( Chassis_Speed ) ];  // 队列缓冲区：5个Chassis_Speed(vx,vy,vw)
osStaticMessageQDef_t chassis_speed_queueControlBlock;
const osMessageQueueAttr_t chassis_speed_queue_attributes = {
  .name = "chassis_speed_queue",
  .cb_mem = &chassis_speed_queueControlBlock,
  .cb_size = sizeof(chassis_speed_queueControlBlock),
  .mq_mem = &chassis_speed_queueBuffer,
  .mq_size = sizeof(chassis_speed_queueBuffer)
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartCAN_Receive_Task(void *argument);
void StartChassis_Control_Task(void *argument);
void StartCommand_Task(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of chassis_speed_queue */
  chassis_speed_queueHandle = osMessageQueueNew (5, sizeof(Chassis_Speed), &chassis_speed_queue_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of CAN_Receive_Tas */
  CAN_Receive_TasHandle = osThreadNew(StartCAN_Receive_Task, NULL, &CAN_Receive_Tas_attributes);

  /* creation of Chassis_Control */
  Chassis_ControlHandle = osThreadNew(StartChassis_Control_Task, NULL, &Chassis_Control_attributes);

  /* creation of Command_Task */
  Command_TaskHandle = osThreadNew(StartCommand_Task, NULL, &Command_Task_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartCAN_Receive_Task */
/**
  * @brief  Function implementing the CAN_Receive_Tas thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartCAN_Receive_Task */
void StartCAN_Receive_Task(void *argument)
{
  /* USER CODE BEGIN StartCAN_Receive_Task */
  
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END StartCAN_Receive_Task */
}

/* USER CODE BEGIN Header_StartChassis_Control_Task */
/**
* @brief Function implementing the Chassis_Control thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartChassis_Control_Task */
void StartChassis_Control_Task(void *argument)
{
  /* USER CODE BEGIN StartChassis_Control_Task */
  chassis_control_init();          // 初始化底盘电机PID
  Chassis_Speed speed;             // 接收遥控器下发的目标速度

  /* Infinite loop */
  for(;;)
  {
    // 非阻塞接收遥控器速度指令，有新的就更新目标速度
    if (osMessageQueueGet(chassis_speed_queueHandle, &speed, NULL, 0) == osOK)
    {
      chassis_set_target_speed(&speed);
    }
    // 执行底盘控制：运动学解算 + PID计算 + CAN发送
    chassis_control_task();
    osDelay(1);                    // 1ms 控制周期
  }
  /* USER CODE END StartChassis_Control_Task */
}

/* USER CODE BEGIN Header_StartCommand_Task */
/**
* @brief Function implementing the Command_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartCommand_Task */
void StartCommand_Task(void *argument)
{
  /* USER CODE BEGIN StartCommand_Task */
  remote_control_init();
  const RC_ctrl_t *rc;
  Chassis_Speed speed;
  /* Infinite loop */
  for(;;)
  {
    rc = get_remote_control_point();  // 获取遥控器数据

    // 遥控器通道映射：ch[3]=左摇杆上下(前后), ch[2]=左摇杆左右(左右)
    // 范围 [-660, 660] 映射到 [-MAX_CHASSIS_SPEED, MAX_CHASSIS_SPEED] m/s
    // 摇杆死区：RC_DEADZONE 以内视为0，消除摇杆中位抖动
    speed.vx =  (abs(rc->rc.ch[2]) > RC_DEADZONE) ? (float)rc->rc.ch[3] * MAX_CHASSIS_SPEED / 660.0f : 0.0f;  // 前后速度
    speed.vy =  (abs(rc->rc.ch[3]) > RC_DEADZONE) ? (float)rc->rc.ch[2] * MAX_CHASSIS_SPEED / 660.0f : 0.0f;  // 左右速度
    speed.vw =  0;  // 旋转速度默认为0

    // 右侧开关切换模式
    if (rc->rc.s[1] == RC_SW_UP)
    {
      speed.vw = -2.0f;  // 上拨：小陀螺模式，原地旋转
    }
    else if (rc->rc.s[1] == RC_SW_DOWN)
    {
      speed.vx = 0;  // 下拨：停止模式，所有速度置零
      speed.vy = 0;
      speed.vw = 0;
    }

    osMessageQueuePut(chassis_speed_queueHandle, &speed, 0, 0);  // 发送速度到底盘控制任务
    osDelay(10);
  }
  /* USER CODE END StartCommand_Task */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */