#pragma once

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "component/ahrs.h"
#include "bsp/can.h"

/* Exported constants ------------------------------------------------------- */


/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */

/* 虚拟底盘反馈数据结构体 */
typedef struct {
    MOTOR_Feedback_t joint[4];      // 4个关节电机反馈数据
    MOTOR_Feedback_t wheel[2];      // 2个轮子电机反馈数据
    struct {
        DEVICE_Header_t header;     // 设备通用头部
        AHRS_Accl_t accl;          // 加速度计数据
        AHRS_Gyro_t gyro;          // 陀螺仪数据
        AHRS_Eulr_t euler;         // 欧拉角数据
        AHRS_Quaternion_t quat;    // 四元数数据
    } imu;
} Virtual_Chassis_Feedback_t;

/* 虚拟底盘输出控制结构体 */
typedef struct {
    float joint_torque[4];          // 4个关节电机目标力矩
    uint8_t wheel_left_cmd[8];      // 左轮控制命令数据
    uint8_t wheel_right_cmd[8];     // 右轮控制命令数据
    bool enable_joints;             // 关节使能标志
} Virtual_Chassis_Output_t;

/* 电机CAN参数结构体 */
typedef struct {
    BSP_CAN_t can;              // 电机所在CAN总线
    uint16_t enable_id;         // 电机使能命令CAN ID (121)
    uint16_t joint_cmd_id;      // 关节力矩控制命令CAN ID (122)
    uint16_t wheel_left_id;     // 左轮控制命令CAN ID (128)
    uint16_t wheel_right_id;    // 右轮控制命令CAN ID (129)
    uint16_t joint_feedback_base_id;  // 关节反馈基础ID (124-127)
    uint16_t wheel_left_feedback_id;  // 左轮反馈ID (130)
    uint16_t wheel_right_feedback_id; // 右轮反馈ID (131)
} Virtual_Chassis_MotorParam_t;

/* IMU参数结构体 */
typedef struct {
    BSP_CAN_t can;              // IMU所在CAN总线
    uint16_t accl_id;           // 加速度计数据CAN ID (0x301/769)
    uint16_t gyro_id;           // 陀螺仪数据CAN ID (0x302/770)
    uint16_t euler_id;          // 欧拉角数据CAN ID (0x303/771)
    uint16_t quat_id;           // 四元数数据CAN ID (0x304/772)
} Virtual_Chassis_IMUParam_t;

/* 虚拟底盘参数配置结构体 */
typedef struct {
    Virtual_Chassis_MotorParam_t motors; // 4个电机CAN参数
    Virtual_Chassis_IMUParam_t imu;          // IMU CAN参数
} Virtual_Chassis_Param_t;

/* 虚拟底盘设备结构体 */
typedef struct {
    DEVICE_Header_t header;                      // 设备通用头部
    Virtual_Chassis_Param_t param;               // 参数配置
    Virtual_Chassis_Feedback_t data;             // 反馈数据
    Virtual_Chassis_Output_t output;             // 控制输出
} Virtual_Chassis_t;

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

/**
 * @brief 初始化虚拟底盘设备
 * @param chassis 虚拟底盘设备结构体指针
 * @param param 虚拟底盘参数配置指针
 * @return DEVICE_OK 成功，其他值失败
 */
int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param);

/**
 * @brief 更新虚拟底盘数据（包括电机和IMU数据）
 * @param chassis 虚拟底盘设备结构体指针
 * @return DEVICE_OK 成功，其他值失败
 */
int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis);

/**
 * @brief 发送电机使能命令
 * @param chassis 虚拟底盘设备结构体指针
 * @return DEVICE_OK 成功，其他值失败
 */
int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis);

/**
 * @brief 发送关节电机力矩控制命令
 * @param chassis 虚拟底盘设备结构体指针
 * @param torques 4个关节电机的目标力矩数组
 * @return DEVICE_OK 成功，其他值失败
 */
int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques[4]);

/**
 * @brief 发送轮子电机控制命令
 * @param chassis 虚拟底盘设备结构体指针
 * @param left_cmd 左轮控制命令数据（8字节）
 * @param right_cmd 右轮控制命令数据（8字节）
 * @return DEVICE_OK 成功，其他值失败
 */
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out);

#ifdef __cplusplus
}
#endif