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

#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/kalman_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)，固定机械尺寸 */


// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
/* 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);
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);

/* 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.roll_force);
  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);
  }

  /* 重置卡尔曼滤波器 */
  KF_Reset(&c->kf_state_est);

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

  /*
   * 轮子角速度补偿算法
   * w = w_wheel - pitch_gyro + joint_speed
   */
  float left_w_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
  float right_w_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;

  /*
   * 机体速度计算 (参考港大开源)
   * v = w*R + L0*dtheta*cos(theta) + dL0*sin(theta)
   */
  float left_v_body = left_w_compensated * WHEEL_RADIUS +
                      c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta * cosf(c->vmc_[0].leg.theta) +
                      c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);

  float right_v_body = right_w_compensated * WHEEL_RADIUS +
                       c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta * cosf(c->vmc_[1].leg.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;

  /*
   * 获取IMU前向加速度（世界坐标系x方向）
   * 使用旋转矩阵将机体加速度(已去重力)转到世界系
   * axE = cos(pitch)*ax_body + sin(roll)*sin(pitch)*ay_body + cos(roll)*sin(pitch)*az_body
   */
  float pitch = c->feedback.imu.euler.pit;
  float roll  = c->feedback.imu.euler.rol;
  float ax_body = c->feedback.imu.accl.x;
  float ay_body = c->feedback.imu.accl.y;
  float az_body = c->feedback.imu.accl.z;

  /* 去除重力分量（机体系下） */
  float gravity_bx = -9.81f * sinf(pitch);
  float gravity_by =  9.81f * sinf(roll) * cosf(pitch);
  float gravity_bz =  9.81f * cosf(roll) * cosf(pitch);
  float motion_ax = ax_body - gravity_bx;
  float motion_ay = ay_body - gravity_by;
  float motion_az = az_body - gravity_bz;

  /* 旋转到世界坐标系x方向 */
  float accel_world_x = cosf(pitch) * motion_ax +
                        sinf(roll) * sinf(pitch) * motion_ay +
                        cosf(roll) * sinf(pitch) * motion_az;

  /* ==================== 卡尔曼滤波器融合 ==================== */
  /* 动态更新状态转移矩阵 F（根据实际dt）*/
  float dt = c->dt;
  if (dt <= 0.0f || dt > 0.1f) dt = 0.001f;

  /* F = [1, dt; 0, 1] */
  c->kf_state_est.F_data[0] = 1.0f;
  c->kf_state_est.F_data[1] = dt;
  c->kf_state_est.F_data[2] = 0.0f;
  c->kf_state_est.F_data[3] = 1.0f;

  /* 量测向量: z = [v_wheel, a_imu]
   * 注意：必须写入 measured_vector，KF_Measure 会将其拷贝到 z_data
   * 直接写 z_data 会被 KF_Measure 用 measured_vector(全零) 覆盖！ */
  c->kf_state_est.measured_vector[0] = vel_measured;
  c->kf_state_est.measured_vector[1] = accel_world_x;

  /* 动态调整R矩阵：离地时降低轮速信任度 */
  if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
    c->kf_state_est.R_data[0] = 100000.0f;  /* 离地：不信任轮速 */
  } else {
    c->kf_state_est.R_data[0] = 100.0f;     /* 接地：信任轮速 */
  }
  /* R[1][1] = 1000000.0f 保持不变，始终不信任加速度量测 */

  /* 执行卡尔曼滤波 */
  float *kf_result = KF_Update(&c->kf_state_est);

  /* KF输出: [velocity, acceleration] */
  if (kf_result != NULL) {
    c->chassis_state.velocity_x = kf_result[0];
    /* 位移 = 滤波后的速度积分（不让加速度误差被二次积分放大） */
    c->chassis_state.position_x += c->chassis_state.velocity_x * dt;
  }
}

/**
 * @brief 计算五杆机构弹簧等效支撑力
 * @param leg_length 腿长 L_AE (m)，有效范围 [0.0391, 0.4689]
 * @return 等效支撑力 (N)，无解时返回 NAN
 */
