/*
    imu Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "component/ahrs/ahrs.h"
#include <string.h>
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* CAN ID定义 - 使用标准帧 */
#define CAN_ID_IMU_ACCEL   0x100  /* 加速度计数据 */
#define CAN_ID_IMU_GYRO    0x101  /* 陀螺仪数据 */
#define CAN_ID_IMU_EULER   0x102  /* 欧拉角数据 */
#define CAN_ID_IMU_QUAT    0x103  /* 四元数数据 */

/* 数据范围定义（与DM_IMU解析一致） */
#define ACCEL_CAN_MAX (58.8f)
#define ACCEL_CAN_MIN (-58.8f)
#define GYRO_CAN_MAX  (34.88f)
#define GYRO_CAN_MIN  (-34.88f)
#define PITCH_CAN_MAX (90.0f)
#define PITCH_CAN_MIN (-90.0f)
#define ROLL_CAN_MAX  (180.0f)
#define ROLL_CAN_MIN  (-180.0f)
#define YAW_CAN_MAX   (180.0f)
#define YAW_CAN_MIN   (-180.0f)
#define QUAT_MIN      (-1.0f)
#define QUAT_MAX      (1.0f)

/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
static AHRS_All_t imu_data;

/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/**
 * @brief 浮点数转换为无符号整数函数
 */
static uint32_t float_to_uint(float x, float x_min, float x_max, int bits) {
    float span = x_max - x_min;
    float offset = x_min;
    // 限幅
    if (x > x_max) x = x_max;
    if (x < x_min) x = x_min;
    return (uint32_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

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

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

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

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  BSP_CAN_Init();
  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
    /* 从消息队列获取IMU数据 */
    if (osMessageQueueGet(task_runtime.msgq.imu, &imu_data, NULL, 0) == osOK) {
      BSP_CAN_StdDataFrame_t can_frame;
      
      /* 包1: 加速度计数据 (acc_x, acc_y, acc_z) - 使用21位精度 */
      can_frame.id = CAN_ID_IMU_ACCEL;
      can_frame.dlc = 8;
      uint32_t acc_x = float_to_uint(imu_data.accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
      uint32_t acc_y = float_to_uint(imu_data.accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
      uint32_t acc_z = float_to_uint(imu_data.accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
      // 打包: acc_x(21bit) acc_y(21bit) acc_z(21bit) = 63bits
      can_frame.data[0] = (acc_x >> 13) & 0xFF;
      can_frame.data[1] = (acc_x >> 5) & 0xFF;
      can_frame.data[2] = ((acc_x << 3) & 0xF8) | ((acc_y >> 18) & 0x07);
      can_frame.data[3] = (acc_y >> 10) & 0xFF;
      can_frame.data[4] = (acc_y >> 2) & 0xFF;
      can_frame.data[5] = ((acc_y << 6) & 0xC0) | ((acc_z >> 15) & 0x3F);
      can_frame.data[6] = (acc_z >> 7) & 0xFF;
      can_frame.data[7] = (acc_z << 1) & 0xFE;
      BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
      
      /* 包2: 陀螺仪数据 (gyro_x, gyro_y, gyro_z) - 使用21位精度 */
      can_frame.id = CAN_ID_IMU_GYRO;
      can_frame.dlc = 8;
      uint32_t gyro_x = float_to_uint(imu_data.gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
      uint32_t gyro_y = float_to_uint(imu_data.gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
      uint32_t gyro_z = float_to_uint(imu_data.gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
      // 打包: gyro_x(21bit) gyro_y(21bit) gyro_z(21bit) = 63bits
      can_frame.data[0] = (gyro_x >> 13) & 0xFF;
      can_frame.data[1] = (gyro_x >> 5) & 0xFF;
      can_frame.data[2] = ((gyro_x << 3) & 0xF8) | ((gyro_y >> 18) & 0x07);
      can_frame.data[3] = (gyro_y >> 10) & 0xFF;
      can_frame.data[4] = (gyro_y >> 2) & 0xFF;
      can_frame.data[5] = ((gyro_y << 6) & 0xC0) | ((gyro_z >> 15) & 0x3F);
      can_frame.data[6] = (gyro_z >> 7) & 0xFF;
      can_frame.data[7] = (gyro_z << 1) & 0xFE;
      BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
      
      /* 包3: 欧拉角数据 (pit, yaw, rol) - 使用21位精度 */
      can_frame.id = CAN_ID_IMU_EULER;
      can_frame.dlc = 8;
      uint32_t pit = float_to_uint(imu_data.eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX, 21);
      uint32_t yaw = float_to_uint(imu_data.eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX, 21);
      uint32_t rol = float_to_uint(imu_data.eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX, 21);
      // 打包: pit(21bit) yaw(21bit) rol(21bit) = 63bits
      can_frame.data[0] = (pit >> 13) & 0xFF;
      can_frame.data[1] = (pit >> 5) & 0xFF;
      can_frame.data[2] = ((pit << 3) & 0xF8) | ((yaw >> 18) & 0x07);
      can_frame.data[3] = (yaw >> 10) & 0xFF;
      can_frame.data[4] = (yaw >> 2) & 0xFF;
      can_frame.data[5] = ((yaw << 6) & 0xC0) | ((rol >> 15) & 0x3F);
      can_frame.data[6] = (rol >> 7) & 0xFF;
      can_frame.data[7] = (rol << 1) & 0xFE;
      BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
      
      /* 包4: 四元数数据 (q0, q1, q2, q3) - 使用14位精度打包 */
      can_frame.id = CAN_ID_IMU_QUAT;
      can_frame.dlc = 8;
      uint16_t q0 = float_to_uint(imu_data.quat.q0, QUAT_MIN, QUAT_MAX, 14);
      uint16_t q1 = float_to_uint(imu_data.quat.q1, QUAT_MIN, QUAT_MAX, 14);
      uint16_t q2 = float_to_uint(imu_data.quat.q2, QUAT_MIN, QUAT_MAX, 14);
      uint16_t q3 = float_to_uint(imu_data.quat.q3, QUAT_MIN, QUAT_MAX, 14);
      // 打包: q0(14bit) q1(14bit) q2(14bit) q3(14bit) = 56bits
      can_frame.data[0] = 0;  // 预留
      can_frame.data[1] = (q0 >> 6) & 0xFF;
      can_frame.data[2] = ((q0 << 2) & 0xFC) | ((q1 >> 12) & 0x03);
      can_frame.data[3] = (q1 >> 4) & 0xFF;
      can_frame.data[4] = ((q1 << 4) & 0xF0) | ((q2 >> 10) & 0x0F);
      can_frame.data[5] = (q2 >> 2) & 0xFF;
      can_frame.data[6] = ((q2 << 6) & 0xC0) | ((q3 >> 8) & 0x3F);
      can_frame.data[7] = q3 & 0xFF;
      BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
    }

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