#include "module/balance_chassis.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "component/user_math.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include <math.h>
#include <string.h>

/**
 * @brief 使能所有电机
 * @param c 底盘结构体指针
 * @return 
 */
static int8_t Chassis_MotorEnable(Chassis_t *c) {
    if (c == NULL) return CHASSIS_ERR_NULL;
    
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_Enable(&c->param->joint_motors[i]);
    }

    BSP_TIME_Delay_us(200);

    for (int i = 0; i < 2; i++) {
        MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
    }
    
    return CHASSIS_OK;
}


// /**
//  * @brief 关闭所有电机
//  * @param c 底盘结构体指针
//  * @return 
//  */
// static int8_t Chassis_MotorDisable(Chassis_t *c){
//     if (c == NULL) return CHASSIS_ERR_NULL;
    
//     for (int i = 0; i < 2; i++) {
//         MOTOR_LK_MotorOff(&c->param->wheel_motors[i]);
//     }
//     for (int i = 0; i < 4; i++) {
//         MOTOR_LZ_Disable(&c->param->joint_motors[i], true);
//     }
    
//     return CHASSIS_OK;
// }

static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
  if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */


    Chassis_MotorEnable(c);

    PID_Reset(&c->pid.leg_length[0]);
    PID_Reset(&c->pid.leg_length[1]);
    PID_Reset(&c->pid.yaw);
    PID_Reset(&c->pid.roll);
    PID_Reset(&c->pid.tp[0]);
    PID_Reset(&c->pid.tp[1]);

    c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;

    //清空位移
    c->chassis_state.position_x = 0.0f;
    c->chassis_state.velocity_x = 0.0f;
    c->chassis_state.last_velocity_x = 0.0f;
    c->chassis_state.target_x = 0.0f;


  LQR_Reset(&c->lqr[0]);
  LQR_Reset(&c->lqr[1]);
  c->mode = mode;
  c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化

  return CHASSIS_OK;
}

/* 更新机体状态估计 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
    if (c == NULL) return;
    
    // 从轮子编码器估计机体速度 (参考C++代码)
    float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
    float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒)
    
    // 将dps转换为rad/s
    float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
    float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
    
    float wheel_radius = 0.072f;
    
    float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
    float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
    
    // 机体x方向速度 (轮子中心速度)
    c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
    c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
    
    // 积分得到位置
    c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}


int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
    if (c == NULL || param == NULL || target_freq <= 0.0f) {
        return -1; // 参数错误
    }
    c->param = param;
    
    /*初始化can*/
    BSP_CAN_Init();

    /*初始化和注册所有电机*/
    MOTOR_LZ_Init();

    for (int i = 0; i < 4; i++) {
      MOTOR_LZ_Register(&c->param->joint_motors[i]);
    }
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_Register(&c->param->wheel_motors[i]);
    }

    // Chassis_MotorEnable(c);
    
    /*初始化VMC控制器*/
    VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
    VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);

    /*初始化pid*/
    PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
    PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
    PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
    PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
    PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
    PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);

    /*初始化LQR控制器*/
    LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
    LQR_SetGainMatrix(&c->lqr[0], &param->lqr_gains);
    LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
    LQR_SetGainMatrix(&c->lqr[1], &param->lqr_gains);
    
    /*初始化机体状态*/
    c->chassis_state.position_x = 0.0f;
    c->chassis_state.velocity_x = 0.0f;
    c->chassis_state.last_velocity_x = 0.0f;
    c->chassis_state.target_x = 0.0f;
    
    /*初始化yaw控制状态*/
    c->yaw_control.target_yaw = 0.0f;
    c->yaw_control.current_yaw = 0.0f;
    c->yaw_control.yaw_force = 0.0f;

    return CHASSIS_OK;
}

int8_t Chassis_UpdateFeedback(Chassis_t *c){
    if (c == NULL) {
        return -1; // 参数错误
    }
    /*更新电机反馈*/
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_Update(&c->param->joint_motors[i]);
    }
    MOTOR_LK_Update(&c->param->wheel_motors[0]);
    MOTOR_LK_Update(&c->param->wheel_motors[1]);
    
    /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
    // 更新关节电机反馈并检查离线状态
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
        if (joint_motor != NULL) {
            // c->feedback.joint[i] = joint_motor->motor.feedback;
            // // c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
            // if (c->feedback.joint[i].rotor_abs_angle > M_PI ){
            //     c->feedback.joint[i].rotor_abs_angle -= M_2PI;
            // }
            // c->feedback.joint[i].rotor_abs_angle = - c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
            if (joint_motor->motor.feedback.rotor_abs_angle > M_PI ){
                joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
            } 
            joint_motor->motor.feedback.rotor_abs_angle = - joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整

            c->feedback.joint[i] = joint_motor->motor.feedback;
        } 
    }
    
    // 更新轮子电机反馈并检查离线状态
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
        if (wheel_motor != NULL) {
            c->feedback.wheel[i] = wheel_motor->motor.feedback;
        }
    }
    
    // 更新机体状态估计
    Chassis_UpdateChassisState(c);
    
    return 0;
}

