/*
 * @file virtual_chassis.c
 * @brief 虚拟底盘设备驱动实现 - 控制端
 * @details 作为控制端，发送控制命令到底盘，接收底盘反馈数据
 */

/* Includes ----------------------------------------------------------------- */
#include "device/virtual_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include <string.h>
#include <math.h>

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */

/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
/**
 * @brief 解析关节电机反馈数据 (ID: 124-127)
 * @param chassis 虚拟底盘结构体
 * @param id CAN ID
 * @param data 数据包
 * @return 解析结果
 */
static int8_t Virtual_Chassis_DecodeJointMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
    if (chassis == NULL || data == NULL) {
        return DEVICE_ERR_NULL;
    }

    int motor_idx = id - chassis->param.motors.joint_feedback_base_id;
    if (motor_idx < 0 || motor_idx >= 4) {
        return DEVICE_ERR;
    }

    // 解析关节电机反馈数据：转矩电流(2字节) + 位置(3字节) + 速度(3字节)
    
    // 1. 解析转矩电流 - 前2字节，16位有符号整数 (精度0.01 Nm)
    int16_t torque_int;
    memcpy(&torque_int, &data[0], sizeof(int16_t));
    chassis->data.joint[motor_idx].torque_current = (float)torque_int / 100.0f;
    
    // 2. 解析位置 - 第3-5字节，24位有符号整数 (精度0.0001 rad)
    int32_t angle_int = 0;
    angle_int |= ((int32_t)data[2]) << 16;
    angle_int |= ((int32_t)data[3]) << 8;
    angle_int |= ((int32_t)data[4]);
    
    // 符号扩展（24位转32位）
    if (angle_int & 0x800000) {
        angle_int |= 0xFF000000;
    }
    chassis->data.joint[motor_idx].rotor_abs_angle = (float)angle_int / 10000.0f;
    
    // 3. 解析速度 - 第6-8字节，24位有符号整数 (精度0.001 rad/s)
    int32_t velocity_int = 0;
    velocity_int |= ((int32_t)data[5]) << 16;
    velocity_int |= ((int32_t)data[6]) << 8;
    velocity_int |= ((int32_t)data[7]);
    
    // 符号扩展（24位转32位）
    if (velocity_int & 0x800000) {
        velocity_int |= 0xFF000000;
    }
    chassis->data.joint[motor_idx].rotor_speed = (float)velocity_int / 1000.0f;
    
    chassis->data.joint[motor_idx].temp = 0.0f;
    
    return DEVICE_OK;
}

/**
 * @brief 解析轮子电机反馈数据 (ID: 130-131)
 * @param chassis 虚拟底盘结构体
 * @param id CAN ID
 * @param data 数据包
 * @return 解析结果
 */
static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
    if (chassis == NULL || data == NULL) {
        return DEVICE_ERR_NULL;
    }

    int wheel_idx;
    if (id == chassis->param.motors.wheel_left_feedback_id) {
        wheel_idx = 0;  // 左轮
    } else if (id == chassis->param.motors.wheel_right_feedback_id) {
        wheel_idx = 1;  // 右轮
    } else {
        return DEVICE_ERR;
    }

    // 解析轮子电机反馈数据：角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节)
    
    // 角度 - 精度0.01度，需转换为弧度
    int16_t angle_int;
    angle_int = (data[0] << 8) | data[1];
    chassis->data.wheel[wheel_idx].rotor_abs_angle = (float)angle_int / 100.0f * M_PI / 180.0f;
    
    // 速度 - 精度1dps，直接赋值
    int16_t speed_int;
    speed_int = (data[2] << 8) | data[3];
    chassis->data.wheel[wheel_idx].rotor_speed = (float)speed_int;
    
    // 力矩电流
    int16_t current_int;
    current_int = (data[4] << 8) | data[5];
    chassis->data.wheel[wheel_idx].torque_current = (float)current_int;
    
    // 编码器值（暂不使用）
    chassis->data.wheel[wheel_idx].temp = 0.0f;
    
    return DEVICE_OK;
}

