#pragma once

#ifdef __cplusplus
extern "C"
{
#endif

#include "bsp/struct_typedef.h"
#include "component/filter.h"
#include "component/pid.h"
#include "component/ahrs.h"
#include "device/buzzer.h"
#include "device/bmi088.h"
#include "component/user_math.h"
#include "component/filter.h"
#include "device/motor_rm.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 */

// m3508的电机转速转换为底盘的实际速度
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f

#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f

//     纵向/横向
#define radians atan(1.0f * 330 /330) // 角度制

	// 四个舵轮的安装误差
	typedef struct
	{
		fp32 MOTOR_OFFSET[4];
	} MotorOffset_t;

	/*底盘的类型*/
	typedef enum
	{
		CHASSIS_TYPE_MECANUM,	 /* 麦轮 */
		CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
		CHASSIS_TYPE_AGV_3,		 /* AGV舵轮 */
		CHASSIS_TYPE_AGV_4,		 /* AGV舵轮 */
	} Chassis_Type_e;

	/*底盘的电机轮组*/
	typedef enum
	{
		DJI_M3508,
		DJI_G6020,
		AGV_Group,
	} Chassis_Motortype_e;

	/*底盘模式*/
	typedef enum
	{
		STOP, // 底盘平行
		RC,	  // 遥控模式
		LOCK,  //底盘锁死
	} Chassis_mode_t;
	// typedef struct
	// {

	// 	Chassis_mode_t mode; // 底盘模式

	// } Chassis_CMD__t;

	typedef struct
	{
		int cmd_power_on_safe; // 上电安全标志

		Chassis_mode_t mode;

		// 遥控器输出值
		fp32 Vx;
		fp32 Vy;
		fp32 Vw;
		fp32 throttle;

	} Chassis_CMD_t;

	typedef struct
	{

		BMI088_t bmi088;

		/*可通过该枚举类型来决定Imu的数据量纲*/
		enum
		{
			IMU_DEGREE, // 角度值（0-360）
			IMU_RADIAN	// 弧度制（0-2pi)
		} angle_mode;

		AHRS_Eulr_t imu_eulr; // 解算后存放欧拉角（弧度制）
	} ChassisImu_t;

	/*底盘参数结构体*/
	typedef struct
	{

		//Chassis_Type_e chassis_type;	/*  */
		//Chassis_Motortype_e motor_type; /**/

		/*该部分决定PID的参数整定在config中修改*/
		KPID_Params_t C6020Omega_param;
		KPID_Params_t C6020Angle_param;
		KPID_Params_t M3508v_param;

		// KPID_Params_t Chassis_AngleAdjust_param;
		// KPID_Params_t RadarAngle_param;
		// KPID_Params_t RadarSpeed_param;
		//   KPID_Params_t SickVx_param;
		//   KPID_Params_t SickVy_param;
		//   KPID_Params_t SickVw_param;

		// KPID_Params_t M3508_param;
		// KPID_Params_t C6020pitAngle_param;
		// KPID_Params_t C6020pitOmega_param;

		MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
		MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机

	} Chassis_Param_t;

	typedef struct
	{
		fp32 Vx;
		fp32 Vy;
		fp32 Vw;
		fp32 mul; // 油门倍率
	} ChassisMove_Vec;

	typedef struct
	{
		float rotor3508_out[4];
		float rotor6020_out[4];
	} Chassis_out_t;

	typedef struct
	{
		uint64_t last_wakeup;
		float dt;

		Chassis_mode_t mode;
		ChassisMove_Vec move_vec; // 最终输入速度

		/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
		struct
		{
			fp32 rotor3508_jiesuan_1[4];
			fp32 rotor3508_jiesuan_2[4];
			fp32 rotor6020_jiesuan_1[4];
			fp32 rotor6020_jiesuan_2[4];
			// fp32 rotor6020_elur_yaw;

			fp32 motor6020_target[4];
			fp32 motor3508_target[4];

		} hopemotorout;

		struct
		{
			fp32 final_6020out[4];
			fp32 final_3508out[4];
			// fp32 final_pitchout;
		} final_out;

		struct
		{
			fp32 rotor_rpm3508[4];
			fp32 rotor_current3508[4];
			fp32 rotor_rpm6020[4];
			fp32 rotor_angle6020[4];
			fp32 rotor_current6020[4];
			fp32 rotor_temp6020[4];
		} motorfeedback;

		struct
		{
			KPID_t chassis_6020anglePid[4];
			KPID_t chassis_6020OmegaPid[4];
			KPID_t chassis_3508VPID[4];

		} pid;

		uint8_t keeping_angle_flag; // 是否处于保持角度模式

		// AHRS_Eulr_t set_point; // 底盘纠正目标角
		// fp32 angle_current;	   // 当前角度
		// fp32 angle_piancha;	   // 偏差角度
		// fp32 yaw_out;		   // 角度pid输出值

		ChassisImu_t pos088;		 // 088的实时姿态
		MotorOffset_t motoroffset;	 // 5065校准数据
		Chassis_Param_t *param;		 // 一些固定的参数
		fp32 vofa_send[8];			 // vofa输出数据
		LowPassFilter2p_t filled[11]; // 低通滤波器
		float keep_angle[4];		 // 保持的 6020 角度

		Chassis_out_t out;
	} Chassis_t;

	// typedef struct
	// {

	// 	uint64_t last_wakeup;
	// 	float dt;

	// 	Chassis_mode_t mode;
	// 	//   CMD_Chassis_navi_pos pos;

	// 	ChassisMove_Vec move_vec; // 最终输入速度

	// 	/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
	// 	struct
	// 	{
	// 		fp32 rotor3508_jiesuan_1[4];
	// 		fp32 rotor3508_jiesuan_2[4];
	// 		fp32 rotor6020_jiesuan_1[4];
	// 		fp32 rotor6020_jiesuan_2[4];
	// 		fp32 rotor6020_elur_yaw;

	// 		fp32 motor6020_target[4];
	// 		fp32 motor3508_target[4];

	// 	} hopemotorout;

	// 	/*经PID计算后的实际发送给电机的实时输出值*/
	// 	struct
	// 	{
	// 		fp32 final_6020out[4];
	// 		// fp32 final_5065out[4];
	// 		fp32 final_3508out[4];
	// 		fp32 final_pitchout;
	// 	} final_out;

	// 	/*电机反馈数据*/
	// 	struct
	// 	{
	// 		fp32 rotor_rpm3508[4];
	// 		fp32 rotor_current3508[4];

	// 		fp32 rotor_rpm6020[4];
	// 		fp32 rotor_angle6020[4];
	// 		fp32 rotor_current6020[4];
	// 		fp32 rotor_temp6020[4];

	// 	} motorfeedback;

	// 	/*pid*/
	// 	struct
	// 	{
	// 		KPID_t chassis_6020anglePid[4];
	// 		KPID_t chassis_6020OmegaPid[4];
	// 		// KPID_t Chassis_AngleAdjust;
	// 		// KPID_t chassis_RadarspeedPID[2];
	// 		//  KPID_t chassis_SickVx;
	// 		//   KPID_t chassis_SickVy;
	// 		//   KPID_t chassis_SickVw;

	// 		KPID_t chassis_3508VPID[4];
	// 		// KPID_t chassis_pitAngle6020;
	// 		// KPID_t chassis_pitOmega6020;
	// 	} pid;

	// 	// fp32 chassis_yaw;			// 码盘世界坐标系下底盘角度
	// 	uint8_t keeping_angle_flag; // 是否处于保持角度模式

	// 	AHRS_Eulr_t set_point; // 底盘纠正目标角
	// 	fp32 angle_current;	   // 当前角度
	// 	fp32 angle_piancha;	   // 偏差角度
	// 	fp32 yaw_out;		   // 角度pid输出值

	// 	ChassisImu_t pos088;		 // 088的实时姿态
	// 	MotorOffset_t motoroffset;	 // 5065校准数据
	// 	Chassis_Param_t *param;		 // 一些固定的参数
	// 	fp32 vofa_send[8];			 // vofa输出数据
	// 	LowPassFilter2p_t filled[9]; // 低通滤波器
	// 	float keep_angle[4];		 // 保持的 6020 角度

	// 	// 	BUZZER_t buzzer_radar_angle;      // 用于雷达角度纠正的蜂鸣器控制
	// 	//   BUZZER_t buzzer_nuc_flag;         // 用于nuc校准成功的蜂鸣器控制

	// } Chassis_t;
	int8_t chassis_init(Chassis_t *c,  Chassis_Param_t *param, float target_freq);
	int8_t Chassis_update(Chassis_t *c);
	int8_t Chassis_Control(Chassis_t *c,  Chassis_CMD_t *c_cmd);
	void Chassis_Setoutput(Chassis_t *c);
#ifdef __cplusplus
}
#endif