static float Chassis_CalcSpringForce(float leg_length)
{
    // 五杆机构几何参数 (单位: m)
    const float L_AB = 0.215f;  // 基座长度
    const float L_AC = 0.042f;  // 弹簧上端高度
    const float L_BD = 0.050f;  // 杆BD长度
    const float L_BE = 0.254f;  // 杆BE长度
    const float Fs = 360.0f;    // 弹簧恒力 (N)
    
    // 通过余弦定理计算∠ABE的余弦值
    float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);
    
    // 检查解的有效性
    if (fabsf(cos_theta) > 1.0f) {
        return NAN;  // 无物理解
    }
    
    float theta = acosf(cos_theta);
    float sin_theta = sinf(theta);
    
    // 计算等效支撑力
    // F_eq = (Fs × |力臂| × L_AB × sin(θ)) / (L_CD × L_BE × L_AE)
    return (Fs * fabsf(L_BD*cos_theta*L_AC - L_AB*L_BD*sin_theta) * L_AB*sin_theta) 
           / (sqrtf((L_AB - L_BD*cos_theta)*(L_AB - L_BD*cos_theta) 
                  + (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
}

/**
 * @brief 跳跃控制状态机
 * @param c 底盘结构体指针
 * @param c_cmd 控制指令
 * @param target_L0 输出的目标腿长数组[2]
 * @param extra_force 输出的额外力数组[2]
 *
 * @note 跳跃流程：
 *   IDLE -> CROUCH(蓄力下蹲) -> LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE
 *   蓄力阶段：用强力前馈把腿压短，同时等腿真正到位再起跳
 *   起跳阶段：直接给大力蹬地，绕过正常PID/LQR
 */
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
  uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);

  /* 初始化额外力为0 */
  extra_force[0] = 0.0f;
  extra_force[1] = 0.0f;

  /* 检测跳跃触发 */
  if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
    c->jump.state = JUMP_CROUCH;
    c->jump.state_start_time = current_time;
    c->jump.trigger = false;
  }

  /* elapsed_time 必须在触发逻辑之后计算，否则第一帧会用旧的 state_start_time */
  uint32_t elapsed_time = current_time - c->jump.state_start_time;

  /* 跳跃状态机 */
  switch (c->jump.state) {
    case JUMP_IDLE:
      break;

    case JUMP_CROUCH: {
      /* 蓄力阶段：目标腿长设短 */
      target_L0[0] = c->param->jump.crouch_leg_length;
      target_L0[1] = c->param->jump.crouch_leg_length;
      extra_force[0] = 0.0f;
      extra_force[1] = 0.0f;

      /* 位置驱动：腿缩到位就起跳（差值<2cm） */
      float ct = c->param->jump.crouch_leg_length;
      bool leg_ready = (c->vmc_[0].leg.L0 < ct + 0.02f) &&
                       (c->vmc_[1].leg.L0 < ct + 0.02f);
      if (leg_ready) {
        c->jump.state = JUMP_LAUNCH;
        c->jump.state_start_time = current_time;
      }
      /* 超时保护 */
      if (elapsed_time >= c->param->jump.crouch_time_ms) {
        c->jump.state = JUMP_LAUNCH;
        c->jump.state_start_time = current_time;
      }
    } break;

    case JUMP_LAUNCH: {
      /* 起跳阶段：腿伸长 + 额外蹬地力 */
      target_L0[0] = c->param->leg.max_length;
      target_L0[1] = c->param->leg.max_length;
      extra_force[0] = c->param->jump.launch_force;
      extra_force[1] = c->param->jump.launch_force;

      /* 位置驱动：腿伸到位就收腿（差值<3cm） */
      bool leg_ext = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
                     (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
      if (leg_ext) {
        c->jump.state = JUMP_RETRACT;
        c->jump.state_start_time = current_time;
      }
      /* 超时保护 */
      if (elapsed_time >= c->param->jump.launch_time_ms) {
        c->jump.state = JUMP_RETRACT;
        c->jump.state_start_time = current_time;
      }
    } break;

    case JUMP_RETRACT: {
      /* 收腿阶段 */
      target_L0[0] = c->param->jump.retract_leg_length;
      target_L0[1] = c->param->jump.retract_leg_length;
      extra_force[0] = c->param->jump.retract_force;
      extra_force[1] = c->param->jump.retract_force;

      /* 位置驱动：腿缩到位就进落地缓冲 */
      float rt = c->param->jump.retract_leg_length;
      bool leg_retracted = (c->vmc_[0].leg.L0 < rt + 0.02f) &&
                           (c->vmc_[1].leg.L0 < rt + 0.02f);
      if (leg_retracted) {
        c->jump.state = JUMP_LAND;
        c->jump.state_start_time = current_time;
      }
      /* 超时保护 */
      if (elapsed_time >= c->param->jump.retract_time_ms) {
        c->jump.state = JUMP_LAND;
        c->jump.state_start_time = current_time;
      }
    } break;

    case JUMP_LAND:
      /* 落地缓冲：时间到就回 IDLE */
      if (elapsed_time >= c->param->jump.land_time_ms) {
        c->jump.state = JUMP_IDLE;
      }
      break;

    default:
      c->jump.state = JUMP_IDLE;
      break;
  }
}

