/*
 底盘模组
 */
#pragma once


#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/can.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "component\ahrs.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0)        /* 运行正常 */
#define CHASSIS_ERR (-1)      /* 运行时出现了一些小错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时出配置了错误的ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时出配置了错误的Chassis_Type_t */
	
#define MAX_MOTOR_CURRENT 20.0f 
/* 底盘控制模式 */
typedef enum {
  CHASSIS_MODE_RELAX, /* 放松模式,电机不输出。一般情况底盘初始化之后的模式 */
  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制静止。用于机器人停止状态 */
  CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35°跟随云台 */
  CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
  CHASSIS_MODE_INDEPENDENT, /*独立模式。底盘运行不受云台影响 */
  CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
} Chassis_Mode_t;
	
/* 小陀螺转动模式 */
typedef enum {
  ROTOR_MODE_CW,   /* 顺时针转动 */
  ROTOR_MODE_CCW,  /* 逆时针转动 */
  ROTOR_MODE_RAND, /* 随机转动 */
} Chassis_RotorMode_t;

/* 底盘控制命令 */
typedef struct {
  Chassis_Mode_t mode;     /* 底盘运行模式 */
  Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
} ChassisCmd_t;

/* 限位 */
typedef struct {
  float max;
  float min;
} Chassis_Limit_t;

	/* 底盘类型（底盘的物理设计） */
typedef enum {
  CHASSIS_TYPE_MECANUM,    /* 麦克纳姆轮 */
  CHASSIS_TYPE_PARLFIX4,   /* 平行摆放的四个驱动轮 */
  CHASSIS_TYPE_PARLFIX2,   /* 平行摆放的两个驱动轮 */
  CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆放的四个全向轮 */
  CHASSIS_TYPE_OMNI_PLUS,  /* 十字型摆设的四个全向轮 */
  CHASSIS_TYPE_DRONE,      /* 无人机底盘 */
  CHASSIS_TYPE_SINGLE,     /* 单个摩擦轮 */
} Chassis_Type_t;


/* 底盘参数结构体,ALL初始化参数 */
typedef struct {
	MOTOR_RM_Param_t motor_param[4];
	  struct {
    KPID_Params_t motor_pid_param;  /* 底盘电机PID参数 */
    KPID_Params_t follow_pid_param; /* 跟随云台PID参数 */
  } pid;
  Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */

  /* 低通滤波器截至频率*/
  struct {
    float in;  /* 输入 */
    float out; /* 输出 */
  } low_pass_cutoff_freq;

  /* 电机反装，应该和云台设置相同*/
  struct {
    bool yaw;
  } reverse;
	struct {
        float max_vx, max_vy, max_wz;
        float max_current; 
    } limit; 
} Chassis_Params_t;

typedef struct {
  AHRS_Gyro_t gyro;
  AHRS_Eulr_t eulr;
} Chassis_IMU_t;

typedef struct {
  Chassis_IMU_t imu;                  // 底盘 IMU(yaw 用来跟随云台)
  MOTOR_Feedback_t motor[4];          // 四个 3508电机 反馈
} Chassis_Feedback_t;

/* 底盘输出结构体*/
typedef struct {
	float motor[4];  
} Chassis_Output_t;

/*
 * 运行的主结构体￣
 * 包含初始化参数,中间变量,输出变量
 */
typedef struct {
  uint64_t last_wakeup;
  float dt;

  Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */

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


  /* 底盘设计 */
  int8_t num_wheel; /* 底盘轮子数量 */
  Mixer_t mixer;    /* 混合器,移动向量->电机目标值 */
  MoveVector_t move_vec; /* 底盘实际的运动向量 */
  MOTOR_RM_t *motors[4];/*指向底盘每个电机参数*/
	AHRS_Eulr_t mech_zero;
  float wz_multi; /* 小陀螺旋转模式 */

  /* PID计算目标值 */
  struct {
    float motor_rpm[4]; /* 电机转速的动态数组,单位:RPM */
  } setpoint;

  /* 反馈控制用的PID */
  struct {
    KPID_t motor[4]; /* 控制轮子电机用的PID的动态数组 */
    KPID_t follow; /* 跟随云台用的PID */
  } pid;
 
	struct {
        Chassis_Limit_t vx, vy, wz; 
    } limit; 
	
  /* 滤波器 */
  struct {
    LowPassFilter2p_t in[4];  /* 反馈值滤波器 */
    LowPassFilter2p_t out[4]; /* 输出值滤波器 */
  } filter;

  Chassis_Output_t out; /* 电机输出 */
	Chassis_Feedback_t feedback;
	//float out_motor[4];
} 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,
                    AHRS_Eulr_t *mech_zero, float target_freq);

/**
 * \brief 更新底盘反馈信息
 *
 * \param c 包含底盘数据的结构体
 * \param can CAN设备结构体
 *
 * \return 运行结果
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c, const Chassis_Feedback_t *feedback);

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


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

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


/**
 * @brief 底盘功率限制
 *
 * @param c 底盘数据
 * @param cap 电容数据
 * @param ref 裁判系统数据
 * @return 函数运行结果
 */
//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
//                          const Referee_ForChassis_t *ref);


/**
 * @brief 导出底盘数据
 *
 * @param chassis 底盘数据结构体
 * @param ui UI数据结构体
 */
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);


#ifdef __cplusplus
}
#endif