/**
 * @file balance_chassis.c
 * @brief 平衡底盘控制模块
 */

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

/* Private defines ---------------------------------------------------------- */
#define LIMIT(x, min, max)  ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))

#define WHEEL_RADIUS        0.068f   /* 轮子半径 (m) */
#define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO    8.0f     /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH     0.12f    /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE    0.22f    /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */
#define MAX_LEG_LENGTH      0.42f    /* 最大腿长 (m) */
#define NON_CONTACT_THETA   0.16f    /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE     60.0f    /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE    60.0f    /* 右腿基础支撑力 (N) */

/* Private function prototypes ---------------------------------------------- */
static int8_t Chassis_MotorEnable(Chassis_t *c);
static int8_t Chassis_MotorRelax(Chassis_t *c);
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode);
static void Chassis_UpdateChassisState(Chassis_t *c);
static void Chassis_ResetControllers(Chassis_t *c);
static void Chassis_SelectYawZeroPoint(Chassis_t *c);

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

/**
 * @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_param[i]);
  }
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_MotorOn(&c->param->wheel_param[i]);
  }
  return CHASSIS_OK;
}

/**
 * @brief 放松所有电机
 * @param c 底盘结构体指针
 * @return 操作结果
 */
static int8_t Chassis_MotorRelax(Chassis_t *c) {
  if (c == NULL) return CHASSIS_ERR_NULL;

  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_Relax(&c->param->joint_param[i]);
  }
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_Relax(&c->param->wheel_param[i]);
  }
  return CHASSIS_OK;
}

/**
 * @brief 重置所有控制器
 * @param c 底盘结构体指针
 */
static void Chassis_ResetControllers(Chassis_t *c) {
  /* 重置PID控制器 */
  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]);

  /* 重置LQR控制器 */
  LQR_Reset(&c->lqr[0]);
  LQR_Reset(&c->lqr[1]);

  /* 重置滤波器 */
  for (int i = 0; i < 4; i++) {
    LowPassFilter2p_Reset(&c->filter.joint_out[i], 0.0f);
  }
  for (int i = 0; i < 2; i++) {
    LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
  }

  /* 清空机体状态 */
  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;
}

/**
 * @brief 选择yaw轴零点（双零点自动选择）
 * @param c 底盘结构体指针
 */
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
  float current_yaw = c->feedback.yaw.rotor_abs_angle;
  float zero_point_1 = c->param->mech_zero_yaw;
  float zero_point_2 = zero_point_1 + M_PI;

  float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
  float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);

  if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
    c->yaw_control.target_yaw = zero_point_1;
    c->yaw_control.is_reversed = false;
  } else {
    c->yaw_control.target_yaw = zero_point_2;
    c->yaw_control.is_reversed = true;
  }
}

/**
 * @brief 设置底盘模式
 * @param c 底盘结构体指针
 * @param mode 目标模式
 * @return 操作结果
 */
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);
  Chassis_ResetControllers(c);
  Chassis_SelectYawZeroPoint(c);

  c->mode = mode;
  return CHASSIS_OK;
}

/**
 * @brief 更新机体状态估计
 * @param c 底盘结构体指针
 * @note 基于轮腿机器人运动学的精确速度估计算法
 *       综合考虑轮子速度、关节运动和机体姿态的影响
 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
  if (c == NULL) return;

  /* 获取轮子原始速度数据 (dps -> rad/s) */
  float left_wheel_speed = c->feedback.wheel[0].rotor_speed * (M_PI / 180.0f);
  float right_wheel_speed = c->feedback.wheel[1].rotor_speed * (M_PI / 180.0f);

  /* 获取关节速度和陀螺仪数据 (dps -> rad/s) */
  float left_joint_speed = c->feedback.joint[1].rotor_speed * (M_PI / 180.0f);   /* 左前关节 */
  float right_joint_speed = c->feedback.joint[2].rotor_speed * (M_PI / 180.0f);  /* 右前关节 */
  float pitch_gyro = c->feedback.imu.gyro.y;  /* 俯仰角速度 rad/s */

  /*
   * 轮子角速度补偿算法
   * 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰
   * 公式来源：五连杆机构运动学分析
   */
  float left_w_wheel_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
  float right_w_wheel_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;

  /*
   * 机体速度计算算法
   * 综合考虑：
   * 1. 轮子接触点线速度：omega_wheel * r_wheel
   * 2. 摆杆运动贡献：L0 * d_theta (摆杆角速度在x方向的分量)
   * 3. 腿长变化贡献：d_L0 * sin(theta) (腿长变化在x方向的分量)
   */
  float left_v_body = left_w_wheel_compensated * WHEEL_RADIUS +
                      c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta +
                      c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);

  float right_v_body = right_w_wheel_compensated * WHEEL_RADIUS +
                       c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta +
                       c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);

  /* 保存历史速度 */
  c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;

  /* 计算平均速度 */
  float vel_measured = (left_v_body + right_v_body) / 2.0f;

  /*
   * 接地状态检测与速度修正
   * 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0
   */
  if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
    vel_measured = 0.0f;
  }

  /* 使用二阶低通滤波器平滑速度估计 */
  c->chassis_state.velocity_x = LowPassFilter2p_Apply(&c->filter.velocity_est, vel_measured);

  /* 位置积分更新 */
  c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}

