/*
    vofa Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/balance_chassis.h"
#include "device/vofa.h"
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Chassis_t chassis_vofa;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_vofa(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / VOFA_FREQ;

  osDelay(VOFA_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
    // 从消息队列获取chassis数据
    if (osMessageQueueGet(task_runtime.msgq.chassis.vofa, &chassis_vofa, NULL, 0) == osOK) {
      // 准备要发送的数据通道
      float channels[16];
      
      // 通道0-1: 左右腿的VMC腿长
      channels[0] = chassis_vofa.vmc_[0].leg.L0;
      channels[1] = chassis_vofa.vmc_[1].leg.L0;
      
      // 通道2-3: 左右腿的VMC角度
      channels[2] = chassis_vofa.vmc_[0].leg.theta;
      channels[3] = chassis_vofa.vmc_[1].leg.theta;
      
      // 通道4-6: IMU欧拉角 (pitch, roll, yaw)
      channels[4] = chassis_vofa.feedback.imu.euler.pit;
      channels[5] = chassis_vofa.feedback.imu.euler.rol;
      channels[6] = chassis_vofa.feedback.imu.euler.yaw;
      
      // 通道7-8: 左右轮速度
      channels[7] = chassis_vofa.vmc_[0].leg.d_theta;
      channels[8] = chassis_vofa.vmc_[0].leg.dd_theta;
      
      // 通道9-10: 机体位置和速度
      channels[9] = chassis_vofa.chassis_state.position_x;
      channels[10] = chassis_vofa.chassis_state.velocity_x;
      
      // 通道11-12: 左右腿的虚拟支撑力
      channels[11] = chassis_vofa.vmc_[0].leg.F_virtual;
      channels[12] = chassis_vofa.vmc_[1].leg.F_virtual;
      
      // 通道13-14: 左右腿的虚拟摆力矩
      channels[13] = chassis_vofa.vmc_[0].leg.T_virtual;
      channels[14] = chassis_vofa.vmc_[1].leg.T_virtual;
      
      // 通道15: yaw控制力矩
      channels[15] = chassis_vofa.yaw_control.yaw_force;
      
      // 使用JustFloat协议和DMA发送
      VOFA_Send(channels, 16, true);
    }
    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}