/* 数据范围定义 - 与发送端一致 */
#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       (M_PI_2)    /* π/2 弧度 ≈ 90° */
#define PITCH_CAN_MIN       (-M_PI_2)   /* -π/2 弧度 ≈ -90° */
#define ROLL_CAN_MAX        (M_PI)      /* π 弧度 ≈ 180° */
#define ROLL_CAN_MIN        (-M_PI)     /* -π 弧度 ≈ -180° */
#define YAW_CAN_MAX         (M_PI)      /* π 弧度 ≈ 180° */
#define YAW_CAN_MIN         (-M_PI)     /* -π 弧度 ≈ -180° */
#define QUATERNION_MIN      (-1.0f)
#define QUATERNION_MAX      (1.0f)

/* 反向映射宏：将16位整数还原为浮点值 */
#define UNMAP_FROM_INT16(int_val, min, max) \
    (((float)(int_val) + 32768.0f) / 65535.0f * ((max) - (min)) + (min))

/**
 * @brief 解析IMU数据
 * @param chassis 虚拟底盘结构体
 * @param id CAN ID
 * @param data 数据包
 * @return 解析结果
 */
static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
    if (chassis == NULL || data == NULL) {
        return DEVICE_ERR_NULL;
    }

    if (id == chassis->param.imu.accl_id) { 
        // 加速度计数据 - 每轴使用16位编码 (6字节总共)
        int16_t ax_int, ay_int, az_int;
        
        memcpy(&ax_int, &data[0], sizeof(int16_t));
        memcpy(&ay_int, &data[2], sizeof(int16_t));
        memcpy(&az_int, &data[4], sizeof(int16_t));
        
        // 反向映射到原始浮点值
        chassis->data.imu.accl.x = UNMAP_FROM_INT16(ax_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
        chassis->data.imu.accl.y = UNMAP_FROM_INT16(ay_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
        chassis->data.imu.accl.z = UNMAP_FROM_INT16(az_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
    }
    else if (id == chassis->param.imu.gyro_id) { 
        // 陀螺仪数据 - 每轴使用16位编码 (6字节总共)
        int16_t gx_int, gy_int, gz_int;
        
        memcpy(&gx_int, &data[0], sizeof(int16_t));
        memcpy(&gy_int, &data[2], sizeof(int16_t));
        memcpy(&gz_int, &data[4], sizeof(int16_t));
        
        // 反向映射到原始浮点值
        chassis->data.imu.gyro.x = UNMAP_FROM_INT16(gx_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
        chassis->data.imu.gyro.y = UNMAP_FROM_INT16(gy_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
        chassis->data.imu.gyro.z = UNMAP_FROM_INT16(gz_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
    }
    else if (id == chassis->param.imu.euler_id) { 
        // 欧拉角数据 - 每角使用16位编码 (6字节总共)
        int16_t yaw_int, pitch_int, roll_int;
        
        memcpy(&yaw_int, &data[0], sizeof(int16_t));
        memcpy(&pitch_int, &data[2], sizeof(int16_t));
        memcpy(&roll_int, &data[4], sizeof(int16_t));
        
        // 反向映射到原始浮点值
        chassis->data.imu.euler.yaw = UNMAP_FROM_INT16(yaw_int, YAW_CAN_MIN, YAW_CAN_MAX);
        chassis->data.imu.euler.pit = UNMAP_FROM_INT16(pitch_int, PITCH_CAN_MIN, PITCH_CAN_MAX);
        chassis->data.imu.euler.rol = UNMAP_FROM_INT16(roll_int, ROLL_CAN_MIN, ROLL_CAN_MAX);
    }
    else if (id == chassis->param.imu.quat_id) { 
        // 四元数数据 - 每个分量使用16位编码 (8字节总共)
        int16_t q0_int, q1_int, q2_int, q3_int;
        
        memcpy(&q0_int, &data[0], sizeof(int16_t));
        memcpy(&q1_int, &data[2], sizeof(int16_t));
        memcpy(&q2_int, &data[4], sizeof(int16_t));
        memcpy(&q3_int, &data[6], sizeof(int16_t));
        
        // 反向映射到原始浮点值
        chassis->data.imu.quat.q0 = UNMAP_FROM_INT16(q0_int, QUATERNION_MIN, QUATERNION_MAX);
        chassis->data.imu.quat.q1 = UNMAP_FROM_INT16(q1_int, QUATERNION_MIN, QUATERNION_MAX);
        chassis->data.imu.quat.q2 = UNMAP_FROM_INT16(q2_int, QUATERNION_MIN, QUATERNION_MAX);
        chassis->data.imu.quat.q3 = UNMAP_FROM_INT16(q3_int, QUATERNION_MIN, QUATERNION_MAX);
    }
    else {
        return DEVICE_ERR;
    }

    return DEVICE_OK;
}

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

/**
 * @brief 初始化虚拟底盘设备
 */
int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param) {
    if (chassis == NULL || param == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 复制参数配置
    chassis->param = *param;
    
    // 初始化设备头部
    chassis->header.online = false;
    chassis->header.last_online_time = 0;
    
    // 初始化数据
    memset(&chassis->data, 0, sizeof(Virtual_Chassis_Feedback_t));
    memset(&chassis->output, 0, sizeof(Virtual_Chassis_Output_t));

    BSP_CAN_Init();

    // 注册关节电机反馈ID (124-127)
    for (int i = 0; i < 4; i++) {
        BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.joint_feedback_base_id + i, 3);
    }
    
    // 注册轮子电机反馈ID (130-131)
    BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, 3);
    BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, 3);
    
    // 注册IMU数据ID (769-772 / 0x301-0x304)
    BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.accl_id, 3);
    BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.gyro_id, 3);
    BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.euler_id, 3);
    BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.quat_id, 3);
    
    // 设置设备在线状态
    chassis->header.online = true;
    chassis->header.last_online_time = BSP_TIME_Get_us();
    
    return DEVICE_OK;
}


