#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include "component/user_math.h"
#include "device/motor_lz.h"
#include "device/virtual_chassis.h"
#include <math.h>
#include <stddef.h>
#include <string.h>

static Virtual_Chassis_t virtual_chassis;

/**
 * @brief 使能所有电机
 * @param c 底盘结构体指针
 * @return
 */
static int8_t Chassis_MotorEnable(Chassis_t *c) {
  if (c == NULL)
    return CHASSIS_ERR_NULL;

  Virtual_Chassis_Enable(&virtual_chassis);

  return CHASSIS_OK;
}

static int8_t Chassis_MotorRelax(Chassis_t *c) {
  if (c == NULL)
    return CHASSIS_ERR_NULL;
  float relax_torque[4] = {0.0f, 0.0f, 0.0f, 0.0f};
  Virtual_Chassis_SendJointTorque(&virtual_chassis, relax_torque);
  Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f);
  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_MODE_JUMP和CHASSIS_MODE_WHELL_LEG_BALANCE之间切换时不重置
  if ((c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) ||
      (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP)) {
    c->mode = 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;
  c->chassis_state.target_velocity_x = 0.0f;
  c->chassis_state.last_target_velocity_x = 0.0f;

  LQR_Reset(&c->lqr[0]);
  LQR_Reset(&c->lqr[1]);

  LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f);
  LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f);
  LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f);
  LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f);
  LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
  LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);

  c->mode = mode;
  c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
  c->jump_time=0;
  c->wz_multi=0.0f;

  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;

  c->mode = CHASSIS_MODE_RELAX;

  /*初始化can*/
  BSP_CAN_Init();

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

  Virtual_Chassis_Init(&virtual_chassis, &c->param->virtual_chassis_param);
  MOTOR_DM_Register(&c->param->yaw_motor);

  /*初始化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_gains);
  LQR_Init(&c->lqr[1], &param->lqr_gains);

  LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq,
                       param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq,
                       param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq,
                       param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq,
                       param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq,
                       param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq,
                       param->low_pass_cutoff_freq.out);

  /*初始化机体状态*/
  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;
  c->chassis_state.target_velocity_x = 0.0f;
  c->chassis_state.last_target_velocity_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; // 参数错误
  }
  Virtual_Chassis_Update(&virtual_chassis);

  for (int i = 0; i < 4; i++) {
    c->feedback.joint[i] = virtual_chassis.data.joint[i];
    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; // 机械零点调整
  }
  for (int i = 0; i < 2; i++) {
    c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
  }
  c->feedback.imu.accl = virtual_chassis.data.imu.accl;
  c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
  c->feedback.imu.euler = virtual_chassis.data.imu.euler;

  // 更新机体状态估计
  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;
  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; // 设置模式失败
  }

  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);

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

    Chassis_LQRControl(c, c_cmd);

    break;

  case CHASSIS_MODE_RECOVER: {
    float fn, tp;
    fn = -20.0f;
    tp = 0.0f;

    Chassis_LQRControl(c, c_cmd);

    VMC_InverseSolve(&c->vmc_[0], fn, tp);
    VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
    VMC_InverseSolve(&c->vmc_[1], fn, tp);
    VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);

    c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;

    c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
    Chassis_Output(c); // 统一输出

  } break;
  case CHASSIS_MODE_JUMP:
    // 跳跃模式状态机
    // 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
    // jump_time == 0: 未开始跳跃
    // jump_time == 1: 已完成跳跃（不再触发）
    // jump_time > 1: 跳跃进行中
    
    // 检测是否需要开始新的跳跃（state为0且jump_time为0）
    if (c->state == 0 && c->jump_time == 0) {
      // 开始跳跃，进入state 1
      c->state = 1;
      c->jump_time = BSP_TIME_Get_us();
    }
    
    // 只有在跳跃进行中时才处理状态转换（jump_time > 1）
    if (c->jump_time > 1) {
      // 计算已经过的时间（微秒）
      uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
      
      // 状态转换逻辑
      if (c->state == 1 && elapsed_us >= 300000) {
        // state 1 保持0.3s后转到state 2
        c->state = 2;
        c->jump_time = BSP_TIME_Get_us(); // 重置计时
      } else if (c->state == 2 && elapsed_us >= 200000) {
        // state 2 保持0.2s后转到state 3
        c->state = 3;
        c->jump_time = BSP_TIME_Get_us(); // 重置计时
      } else if (c->state == 3 && elapsed_us >= 300000) {
        // state 3 保持0.3s后转到state 0
        c->state = 0;
        c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
      }
    }
    
    // 执行LQR控制（包含PID腿长控制）
    Chassis_LQRControl(c, c_cmd);
    Chassis_Output(c); // 统一输出
    break;
  case CHASSIS_MODE_WHELL_LEG_BALANCE:
  case CHASSIS_MODE_ROTOR:

    // 执行LQR控制（包含PID腿长控制）
    Chassis_LQRControl(c, c_cmd);
    Chassis_Output(c); // 统一输出
    break;

    break;
  default:
    return CHASSIS_ERR_MODE;
  }

  return CHASSIS_OK;
}

