/*
 * @file virtual_chassis_example.c
 * @brief 虚拟底盘设备使用示例
 * @details 展示如何配置和使用虚拟底盘与平衡底盘模块通信
 */

/* Includes ----------------------------------------------------------------- */
#include "device/virtual_chassis.h"
#include "bsp/can.h"

/* Private variables -------------------------------------------------------- */
static Virtual_Chassis_t g_virtual_chassis;

/* Example configuration ---------------------------------------------------- */

/**
 * @brief 虚拟底盘配置示例
 */
void Virtual_Chassis_Example_Init(void) {
    // 配置虚拟底盘参数
    Virtual_Chassis_Param_t chassis_param = {
        .motors = {
            .can = BSP_CAN_1,                    // 电机使用CAN1总线
            .enable_id = 121,                    // 使能命令ID
            .joint_cmd_id = 122,                 // 关节力矩控制命令ID
            .wheel_left_id = 128,                // 左轮控制命令ID
            .wheel_right_id = 129,               // 右轮控制命令ID
            .joint_feedback_base_id = 124,       // 关节反馈基础ID (124-127)
            .wheel_left_feedback_id = 130,       // 左轮反馈ID
            .wheel_right_feedback_id = 131,      // 右轮反馈ID
        },
        .imu = {
            .can = BSP_CAN_1,                    // IMU使用CAN1总线
            .accl_id = 0x301,                    // 加速度计数据ID (769)
            .gyro_id = 0x302,                    // 陀螺仪数据ID (770)
            .euler_id = 0x303,                   // 欧拉角数据ID (771)
            .quat_id = 0x304,                    // 四元数数据ID (772)
        }
    };
    
    // 初始化虚拟底盘
    Virtual_Chassis_Init(&g_virtual_chassis, &chassis_param);
}

/**
 * @brief 虚拟底盘控制示例
 */
void Virtual_Chassis_Example_Control(void) {
    // 1. 更新底盘数据（接收反馈）
    Virtual_Chassis_Update(&g_virtual_chassis);
    
    // 2. 使能关节电机
    Virtual_Chassis_Enable(&g_virtual_chassis);
    
    // 3. 发送关节电机力矩控制命令
    float joint_torques[4] = {0.5f, -0.3f, 0.2f, -0.1f}; // 4个关节的目标力矩 (Nm)
    Virtual_Chassis_SendJointTorque(&g_virtual_chassis, joint_torques);
    
    // 4. 发送轮子电机控制命令（示例：速度控制）
    uint8_t left_wheel_cmd[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};   // 左轮命令
    uint8_t right_wheel_cmd[8] = {0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01};  // 右轮命令
    Virtual_Chassis_SendWheelCommand(&g_virtual_chassis, left_wheel_cmd, right_wheel_cmd);
}

/**
 * @brief 虚拟底盘数据读取示例
 */
void Virtual_Chassis_Example_ReadData(void) {
    // 读取关节电机反馈数据
    for (int i = 0; i < 4; i++) {
        MOTOR_Feedback_t *joint = &g_virtual_chassis.data.joint[i];
        // 使用 joint->torque_current, joint->rotor_abs_angle, joint->rotor_speed
    }
    
    // 读取轮子电机反馈数据
    for (int i = 0; i < 2; i++) {
        MOTOR_Feedback_t *wheel = &g_virtual_chassis.data.wheel[i];
        // 使用 wheel->torque_current, wheel->rotor_abs_angle, wheel->rotor_speed
    }
    
    // 读取IMU数据
    AHRS_Accl_t *accl = &g_virtual_chassis.data.imu.accl;
    AHRS_Gyro_t *gyro = &g_virtual_chassis.data.imu.gyro;
    AHRS_Eulr_t *euler = &g_virtual_chassis.data.imu.euler;
    AHRS_Quaternion_t *quat = &g_virtual_chassis.data.imu.quat;
    
    // 使用IMU数据进行姿态估计或控制
}

/**
 * @brief 获取虚拟底盘实例（供其他模块使用）
 */
Virtual_Chassis_t* Virtual_Chassis_GetInstance(void) {
    return &g_virtual_chassis;
}