/*
 * 平衡底盘模组
 */

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

//             Front
//         | 1       4 |
//  (1)Left|           |Right(2)
//         | 2       3 |

/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "component/vmc.h"
#include "component/lqr.h"
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/kalman_filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"

/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0)        /* 运行正常 */
#define CHASSIS_ERR (-1)      /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */

/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */

typedef enum {
  CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
  CHASSIS_MODE_RECOVER,          /* 复位模式 */
  // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
  CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
  CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
} Chassis_Mode_t;

/* 跳跃状态枚举 */
typedef enum {
  JUMP_IDLE,      /* 待机状态 */
  JUMP_CROUCH,    /* 蓄力阶段：腿缩短蓄力 */
  JUMP_LAUNCH,    /* 起跳阶段：发力向下 */
  JUMP_RETRACT,   /* 收腿阶段：腿收缩准备落地 */
  JUMP_LAND       /* 落地阶段：恢复正常控制 */
} Jump_State_t;

/* 自起状态枚举 */
typedef enum {
  RECOVER_FLIP,      /* 翻身阶段：theta朝上时，先慢慢翻转到腿朝下 */
  RECOVER_STRETCH,   /* 伸腿阶段：腿伸到最大长度 */
  RECOVER_BACKLEG,   /* 后甩腿阶段：phi0 趋近目标角度 */
  RECOVER_COMPLETE   /* 完成：切换到平衡模式 */
} Recover_State_t;

typedef struct {
  Chassis_Mode_t mode;           /* 底盘模式 */
  MoveVector_t move_vec;         /* 底盘运动向量 */
  float height;               /* 目标高度 */
  bool jump_trigger;             /* 跳跃触发标志 */
} Chassis_CMD_t;

typedef struct {
  AHRS_Accl_t accl;   /* IMU加速度计 */
  AHRS_Gyro_t gyro;   /* IMU陀螺仪 */
  AHRS_Eulr_t euler;  /* IMU欧拉角 */
}Chassis_IMU_t;

typedef struct {
  MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
  MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
  MOTOR_Feedback_t yaw;   /* 云台Yaw轴电机反馈 */
  Chassis_IMU_t imu; /* IMU数据 */
}Chassis_Feedback_t;

