/*
    test Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "cmsis_os2.h"
#include "module/chassis.h"
#include "module/config.h"
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Chassis_t chassis;
ChassisCmd_t chassis_cmd;         // 控制命令
Chassis_IMU_t chassis_imu;
Chassis_Feedback_t chassis_fb;    // 反馈的底盘状态
static AHRS_Eulr_t mech_zero = {0};      // 机械零位（初始 yaw）
#define DR16_CH_VALUE_MID (1024u)

/* USER STRUCT END */

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

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

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

//  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
//  /* USER CODE INIT BEGIN */
//   Config_RobotParam_t *cfg = Config_GetRobotParam();
//   Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, &mech_zero, TEST_FREQ);
//	
//	  bool mech_zero_locked = false;                 // 首次 IMU 到来后只锁一次

//  // 默认命令：安全状态
//  chassis_cmd.mode = CHASSIS_MODE_RELAX;
//  chassis_cmd.ctrl_vec.vx = 0.0f;
//  chassis_cmd.ctrl_vec.vy = 0.0f;
//  chassis_cmd.ctrl_vec.wz = 0.0f;
////  /* USER CODE INIT END */
//  
//  while (1) {
//    tick += delay_tick; /* 计算下一个唤醒时刻 */
//    /* USER CODE BEGIN */
//		if (osMessageQueueGet(task_runtime.msgq.Chassis_imu, &chassis_imu, NULL, 0) == osOK) {
//		chassis_fb.imu = chassis_imu;
//    chassis.feedback.imu = chassis_imu;
//			 if (!mech_zero_locked) {
//        mech_zero.yaw = chassis_imu.eulr.yaw;  // 只在需要“上电即定零”时保留
//        mech_zero_locked = true;
//    }
//}
////if(osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
////	
////}
//        Chassis_UpdateFeedback(&chassis, &chassis_fb);
//        // 构造底盘指令 
//				chassis_cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
//        // 控制计算
//        Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
//        // 输出电流到电机   
//        Chassis_Output(&chassis);

////    /* USER CODE END */
//    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
//  }
//  
//}


void Task_test(void *argument) {
    (void)argument;

    /* 以 TEST_FREQ 定频运行 */
    const uint32_t delay_tick = osKernelGetTickFreq() / TEST_FREQ;
    osDelay(TEST_INIT_DELAY);

    uint32_t tick = osKernelGetTickCount();

    /* === 初始化底盘 === */
    Config_RobotParam_t *cfg = Config_GetRobotParam();
    Chassis_Init(&chassis, &cfg->chassis_param, &mech_zero, (float)TEST_FREQ);

    /* 默认安全命令 */
    chassis_cmd.mode = CHASSIS_MODE_RELAX;
    chassis_cmd.ctrl_vec.vx = 0.0f;
    chassis_cmd.ctrl_vec.vy = 0.0f;
    chassis_cmd.ctrl_vec.wz = 0.0f;

    bool mech_zero_locked = false;

    while (1) {
        tick += delay_tick;

        /* 1) 非阻塞取 IMU（如果来了就更新；没来就用上一帧） */
        if (osMessageQueueGet(task_runtime.msgq.Chassis_imu, &chassis_imu, NULL, 0) == osOK) {
            chassis_fb.imu = chassis_imu;
            chassis.feedback.imu = chassis_imu;  // 兼容你在其他处直接读 chassis.feedback 的用法

            if (!mech_zero_locked) {
                mech_zero.yaw = chassis_imu.eulr.yaw; // 首帧锁零（按需保留）
                mech_zero_locked = true;
            }
        }

        /* 2) 免遥控：设定固定运动指令（独立模式，不跟随云台） */
        chassis_cmd.mode = CHASSIS_MODE_INDEPENDENT;
        chassis_cmd.ctrl_vec.vx = 1.0f;
        chassis_cmd.ctrl_vec.vy = 0.0f;
        chassis_cmd.ctrl_vec.wz = 0.0f;

        /* 3) 更新反馈 → 控制 → 输出 */
        Chassis_UpdateFeedback(&chassis, &chassis_fb);
        Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
        Chassis_Output(&chassis);

        osDelayUntil(tick);
    }
}