int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
    if (c == NULL) {
        return -1; // 参数错误
    }
    c->feedback.imu = imu;
    // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit;
    return 0;
}

int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
    if (c == NULL || c_cmd == NULL) {
        return CHASSIS_ERR_NULL; // 参数错误
    }
    c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
    c->lask_wakeup = BSP_TIME_Get_us();

    /*设置底盘模式*/
    if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
        return CHASSIS_ERR_MODE; // 设置模式失败
    }

    /*根据底盘模式执行不同的控制逻辑*/
    switch (c->mode) {
        case CHASSIS_MODE_RELAX:
            // 放松模式，电机不输出

            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LZ_Relax(&c->param->joint_motors[0]);
            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LZ_Relax(&c->param->joint_motors[1]);
            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LZ_Relax(&c->param->joint_motors[2]);
            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LZ_Relax(&c->param->joint_motors[3]);
            BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成
            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LK_Relax(&c->param->wheel_motors[0]);
            // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
            MOTOR_LK_Relax(&c->param->wheel_motors[1]);

            // 更新VMC正解算用于状态估计
            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);

            Chassis_LQRControl(c, c_cmd);

            break;

        case CHASSIS_MODE_RECOVER:
        {
            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            
            float fn;
            float tp1, tp2;
            fn = -20.0f;


            // tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
            // tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);

            VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
            VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
            VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
            VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
            // Chassis_MotorEnable(c);
            c->output.wheel[0] = 0.0f;
            c->output.wheel[1] = 0.0f;
            Chassis_Output(c); // 统一输出
        }
        break;
            
        case CHASSIS_MODE_WHELL_BALANCE:
            // 更新VMC正解算用于状态估计
            for (int i = 0; i < 4; i++) {
                c->output.joint[i].torque = 0.0f;
            }
            for (int i = 0; i < 2; i++) {
                c->output.wheel[i] = 0.0f;
            }

            // 更新VMC正解算用于状态估计
            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);

            // VMC_InverseSolve(&c->vmc_[1], fn, tp);
            // VMC_GetJointTorques(&c->vmc_[1], &t1, &t2);
            
            // c->output.joint[3].torque = t1;
            // c->output.joint[2].torque = t2;

            Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
            // c->output.wheel[0] = 0.2f;
            // c->output.wheel[1] = 0.2f;
            Chassis_Output(c); // 统一输出
            break;
            
        case CHASSIS_MODE_WHELL_LEG_BALANCE:
            // 轮腿平衡模式，使用LQR控制和PID腿长控制
            
            // 更新VMC正解算
            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
            
            // 执行LQR控制（包含PID腿长控制）
            if (Chassis_LQRControl(c, c_cmd) != 0) {
                // LQR控制失败，切换到安全模式
                return CHASSIS_ERR;
            }
            
            Chassis_Output(c); // 统一输出
            break;
            
        default:
            return CHASSIS_ERR_MODE;
    }
    
    return CHASSIS_OK;
}

void Chassis_Output(Chassis_t *c) {
    if (c == NULL) return;
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_MotionParam_t param = {0};
        param.torque = c->output.joint[i].torque;
        MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
    }
    BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
        // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
    }
    
}