/* Public functions --------------------------------------------------------- */

/**
 * @brief 初始化底盘
 * @param c 底盘结构体指针
 * @param param 底盘参数指针
 * @param target_freq 目标控制频率
 * @return 操作结果
 */
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  if (c == NULL || param == NULL || target_freq <= 0.0f) {
    return CHASSIS_ERR;
  }

  c->param = param;
  c->mode = CHASSIS_MODE_RELAX;

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

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

  for (int i = 0; i < 4; i++) {
    if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
      return DEVICE_ERR;
    }
  }

  for (int i = 0; i < 2; i++) {
    if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
      return DEVICE_ERR;
    }
  }

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

  /* 初始化滤波器 */
  for (int i = 0; i < 4; i++) {
    LowPassFilter2p_Init(&c->filter.joint_out[i], target_freq, param->low_pass_cutoff_freq.out);
  }
  for (int i = 0; i < 2; i++) {
    LowPassFilter2p_Init(&c->filter.wheel_out[i], target_freq, param->low_pass_cutoff_freq.out);
  }
  LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f);  /* 速度估计滤波截止频率30Hz */

  Chassis_MotorEnable(c);

  /* 初始化状态变量 */
  memset(&c->chassis_state, 0, sizeof(c->chassis_state));
  memset(&c->yaw_control, 0, sizeof(c->yaw_control));

  return CHASSIS_OK;
}

/**
 * @brief 更新底盘反馈数据
 * @param c 底盘结构体指针
 * @return 操作结果
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
  if (c == NULL) return CHASSIS_ERR_NULL;

  /* 更新所有电机数据 */
  MOTOR_LZ_UpdateAll();
  MOTOR_LK_UpdateAll();

  /* 更新关节电机反馈 */
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
    if (joint_motor != NULL) {
      c->feedback.joint[i] = joint_motor->motor.feedback;
    }

    /* 机械零点调整 */
    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++) {
    MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
    if (wheel_motor != NULL) {
      c->feedback.wheel[i] = wheel_motor->motor.feedback;
    }
  }

  /* 更新机体状态估计 */
  Chassis_UpdateChassisState(c);

  return CHASSIS_OK;
}

/**
 * @brief 更新IMU数据
 * @param c 底盘结构体指针
 * @param imu IMU数据
 * @return 操作结果
 */
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
  if (c == NULL) return CHASSIS_ERR_NULL;

  c->feedback.imu = imu;
  return CHASSIS_OK;
}

/**
 * @brief 底盘控制主函数
 * @param c 底盘结构体指针
 * @param c_cmd 控制指令
 * @return 操作结果
 */
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正解算 */
  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 = -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_WHELL_LEG_BALANCE:
      Chassis_LQRControl(c, c_cmd);
      Chassis_Output(c);
      break;

    default:
      return CHASSIS_ERR_MODE;
  }

  return CHASSIS_OK;
}

/**
 * @brief 底盘输出
 * @param c 底盘结构体指针
 */