typedef struct {
  float joint[4]; /* 四个关节电机输出 */
  float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;


/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {

  MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数
  MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数
  MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
  VMC_Param_t vmc_param[2];        /* VMC参数 */
  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
  
  KPID_Params_t yaw;               /* yaw轴PID控制参数 */
  KPID_Params_t roll;              /* roll轴腿长补偿PID */
  KPID_Params_t roll_force;        /* roll轴力补偿PID */
  KPID_Params_t tp;                /* 摆力矩PID */
  KPID_Params_t leg_length;        /* 腿长PID */
  KPID_Params_t leg_theta;         /* 摆角PID */

  /* 机械参数 */
  struct {
    float wheel_radius;            /* 轮子半径 (m) */
    float wheel_gear_ratio;        /* 轮毂电机减速比 */
    float hip_width;               /* 髋宽，两腿间距 (m) */
    float joint_zero[4];           /* 关节电机零点偏移 */
    float mech_zero_yaw;           /* yaw轴机械零点 */
  } mech;

  /* 腿长控制参数 */
  struct {
    float base_length;             /* 基础腿长 (m) */
    float length_range;            /* 腿长调节范围 (m)，波轮控制 */
    float min_length;              /* 最小腿长 (m) */
    float max_length;              /* 最大腿长 (m) */
    float left_base_force;         /* 左腿基础支撑力 (N) */
    float right_base_force;        /* 右腿基础支撑力 (N) */
  } leg;

  /* 跳跃参数 */
  struct {
    uint32_t crouch_time_ms;       /* 蓄力超时 (ms) */
    uint32_t launch_time_ms;       /* 起跳超时 (ms) */
    uint32_t retract_time_ms;      /* 收腿超时 (ms) */
    uint32_t land_time_ms;         /* 落地缓冲时间 (ms) */
    float crouch_leg_length;       /* 蓄力时腿长 (m) */
    float launch_force;            /* 起跳额外力 (N) */
    float retract_leg_length;      /* 收腿时腿长 (m) */
    float retract_force;           /* 收腿前馈力 (N)，负值=向上 */
  } jump;

  /* 运动控制参数 */
  struct {
    float max_acceleration;        /* 最大加速度 (m/s²) */
    float non_contact_theta;       /* 离地时摆角目标 (rad) */
  } motion;

  /* LQR 目标状态偏移 */
  struct {
    float theta;
    float x;
    float phi;
  } lqr_offset;

  /* 低通滤波器截止频率 */
  struct {
    float in;
    float out;
  } low_pass_cutoff_freq;
} Chassis_Params_t;

/*
 * 运行的主结构体，所有这个文件里的函数都在操作这个结构体
 * 包含了初始化参数，中间变量，输出变量
 */
typedef struct {
  uint64_t lask_wakeup;
  float dt;

  Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
  AHRS_Eulr_t *mech_zero;

  /* 模块通用 */
  Chassis_Mode_t mode; /* 底盘模式 */

  /* 反馈信息 */
  Chassis_Feedback_t feedback;
  /* 控制信息*/
  Chassis_Output_t output;

  VMC_t vmc_[2]; /* 两条腿的VMC */
  LQR_t lqr[2]; /* 两条腿的LQR控制器 */

  /* 机体状态估计 */
  struct {
    float position_x;     /* 机体x位置 */
    float velocity_x;     /* 机体x速度 */
    float last_velocity_x; /* 上一次速度用于数值微分 */
    float target_x;       /* 目标x位置 */
    float target_velocity_x; /* 目标速度 */
    float last_target_velocity_x; /* 上一次目标速度 */
  } chassis_state;

  /* yaw控制相关 */
  struct {
    float target_yaw;     /* 目标yaw角度 */
    float current_yaw;    /* 当前yaw角度 */
    float yaw_force;      /* yaw轴控制力矩 */
    bool is_reversed;     /* 是否使用反转的零点（180度零点），影响前后方向 */
  } yaw_control;

  /* 跳跃控制相关 */
  struct {
    Jump_State_t state;       /* 跳跃状态 */
    bool trigger;             /* 跳跃触发标志 */
    uint32_t state_start_time; /* 当前状态开始时间 (ms) */
    float crouch_leg_length;  /* 蓄力时的腿长 */
    float launch_force;       /* 起跳力 (N) */
    float retract_leg_length; /* 收腿时的腿长 */
  } jump;

  /* 自起控制相关 */
  struct {
    Recover_State_t state[2]; /* 左右腿自起状态 */
  } recover;

  /* PID计算的目标值 */
  struct {
    AHRS_Eulr_t chassis;
  } setpoint;

  /* 反馈控制用的PID */
  struct {
    KPID_t yaw; /* 跟随云台用的PID */
    KPID_t roll;    /* 横滚角腿长补偿PID */
    KPID_t roll_force; /* 横滚角力补偿PID */
    KPID_t tp[2];
    KPID_t leg_length[2]; /* 两条腿的腿长PID */
    KPID_t leg_theta[2]; /* 两条腿的摆角PID */
  } pid;

  /* 滤波器 */
  struct {
    LowPassFilter2p_t joint_out[4];  /* 关节输出滤波器 */
    LowPassFilter2p_t wheel_out[2];  /* 轮子输出滤波器 */
    LowPassFilter2p_t velocity_est;  /* 速度估计滤波器 */
  } filter;

  /* 卡尔曼滤波器：位移速度融合IMU */
  KF_t kf_state_est;

} Chassis_t;

/* Exported functions prototypes -------------------------------------------- */

/**
 * \brief 初始化底盘
 *
 * \param c 包含底盘数据的结构体
 * \param param 包含底盘参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);

/**
 * \brief 更新底盘的反馈信息
 *
 * \param c 包含底盘数据的结构体
 * \return 函数运行结果
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c);


/**
 * @brief 更新IMU数据
 * @param c 包含底盘数据的结构体
 * @param imu IMU数据
 * @return 
 */
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/**
 * \brief 运行底盘控制逻辑
 *
 * \param c 包含底盘数据的结构体
 * \param c_cmd 底盘控制指令
 *
 * \return 函数运行结果
 */
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);

/**
 * \brief LQR控制逻辑
 *
 * \param c 包含底盘数据的结构体
 * \param c_cmd 底盘控制指令
 *
 * \return 函数运行结果
 */
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);

/**
 * \brief 底盘输出值
 *
 * \param s 包含底盘数据的结构体
 * \param out CAN设备底盘输出结构体
 */
void Chassis_Output(Chassis_t *c);

#ifdef __cplusplus
}
#endif