/* 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.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
  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 */

  /*
   * 初始化卡尔曼滤波器（参考港大开源方案）
   * 状态向量 x = [v, a]  (2维：速度和加速度)
   * 量测向量 z = [v, a]  (2维：轮速和IMU加速度)
   * 位移在KF外部用滤波后的速度积分，避免加速度误差被二次积分放大
   */
  KF_Init(&c->kf_state_est, 2, 0, 2);

  float dt_init = 1.0f / target_freq;

  /* 初始协方差矩阵 P (2x2) */
  static float P_Init[4] = {
      1.0f, 0.0f,
      0.0f, 1.0f,
  };
  memcpy(c->kf_state_est.P_data, P_Init, sizeof(P_Init));

  /* 状态转移矩阵 F (2x2): v(k) = v(k-1) + a*dt, a(k) = a(k-1) */
  float F_Init[4] = {
      1.0f, dt_init,
      0.0f, 1.0f,
  };
  memcpy(c->kf_state_est.F_data, F_Init, sizeof(F_Init));

  /* 过程噪声协方差矩阵 Q (2x2) */
  static float Q_Init[4] = {
      0.1f, 0.0f,
      0.0f, 0.1f,
  };
  memcpy(c->kf_state_est.Q_data, Q_Init, sizeof(Q_Init));

  /* 状态最小方差（防止过度收敛） */
  static float state_min_var[2] = {0.005f, 0.1f};
  memcpy(c->kf_state_est.state_min_variance, state_min_var, sizeof(state_min_var));

  /* 关闭自动量测调整（手动管理H/R） */
  c->kf_state_est.use_auto_adjustment = 0;

  /* 量测矩阵 H (2x2): z = H*x, 直接观测速度和加速度 */
  static float H_Init[4] = {
      1.0f, 0.0f,
      0.0f, 1.0f,
  };
  memcpy(c->kf_state_est.H_data, H_Init, sizeof(H_Init));

  /*
   * 量测噪声协方差矩阵 R (2x2)
   * 关键：加速度噪声设极大(1000000)，几乎不信任加速度量测
   * KF主要靠轮速量测修正，加速度仅在预测环节(F矩阵)起辅助作用
   */
  static float R_Init[4] = {
      100.0f,    0.0f,
      0.0f,      1000000.0f,
  };
  memcpy(c->kf_state_est.R_data, R_Init, sizeof(R_Init));

  Chassis_MotorEnable(c);

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

  /* 初始化跳跃状态 */
  c->jump.state = JUMP_IDLE;
  c->jump.trigger = false;
  c->jump.state_start_time = 0;
  c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
  c->jump.launch_force = c->param->jump.launch_force;
  c->jump.retract_leg_length = c->param->jump.retract_leg_length;

  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;
    
    /* 应用零点偏移 */
    c->feedback.joint[i].rotor_abs_angle -= c->param->mech.joint_zero[i];
  }

  /* 更新轮子电机反馈 */
  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;
  }

  /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */
  if (c_cmd->jump_trigger &&
      (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR ||
       c->mode == CHASSIS_MODE_JUMP_TEST) &&
      c->jump.state == JUMP_IDLE) {
    c->jump.trigger = true;
  }

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

  /* 修正离地检测：加上弹簧提供的支撑力 */
  float spring_Fn_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
  float spring_Fn_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
  if (!isnan(spring_Fn_left)) {
    c->vmc_[0].leg.Fn += spring_Fn_left;
    c->vmc_[0].leg.is_ground_contact = (c->vmc_[0].leg.Fn > 20.0f);
  }
  if (!isnan(spring_Fn_right)) {
    c->vmc_[1].leg.Fn += spring_Fn_right;
    c->vmc_[1].leg.is_ground_contact = (c->vmc_[1].leg.Fn > 20.0f);
  }

  /* 根据模式执行控制 */
  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);

      /* 计算弹簧力补偿 */
      float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
      float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
      float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left);
      float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right);

      VMC_InverseSolve(&c->vmc_[0], fn_left, tp);
      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
      VMC_InverseSolve(&c->vmc_[1], fn_right, 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_CALIBRATE: {
    //   Chassis_LQRControl(c, c_cmd);
    //   LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
    //   RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
    //   L_fn = -LF;
    //   VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
    //   VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
    //   VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
    //   VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);

    //   c->output.wheel[0] = 0.0f;
    //   c->output.wheel[1] = 0.0f;
    //   Chassis_Output(c);
    // } break;

    case CHASSIS_MODE_WHELL_LEG_BALANCE:
      Chassis_LQRControl(c, c_cmd);
      Chassis_Output(c);
      break;
    case CHASSIS_MODE_BALANCE_ROTOR:
      Chassis_LQRControl(c, c_cmd);
      c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
      c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
      Chassis_Output(c);
      break;

    case CHASSIS_MODE_JUMP_TEST: {
      /* 跳跃测试模式：架空测试，无平衡/LQR，纯腿长PID + 跳跃状态机 */
      c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;

      /* 轮子不输出 */
      c->output.wheel[0] = 0.0f;
      c->output.wheel[1] = 0.0f;

      /* 基础腿长目标 */
      float test_target_L0[2];
      float test_extra_force[2] = {0.0f, 0.0f};
      float test_base_leg = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range;
      test_target_L0[0] = test_base_leg;
      test_target_L0[1] = test_base_leg;

      /* 跳跃控制 */
      Chassis_JumpControl(c, c_cmd, test_target_L0, test_extra_force);

      /* 安全限幅 */
      test_target_L0[0] = LIMIT(test_target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
      test_target_L0[1] = LIMIT(test_target_L0[1], c->param->leg.min_length, c->param->leg.max_length);

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

      /* 弹簧力补偿 */
      float test_spring_L = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
      if (isnan(test_spring_L)) test_spring_L = 0.0f;
      float test_spring_R = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
      if (isnan(test_spring_R)) test_spring_R = 0.0f;

      /* 左腿：PID + 基础支撑力(对抗重力) - 弹簧力 + 跳跃前馈力 */
      float test_pid_L = PID_Calc(&c->pid.leg_length[0], test_target_L0[0],
                                   c->vmc_[0].leg.L0, test_leg_dl[0], c->dt);
      float test_Fn_L = test_pid_L + c->param->leg.left_base_force - test_spring_L + test_extra_force[0];

      if (VMC_InverseSolve(&c->vmc_[0], test_Fn_L, 0.0f) == 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;
      }

      /* 右腿 */
      float test_pid_R = PID_Calc(&c->pid.leg_length[1], test_target_L0[1],
                                   c->vmc_[1].leg.L0, test_leg_dl[1], c->dt);
      float test_Fn_R = test_pid_R + c->param->leg.right_base_force - test_spring_R + test_extra_force[1];

      if (VMC_InverseSolve(&c->vmc_[1], test_Fn_R, 0.0f) == 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 < 4; i++) {
        c->output.joint[i] = LIMIT(c->output.joint[i], -60.0f, 60.0f);
      }

      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 = c->param->motion.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;
  c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
  c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
  c->chassis_state.target_x += c->chassis_state.target_velocity_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.0f,
      .d_phi = 0.0f,
      .x = c->chassis_state.target_x,
      .d_x = c->chassis_state.target_velocity_x,
  };

  /* ==================== Yaw轴控制 ==================== */
  if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
    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.mech_zero_yaw + manual_offset;
    float base_target_2 = c->param->mech.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 = c->param->motion.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 + c->param->motion.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 = c->param->motion.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 + c->param->motion.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 / c->param->mech.wheel_gear_ratio + c->yaw_control.yaw_force;
  c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - c->yaw_control.yaw_force;

  /* ==================== 横滚角补偿 ==================== */
  /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
  float Rl = c->param->mech.hip_width / 2.0f;  /* 髋宽的一半 */
  float roll_error = c->feedback.imu.euler.rol - 0.0f;  /* Roll角误差 */
  float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0;  /* 右腿减左腿腿长差 */
  
  float A = Delta_L0 * cosf(roll_error) + 2.0f * Rl * sinf(roll_error);
  float B = -Delta_L0 * sinf(roll_error) + 2.0f * Rl * cosf(roll_error);
  
  float roll_leg_compensation_left = 0.0f;
  float roll_leg_compensation_right = 0.0f;
  
  if (fabsf(B) > 0.001f) {  /* 避免除零 */
    float tan_delta = A / B;
    roll_leg_compensation_left = -Rl * tan_delta;
    roll_leg_compensation_right = Rl * tan_delta;
  }
  
  /* 限制几何补偿范围 (适当提高限幅增强补偿效果) */
  float max_leg_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
                               ? 0.12f : 0.0f;  /* 提高到0.05/0.025以提供足够补偿 */
  roll_leg_compensation_left = LIMIT(roll_leg_compensation_left, -max_leg_compensation, max_leg_compensation);
  roll_leg_compensation_right = LIMIT(roll_leg_compensation_right, -max_leg_compensation, max_leg_compensation);
  
  /* 方法2: PID力补偿 (低增益避免抖动) */
  float roll_force_compensation = PID_Calc(&c->pid.roll_force, 0.0f, c->feedback.imu.euler.rol,
                                            c->feedback.imu.gyro.x, c->dt);

  /* ==================== 腿长控制 ==================== */
  float target_L0[2];
  float jump_extra_force[2] = {0.0f, 0.0f};
  
  /* 基础腿长目标 (应用几何前馈补偿) */
  float base_leg_length = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range;
  target_L0[0] = base_leg_length + roll_leg_compensation_left;
  target_L0[1] = base_leg_length + roll_leg_compensation_right;

  /* 跳跃控制：可能会修改目标腿长和额外力 */
  Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);

  /* 智能限幅：短腿不够时将补偿转移到长腿 */
  float compensation_transfer = 0.0f;
  
  /* 检查哪条腿更短（需要缩短的那条） */
  if (target_L0[0] < target_L0[1]) {
    /* 左腿更短，检查是否触碰下限 */
    if (target_L0[0] < c->param->leg.min_length) {
      compensation_transfer = c->param->leg.min_length - target_L0[0];  /* 计算短缺量 */
      target_L0[0] = c->param->leg.min_length;  /* 左腿限制在最小值 */
      target_L0[1] += compensation_transfer;  /* 右腿补偿增加 */
    }
  } else if (target_L0[1] < target_L0[0]) {
    /* 右腿更短，检查是否触碰下限 */
    if (target_L0[1] < c->param->leg.min_length) {
      compensation_transfer = c->param->leg.min_length - target_L0[1];  /* 计算短缺量 */
      target_L0[1] = c->param->leg.min_length;  /* 右腿限制在最小值 */
      target_L0[0] += compensation_transfer;  /* 左腿补偿增加 */
    }
  }
  
  /* 最终安全限幅 */
  target_L0[0] = LIMIT(target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
  target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_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 spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
  if (isnan(spring_force_left)) spring_force_left = 0.0f;

  float virtual_force_left, virtual_torque_left;
  if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
    /* 跳跃阶段：完全接管力控 */
    virtual_force_left = jump_extra_force[0] + pid_output_left;
    virtual_torque_left = 0.0f;
    /* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
    c->output.wheel[0] = 0.0f;
    c->output.wheel[1] = 0.0f;
  } else {
    /* 正常状态：添加Roll力补偿 (低增益避免抖动) */
    virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
                          pid_output_left + c->param->leg.left_base_force - spring_force_left + 
                          jump_extra_force[0] + roll_force_compensation;
    virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
  }

  if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left, virtual_torque_left) == 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 spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
  if (isnan(spring_force_right)) spring_force_right = 0.0f;

  float virtual_force_right, virtual_torque_right;
  if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
    /* 跳跃阶段：完全接管力控 */
    virtual_force_right = jump_extra_force[1] + pid_output_right;
    virtual_torque_right = 0.0f;
  } else {
    /* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
    virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
                           pid_output_right + c->param->leg.right_base_force - spring_force_right + 
                           jump_extra_force[1] - roll_force_compensation;
    virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
  }

  if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right, virtual_torque_right) == 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], -60.0f, 60.0f);
  }

  return CHASSIS_OK;
}