void Chassis_Output(Chassis_t *c) {
  if (c == NULL) return;

  /* 关节输出滤波 */
  // for (int i = 0; i < 4; i++) {
  //   c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
  // }

  /* 发送关节控制指令 */
  MOTOR_LZ_MotionParam_t motion_param = {
      .torque = 0.0f,
      .target_angle = 0.0f,
      .target_velocity = 0.0f,
      .kp = 0.0f,
      .kd = 0.0f,
  };

  for (int i = 0; i < 4; i++) {
    motion_param.torque = c->output.joint[i];
    MOTOR_LZ_MotionControl(&c->param->joint_param[i], &motion_param);
  }

  /* 轮子输出滤波并发送 */
  // for (int i = 0; i < 2; i++) {
  //   c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
  // }
  MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
  MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
}

/**
 * @brief LQR控制
 * @param c 底盘结构体指针
 * @param c_cmd 控制指令
 * @return 操作结果
 */
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;

  /* ==================== 速度控制 ==================== */
  float raw_vx = c_cmd->move_vec.vx * 2.0f;
  float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;

  /* 加速度限制 */
  float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
  float max_velocity_change = MAX_ACCELERATION * c->dt;
  velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);

  float 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_State_t current_state = {
      .theta = c->vmc_[0].leg.theta,
      .d_theta = c->vmc_[0].leg.d_theta,
      .x = c->chassis_state.position_x,
      .d_x = c->chassis_state.velocity_x,
      .phi = c->feedback.imu.euler.pit,
      .d_phi = -c->feedback.imu.gyro.x,
  };

  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 = {
      .theta = 0.0f,
      .d_theta = 0.0f,
      .phi = -0.2f,
      .d_phi = 0.0f,
      .x = c->chassis_state.target_x,
      .d_x = c->chassis_state.target_velocity_x,
  };

  /* ==================== Yaw轴控制 ==================== */
  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;

  float manual_offset = c_cmd->move_vec.vy * M_PI_2;
  float base_target_1 = c->param->mech_zero_yaw + manual_offset;
  float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;

  float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
  float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);

  if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
    c->yaw_control.target_yaw = base_target_1;
    c->yaw_control.is_reversed = false;
  } else {
    c->yaw_control.target_yaw = base_target_2;
    c->yaw_control.is_reversed = true;
  }

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

  /* ==================== 左腿LQR控制 ==================== */
  if (c->vmc_[0].leg.is_ground_contact) {
    LQR_SetTargetState(&c->lqr[0], &target_state);
    LQR_Control(&c->lqr[0]);
  } else {
    target_state.theta = NON_CONTACT_THETA;
    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 + NON_CONTACT_THETA) +
        c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
    c->yaw_control.yaw_force = 0.0f;
  }

  /* ==================== 右腿LQR控制 ==================== */
  if (c->vmc_[1].leg.is_ground_contact) {
    LQR_SetTargetState(&c->lqr[1], &target_state);
    LQR_Control(&c->lqr[1]);
  } else {
    target_state.theta = NON_CONTACT_THETA;
    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 + NON_CONTACT_THETA) +
        c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
    c->yaw_control.yaw_force = 0.0f;
  }

  /* ==================== 轮毂输出 ==================== */
  c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
  c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;

  /* ==================== 横滚角补偿 ==================== */
  float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
                                      c->feedback.imu.gyro.x, c->dt);

  /* 限制补偿范围 */
  float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
                               ? 0.05f : 0.02f;
  roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);

  /* ==================== 腿长控制 ==================== */
  float target_L0[2];
  target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
  target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;

  /* 腿长限幅 */
  target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
  target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);

  /* 获取腿长变化率 */
  float leg_d_length[2];
  VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
  VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);

  /* ==================== 左右腿摆角同步补偿 ==================== */
  float Delta_Tp[2];
  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);
  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);

  /* ==================== 左腿VMC控制 ==================== */
  float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
                                    c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
  float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
                              pid_output_left + LEFT_BASE_FORCE;

  if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
                       c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
    VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
  } else {
    c->output.joint[0] = 0.0f;
    c->output.joint[1] = 0.0f;
  }

  /* ==================== 右腿VMC控制 ==================== */
  float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
                                     c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
  float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
                               pid_output_right + RIGHT_BASE_FORCE;

  if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
                       c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
    VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
  } else {
    c->output.joint[2] = 0.0f;
    c->output.joint[3] = 0.0f;
  }

  /* ==================== 安全限制 ==================== */
  for (int i = 0; i < 2; i++) {
    c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
  }
  for (int i = 0; i < 4; i++) {
    c->output.joint[i] = LIMIT(c->output.joint[i], -15.0f, 15.0f);
  }

  return CHASSIS_OK;
}