void Chassis_Output(Chassis_t *c) {
  if (c == NULL)
    return;
  c->output.joint[0] =
      LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]);
  c->output.joint[1] =
      LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]);
  c->output.joint[2] =
      LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
  c->output.joint[3] =
      LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
  float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
                  c->output.joint[3]};
  Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
  c->output.wheel[0] =
      LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
  c->output.wheel[1] =
      LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
  Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
                                   c->output.wheel[1]);
  // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
  //                                  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++版本的状态估计） */
  static float xhat = 0.0f, x_dot_hat = 0.0f;
  float target_dot_x = 0.0f;
  float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s²

  // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
  x_dot_hat = c->chassis_state.velocity_x;
  xhat = c->chassis_state.position_x;

  // 目标速度设定（原始期望速度）
  float desired_velocity = c_cmd->move_vec.vx * 2;

  // 加速度限制处理
  float velocity_change =
      desired_velocity - c->chassis_state.last_target_velocity_x;
  float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化

  // 限制速度变化率（加速度限制）
  if (velocity_change > max_velocity_change) {
    velocity_change = max_velocity_change;
  } else if (velocity_change < -max_velocity_change) {
    velocity_change = -max_velocity_change;
  }

  // 应用加速度限制后的目标速度
  target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
  c->chassis_state.target_velocity_x = target_dot_x;
  c->chassis_state.last_target_velocity_x = target_dot_x;

  // 更新目标位置
  c->chassis_state.target_x += target_dot_x * c->dt;

  /* 分别计算左右腿的LQR控制 */
  /* 构建当前状态 */
  LQR_State_t current_state = {0};
  current_state.theta = c->vmc_[0].leg.theta;
  current_state.d_theta = c->vmc_[0].leg.d_theta;
  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_UpdateState(&c->lqr[0], &current_state);

  current_state.theta = c->vmc_[1].leg.theta;
  current_state.d_theta = c->vmc_[1].leg.d_theta;
  LQR_UpdateState(&c->lqr[1], &current_state);

  /* 根据当前腿长更新增益矩阵 */
  LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
  LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);

  /* 构建目标状态 */
  LQR_State_t target_state = {0};
  target_state.theta = c->param->theta;                  // 目标摆杆角度
  target_state.d_theta = 0.0f;                // 目标摆杆角速度
  target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154;                   // 目标俯仰角
  target_state.d_phi = 0.0f;                  // 目标俯仰角速度
  target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
  target_state.d_x = target_dot_x;            // 目标速度

  if (c->mode != CHASSIS_MODE_ROTOR){
  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
  c->yaw_control.target_yaw =
      c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2;
  c->yaw_control.yaw_force =
      PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
               c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
  }else{
    CircleAdd(&c->wz_multi,0.000001, M_2PI);
    c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
    CircleAdd(&c->yaw_control.target_yaw,c->wz_multi, M_2PI);
    c->yaw_control.yaw_force =
        PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                 c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
  }

  if (c->vmc_[0].leg.is_ground_contact) {
      /* 更新LQR状态 */
    LQR_SetTargetState(&c->lqr[0], &target_state);
    LQR_Control(&c->lqr[0]);
  } else {
    target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
    LQR_SetTargetState(&c->lqr[0], &target_state);
    c->lqr[0].control_output.T = 0.0f;
    c->lqr[0].control_output.Tp =
        c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) +
        c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
    c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
  }
  if (c->vmc_[1].leg.is_ground_contact){
    LQR_SetTargetState(&c->lqr[1], &target_state);
    LQR_Control(&c->lqr[1]);
  }else {
    target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
    LQR_SetTargetState(&c->lqr[1], &target_state);
    c->lqr[1].control_output.T = 0.0f;
    c->lqr[1].control_output.Tp =
        c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
        c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
    c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
  }

  /* 轮毂力矩输出（参考C++版本的减速比） */
  c->output.wheel[0] =
      c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
  c->output.wheel[1] =
      c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
  /* 腿长控制和VMC逆解算（使用PID控制） */
  float virtual_force[2];
  float target_L0[2];
  float leg_d_length[2]; // 腿长变化率

  /* 横滚角PID补偿计算 */
  // 使用PID控制器计算横滚角补偿力，目标横滚角为0
  float roll_compensation_force =
      PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
               c->feedback.imu.gyro.x, c->dt);

  // 限制补偿力范围，防止过度补偿
  if (roll_compensation_force > 20.0f)
    roll_compensation_force = 20.0f;
  if (roll_compensation_force < -20.0f)
    roll_compensation_force = -20.0f;

  // 目标腿长设定（移除横滚角补偿）
  switch (c->state) {
  case 0: // 正常状态，根据高度指令调节腿长
    target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
    target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
    break;
  case 1: // 准备阶段，腿长收缩
    target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
    target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
    break;
  case 2: // 起跳阶段，腿长快速伸展
    target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
    target_L0[1] = 0.55f; // 右腿：伸展到最大腿长
    break;
  case 3: // 着地阶段，腿长缓冲
    target_L0[0] = 0.1f; // 左腿：缓冲腿长
    target_L0[1] = 0.1f; // 右腿：缓冲腿长
    break;
  default:
    target_L0[0] = 0.15f + c_cmd->height * 0.22f;
    target_L0[1] = 0.15f + c_cmd->height * 0.22f;
    break;
  }

  // 获取腿长变化率
  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] = c->vmc_[0].leg.L0 * 10.0f *
                PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
                         c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
  // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
  Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
                PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
                         c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);

  // 左腿控制
  {
    // 使用PID进行腿长控制
    float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
                                c->vmc_[0].leg.L0, leg_d_length[0], c->dt);

    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 -
    // 横滚角补偿力（左腿减少支撑力）
    virtual_force[0] =
        (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
        pid_output + 55.0f + roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
                         c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
                          &c->output.joint[1]);

    } else {
      // VMC失败，设为0力矩
      c->output.joint[0] = 0.0f;
      c->output.joint[1] = 0.0f;
    }
  }

  // 右腿控制
  {
    // 使用PID进行腿长控制
    float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
                                c->vmc_[1].leg.L0, leg_d_length[1], c->dt);

    // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 +
    // 横滚角补偿力（右腿增加支撑力）
    virtual_force[1] =
        (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
        pid_output + 60.0f - roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
                         c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
                          &c->output.joint[2]);
    } else {
      // VMC失败，设为0力矩
      c->output.joint[2] = 0.0f;
      c->output.joint[3] = 0.0f;
    }
  }

  /* 安全限制 */
  for (int i = 0; i < 2; i++) {
    if (fabsf(c->output.wheel[i]) > 1.0f) {
      c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f;
    }
  }

  for (int i = 0; i < 4; i++) {
    if (fabsf(c->output.joint[i]) > 20.0f) {
      c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
    }
  }

  return CHASSIS_OK;
}