/**
 * @brief 更新虚拟底盘数据（接收底盘反馈数据）
 */
int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_Message_t msg;

    // 接收IMU数据
    if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.accl_id, msg.data);
    }
    if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.gyro_id, msg.data);
    }
    if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.euler_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.euler_id, msg.data);
    }
    if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.quat_id, msg.data);
    }

    // 接收关节电机反馈数据 (124-127)
    for (int i = 0; i < 4; i++) {
        uint16_t joint_id = chassis->param.motors.joint_feedback_base_id + i;
        if (BSP_CAN_GetMessage(chassis->param.motors.can, joint_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
            Virtual_Chassis_DecodeJointMotor(chassis, joint_id, msg.data);
        }
    }

    // 接收轮子电机反馈数据 (130-131)
    if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_left_feedback_id, msg.data);
    }
    if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_right_feedback_id, msg.data);
    }
    
    return DEVICE_OK;
}


/**
 * @brief 发送电机使能命令
 */
int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t enable_frame = {
        .id = chassis->param.motors.enable_id,
        .dlc = 0,
        .data = {0}
    };
    
    chassis->output.enable_joints = true;
    
    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) {
        return DEVICE_OK;
    }
    
    return DEVICE_ERR;
}

/**
 * @brief 发送关节电机力矩控制命令
 */
int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques[4]) {
    if (chassis == NULL || torques == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t torque_frame = {
        .id = chassis->param.motors.joint_cmd_id,
        .dlc = 8,
        .data = {0}
    };
    
    // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
    for (int i = 0; i < 4; i++) {
        int16_t torque_raw = (int16_t)(torques[i] * 100.0f);
        memcpy(&torque_frame.data[i * 2], &torque_raw, sizeof(int16_t));
        chassis->output.joint_torque[i] = torques[i];
    }
    
    return BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &torque_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

/**
 * @brief 发送轮子电机控制命令
 */
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    int16_t torque_control = (int16_t)(left_out * 2048);
        // 构建CAN帧
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = chassis->param.motors.wheel_left_id;
    tx_frame.dlc = 8;

    tx_frame.data[0] = 0xA1;
    tx_frame.data[1] = 0x00;
    tx_frame.data[2] = 0x00;
    tx_frame.data[3] = 0x00;
    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
    tx_frame.data[6] = 0x00;
    tx_frame.data[7] = 0x00;
    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
        return DEVICE_ERR;
    }
    torque_control = (int16_t)(right_out * 2048);
        // 构建CAN帧
    tx_frame.id = chassis->param.motors.wheel_right_id;
    tx_frame.dlc = 8;
    tx_frame.data[0] = 0xA1;
    tx_frame.data[1] = 0x00;
    tx_frame.data[2] = 0x00;
    tx_frame.data[3] = 0x00;
    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
    tx_frame.data[6] = 0x00;
    tx_frame.data[7] = 0x00;
    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
        return DEVICE_ERR;
    }
    
    return DEVICE_OK;
}


/* Private functions -------------------------------------------------------- */