#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "bsp/struct_typedef.h"
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/mixer.h"
#include "component/pid.h"
#include "component/user_math.h"
#include "device/bmi088.h"
#include "device/motor_rm.h"
#include "module/cmd.h"

#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2)
#define CHASSIS_ERR_MODE (-3) /* 底盘模式错误：CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 底盘类型错误：Chassis_Type_t */


typedef enum { 
  CHASSIS_MODE_RELAX, /* 放松模式，电机不输出，一般用于底盘初始化后 */
  CHASSIS_MODE_MOVE
} Chassis_Mode_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, /* X 型摆放的四个全向轮 */
  CHASSIS_TYPE_OMNI_PLUS,  /* 十字型摆放的四个全向轮 */
  CHASSIS_TYPE_DRONE,      /* 无人机底盘 */
  CHASSIS_TYPE_SINGLE,     /* 单个麦克纳姆轮 */
} Chassis_Type_t;


/* 底盘参数结构体，初始化所需参数 */
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 {
  MOTOR_Feedback_t motor[4];          // 四个 3508 电机反馈
	float encoder_gimbalYawMotor;
} 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];/* 指向底盘各个电机参数 */
	float 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;

int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param,
                    float target_freq);

int8_t Chassis_UpdateFeedback(Chassis_t *c);
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now);

void Chassis_Output(Chassis_t *c);
void Chassis_ResetOutput(Chassis_t *c);
#ifdef __cplusplus
}
#endif