/*
 * 底盘模组
 */

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include "component/cmd.h"
#include "component/filter.h"
#include "component/kinematics.h"
#include "component/pid.h"
#include "device/go.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_TYPE_QUADRUPED,   /* 四足机器人 */
} Chassis_Type_t;

typedef enum {
  IMU_BOARD_BMI088,
  IMU_WHEELREC_N100,
  IMU_NUM,
  IMU_ERROR,
} Chassis_ImuType_t;

typedef union {
  float id[GO_MOTOR_NUM];
  struct{
    float lf_hip;   /* 左前腿髋关节长度，单位：米 */
    float lf_thigh; /* 左前腿大腿长度，单位：米 */
    float lf_calf;  /* 左前腿小腿长度，单位：米 */
    float rf_hip;   /* 右前腿髋关节长度，单位：米 */
    float rf_thigh; /* 右前腿大腿长度，单位：米 */
    float rf_calf;  /* 右前腿小腿长度，单位：米 */
    float lr_hip;   /* 左后腿髋关节长度，单位：米 */
    float lr_thigh; /* 左后腿大腿长度，单位：米 */
    float lr_calf;  /* 左后腿小腿长度，单位：米 */
    float rr_hip;   /* 右后腿髋关节长度，单位：米 */
    float rr_thigh; /* 右后腿大腿长度，单位：米 */
    float rr_calf;  /* 右后腿小腿长度，单位：米 */
  } named;
} joint_params;

typedef struct {
  float x;
  float y;
  float z;
} joint_pos;
typedef struct {
  joint_params min;
  joint_params max;
} joint_limits;

typedef union {
  Kinematics_Sign_t leg[GO_MOTOR_NUM/3];
  struct {
    Kinematics_Sign_t lf; /* 左前腿 */
    Kinematics_Sign_t rf; /* 右前腿 */
    Kinematics_Sign_t lr; /* 左后腿 */
    Kinematics_Sign_t rr; /* 右后腿 */
  } named;
} Kinematics_DirectionSign_t;


typedef struct{
  joint_params zero_point; /* 零点角度，单位：弧度 */

  // joint_params length;     /* 关节长度，单位：米 */
 
  joint_params ratio;      /* 关节减速比 */
  joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
  
  Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
  Kinematics_LegOffset_t leg_offset; /* 关节偏移，单位：米 */
  Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
    
}Chassis_Mech_Params_t;

/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
  Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */

  Chassis_Mech_Params_t mech_param; /* 机械参数 */

  KPID_Params_t torque_pid_param; /* 力矩矩控制PID的参数 */
  KPID_Params_t blance_pid_param; /* 平衡PID的参数 */
  /* 低通滤波器截止频率 */
  struct {
    float in;  /* 输入 */
    float out; /* 输出 */
  } low_pass_cutoff_freq;

} Chassis_Params_t;

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

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

  GO_ChassisFeedback_t feedback; /* 底盘反馈信息 */
  GO_ChassisCMD_t output;
  float height; /* 底盘高度，单位：米 */

  struct {
    float x;
    float y;
    float z;
  } foot_base[GO_MOTOR_NUM/3]; /* 四个足端的位置，单位：米 */

  struct{
    struct{
      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
    }start_pos;
    struct{
      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
    }mid1_pos;
    struct{
      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
    }mid2_pos;
    struct{
      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
    }end_pos;  
  } leg_pos; /* 四条腿的关节位置，单位：米 */

  /* 模块通用 */
  CMD_ChassisMode_t mode; /* 底盘模式 */
  
  CMD_ChassisAction_t action; /* 底盘模式 */

  AHRS_Eulr_t eulr_imu; /* 欧拉角，单位：弧度 */

  AHRS_Eulr_t delta_eulr; /* 欧拉角变化量，单位：弧度 */

  AHRS_Eulr_t eulr_setpoint; /* 期望的欧拉角，单位：弧度 */
  
  MoveVector_t move_vec; /* 底盘实际的运动向量 */

  /* 反馈控制用的PID */
  struct {
    KPID_t *motor_id; /* 控制轮子电机用的PID的动态数组 */
    KPID_t blance;    /* 平衡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, const Chassis_Params_t *param,float target_freq);

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

/**
 * @brief 更新底盘的IMU信息
 * @param c 包含底盘数据的结构体
 * @param eulr 包含IMU欧拉角的结构体
 * @return 
 */
int8_t Chassis_UpdateImu(Chassis_t *c, const AHRS_Eulr_t *eulr);

/**
 * \brief 运行底盘控制逻辑
 *
 * \param c 包含底盘数据的结构体
 * \param c_cmd 底盘控制指令
 * \param dt_sec 两次调用的时间间隔
 *
 * \return 函数运行结果
 */
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
                       uint32_t now);

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

/**
 * \brief 清空Chassis输出数据
 *
 * \param out CAN设备底盘输出结构体
 */
// void Chassis_ResetOutput(CAN_ChassisOutput_t *out);

#ifdef __cplusplus
}
#endif