/*
 * 平衡底盘模组
 */

#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/pid.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.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_WHELL_BALANCE,          /* 平衡模式，底盘自我平衡 */
  CHASSIS_MODE_WHELL_LEG_BALANCE,      /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t;

typedef struct {
  Chassis_Mode_t mode;           /* 底盘模式 */
  MoveVector_t move_vec;         /* 底盘运动向量 */
  float height;               /* 目标高度 */
} 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]; /* 两个轮子电机反馈 */
  Chassis_IMU_t imu; /* IMU数据 */
  MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
}Chassis_Feedback_t;

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


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

  MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
  MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */

  VMC_Param_t vmc_param[2];        /* VMC参数 */
  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */

  /* LQR控制器参数 */
  struct {
    float max_wheel_torque; /* 轮毂电机最大力矩限制 */
    float max_joint_torque; /* 关节电机最大力矩限制 */
  } lqr_param;
  
  KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
  KPID_Params_t roll;              /* roll轴PID控制参数，用于横滚角补偿 */
  KPID_Params_t tp;          /* 摆力矩PID控制参数，用于控制摆力矩 */
  KPID_Params_t leg_length;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
  KPID_Params_t leg_theta;          /* 摆角PID控制参数，用于控制虚拟腿摆角 */

  float mech_zero_yaw; /* 机械零点 */

  /* 低通滤波器截止频率 */
  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_Controller_t lqr[2]; /* 两条腿的LQR控制器 */

  int8_t state;

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

  /* yaw控制相关 */
  struct {
    float target_yaw;     /* 目标yaw角度 */
    float current_yaw;    /* 当前yaw角度 */
    float yaw_force;      /* yaw轴控制力矩 */
  } yaw_control;

  float wz_multi; /* 小陀螺模式旋转方向 */

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

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

  /* 滤波器 */
  struct {
    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
    LowPassFilter2p_t *out; /* 输出值滤波器 */
  } filter;

} 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