/*
 * Gimbal IMU Device - 接收来自云台的CAN IMU数据
 */

/* Includes ----------------------------------------------------------------- */
#include "gimbal_imu.h"

#include <string.h>

#include "bsp/fdcan.h"
#include "bsp/time.h"

/* Private function prototypes ---------------------------------------------- */

/**
 * @brief 无符号整数转换为浮点数
 * 
 * @param x_int 输入的整数值
 * @param x_min 最小值
 * @param x_max 最大值
 * @param bits 位数
 * @return float 转换后的浮点数
 */
static float uint_to_float(uint32_t x_int, float x_min, float x_max, int bits) {
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

/**
 * @brief 解析CAN消息中的IMU数据
 */
static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_FROMCAN_t *gimbal_imu, uint32_t id, const uint8_t *data) {
  if (gimbal_imu == NULL || data == NULL) {
    return;
  }
  
  /* 判断是哪个ID的数据 */
  if (id == gimbal_imu->param.accl_id) {
    /* 解析加速度计数据 - 21位精度解包 */
    uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
    uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
    uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
    
    gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
    gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
    gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
    
  } else if (id == gimbal_imu->param.gyro_id) {
    /* 解析陀螺仪数据 - 21位精度解包 */
    uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
    uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
    uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
    
    gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
    gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
    gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
    
  } else if (id == gimbal_imu->param.eulr_id) {
    /* 解析欧拉角数据 - 21位精度解包 */
    uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
    uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
    uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
    
    gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
    gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
    gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
    
  } else if (id == gimbal_imu->param.quat_id) {
    /* 解析四元数数据 - 使用14位精度解包 */
    uint16_t q0 = ((uint16_t)data[1] << 6) | ((uint16_t)data[2] >> 2);
    uint16_t q1 = (((uint16_t)data[2] & 0x03) << 12) | ((uint16_t)data[3] << 4) | ((uint16_t)data[4] >> 4);
    uint16_t q2 = (((uint16_t)data[4] & 0x0F) << 10) | ((uint16_t)data[5] << 2) | ((uint16_t)data[6] >> 6);
    uint16_t q3 = (((uint16_t)data[6] & 0x3F) << 8) | (uint16_t)data[7];
    
    gimbal_imu->data.quat.q0 = uint_to_float(q0, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
    gimbal_imu->data.quat.q1 = uint_to_float(q1, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
    gimbal_imu->data.quat.q2 = uint_to_float(q2, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
    gimbal_imu->data.quat.q3 = uint_to_float(q3, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
  }
}

/* Exported functions ------------------------------------------------------- */

/**
 * @brief 初始化Gimbal IMU设备
 * @param gimbal_imu Gimbal IMU结构体指针
 * @param param 参数配置
 * @return 初始化结果
 *   - DEVICE_OK: 成功
 *   - DEVICE_ERR_NULL: 参数为空
 */
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
  if (gimbal_imu == NULL || param == NULL) {
    return DEVICE_ERR_NULL;
  }
  
  /* 清空数据 */
  memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_FROMCAN_t));
  
  /* 复制参数 */
  gimbal_imu->param = *param;
  
  /* 设置设备类型 */
  gimbal_imu->header.online = false;
  
  /* 注册CAN ID到消息队列 */
  BSP_FDCAN_RegisterId(param->can, param->accl_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
  BSP_FDCAN_RegisterId(param->can, param->gyro_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
  BSP_FDCAN_RegisterId(param->can, param->eulr_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
  BSP_FDCAN_RegisterId(param->can, param->quat_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
  
  return DEVICE_OK;
}

/**
 * @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
 * @param gimbal_imu Gimbal IMU结构体指针
 * @return 更新结果
 *   - DEVICE_OK: 成功
 *   - DEVICE_ERR_NULL: 参数为空
 */
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu) {
  if (gimbal_imu == NULL) {
    return DEVICE_ERR_NULL;
  }
  
  BSP_FDCAN_Message_t msg;
  bool received = false;
  
  /* 读取所有可用的CAN消息 */
  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
    received = true;
  }
  
  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
    received = true;
  }
  
  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
    received = true;
  }
  
  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
    received = true;
  }
  
  /* 更新在线状态和时间戳 */
  if (received) {
    gimbal_imu->header.online = true;
    gimbal_imu->header.last_online_time = BSP_TIME_Get();
  } else {
    /* 超时检测 - 100ms */
    uint64_t now_time = BSP_TIME_Get();
    if (now_time - gimbal_imu->header.last_online_time > 100000) {  /* 100ms超时，单位微秒 */
      gimbal_imu->header.online = false;
    }
  }
  
  return DEVICE_OK;
}