int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    if (c == NULL || c_cmd == NULL) {
        return CHASSIS_ERR_NULL;
    }

    /* 参考C++版本实现的LQR控制 */
    
    /* 地面接触检测（参考C++版本） */
    float leg_fn[2];
    bool onground_flag[2];
    
    // 暂时关闭离地监测，强制设置为着地状态
    leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]);
    leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]);
    onground_flag[0] = true;  // 强制设置左腿着地
    onground_flag[1] = true;  // 强制设置右腿着地
    
    /* 获取VMC状态（等效摆杆参数） */
    float leg_L0[2], leg_theta[2], leg_d_theta[2];
    VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]);
    VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]);
    
    /* 运动参数（参考C++版本的状态估计） */
    static float xhat = 0.0f, x_dot_hat = 0.0f;
    float target_dot_x = 0.0f;
    
    // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
    x_dot_hat = c->chassis_state.velocity_x;
    xhat = c->chassis_state.position_x;
    
    // 目标设定
    target_dot_x = c_cmd->move_vec.vx*2;
    // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
    c->chassis_state.target_x += target_dot_x * c->dt;
    
    /* 分别计算左右腿的LQR控制 */
    float Tw[2] = {0.0f, 0.0f};  // 轮毂力矩
    float Tp[2] = {0.0f, 0.0f};  // 髋关节力矩
    
    for (int leg = 0; leg < 2; leg++) {
        /* 构建当前状态 */
        LQR_State_t current_state = {0};
        current_state.theta = leg_theta[leg];
        current_state.d_theta = leg_d_theta[leg];
        current_state.x = xhat;
        current_state.d_x = x_dot_hat;
        current_state.phi = -c->feedback.imu.euler.pit;
        current_state.d_phi = -c->feedback.imu.gyro.y;
        
        /* 构建目标状态 */
        LQR_State_t target_state = {0};
        target_state.theta = 0.0f; // 目标摆杆角度
        target_state.d_theta = 0.0f; // 目标摆杆角速度
        // target_state.x = 0; // 目标位置
        // target_state.d_x = 0.0f; // 目标速度
        target_state.phi = -0.1f; // 目标俯仰角
        target_state.d_phi = 0.0f; // 目标俯仰角速度
        // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
        // target_state.d_theta = 0.0f; // 目标摆杆角速度
        target_state.x = c->chassis_state.target_x; // 目标位置
        target_state.d_x = target_dot_x; // 目标速度
        // target_state.phi = 0.16f; // 目标俯仰角
        // target_state.d_phi = 0.0f; // 目标俯仰角速度
        
        /* 根据当前腿长更新增益矩阵 */
        LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]);
        
        /* 更新LQR状态 */
        LQR_SetTargetState(&c->lqr[leg], &target_state);
        LQR_UpdateState(&c->lqr[leg], &current_state);
        
        if (onground_flag[leg]) {
            /* 接地状态：使用LQR控制器计算输出 */
            if (LQR_Control(&c->lqr[leg]) == 0) {
                LQR_Input_t lqr_output = {0};
                if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) {
                    Tw[leg] = lqr_output.T;
                    Tp[leg] = lqr_output.Tp;
                    // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
                    // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
                } else {
                    Tw[leg] = 0.0f;
                    Tp[leg] = 0.0f;
                }
            } else {
                Tw[leg] = 0.0f;
                Tp[leg] = 0.0f;
            }
        } else {
            /* 离地状态：简化控制，只控制腿部摆动 */
            Tw[leg] = 0.0f;
            // 只控制摆杆角度
            float theta_error = current_state.theta - target_state.theta;
            float d_theta_error = current_state.d_theta - target_state.d_theta;
            Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error);
        }
    }

    c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;

    c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s

    // 修正目标yaw角度到[-pi, pi]
    if (c->yaw_control.target_yaw > M_PI) {
        c->yaw_control.target_yaw -= M_2PI;
    } else if (c->yaw_control.target_yaw < -M_PI) {
        c->yaw_control.target_yaw += M_2PI;
    }

    c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);
    
    
    /* 轮毂力矩输出（参考C++版本的减速比） */
    c->output.wheel[0] = Tw[0] / 4.5f - c->yaw_control.yaw_force;
    c->output.wheel[1] = Tw[1] / 4.5f + c->yaw_control.yaw_force;
    /* 腿长控制和VMC逆解算（使用PID控制） */
    float virtual_force[2];
    float target_L0[2];
    float leg_d_length[2]; // 腿长变化率
    
    /* 横滚角PID补偿计算 */
    float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt);
    
    // 目标腿长设定
    target_L0[0] = 0.15f + c_cmd->height * 0.2f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
    target_L0[1] = 0.15f + c_cmd->height * 0.2f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
    
    // 获取腿长变化率
    VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
    VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
    
    /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
    float Delta_Tp[2];
    // 使用tp_pid进行左右腿摆角同步控制
    // 左腿补偿力矩：使左腿theta向右腿theta靠拢
    Delta_Tp[0] = leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
    // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
    Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
    
    for (int leg = 0; leg < 2; leg++) {
        // 使用PID进行腿长控制
        float pid_output = PID_Calc(&c->pid.leg_length[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
        
        // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
        virtual_force[leg] = (Tp[leg]) * sinf(leg_theta[leg]) + 
                            pid_output + 25.0f;
                            //  + // PID腿长控制输出
                            // 45.0f; // 基础支撑力
        
        // VMC逆解算（包含摆角补偿）
        // virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败
        // Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
        // Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
        if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) {
        // if (VMC_InverseSolve(&c->vmc_[leg], 0.0, Tp[leg] + Delta_Tp[leg]) == 0) {
            if (leg == 0) {
                VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
            } else {
                VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
            }
        } else {
            // VMC失败，设为0力矩
            if (leg == 0) {
                c->output.joint[0].torque = 0.0f;
                c->output.joint[1].torque = 0.0f;
            } else {
                c->output.joint[2].torque = 0.0f;
                c->output.joint[3].torque = 0.0f;
            }
        }
    }
    
    /* 安全限制 */
    for (int i = 0; i < 2; i++) {
        if (fabsf(c->output.wheel[i]) > 0.8f) {
            c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
        }
    }
    
    for (int i = 0; i < 4; i++) {
        if (fabsf(c->output.joint[i].torque) > 20.0f) {
            c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
        }
    }
    
    return CHASSIS_OK;
}