#ifndef CHASSIS_H
#define CHASSIS_H

#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "user_math.h"
#include "ahrs.h"
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#include "bsp\pwm.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 PI 3.1415926535f

//小量程sick1分米大概变30，大量程sick1分米大概变60
#define	SICKXY_ERROR 10
#define	SICKW_ERROR 5

//     纵向/横向
#define radians atan(1.0*422/440)//角度制
 //四个舵轮的安装误差
   typedef struct {
  fp32 MOTOR_OFFSET[4];  
} MotorOffset_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 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;

/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct
{
  Chassis_Type_e chassis_type; /*  */
  Chassis_Motortype_e motor_type; /**/
	
	/*该部分决定PID的参数整定在config中修改*/
	pid_param_t C6020Omega_param;        
  pid_param_t C6020Angle_param; 
  pid_param_t Chassis_AngleAdjust_param; 	
  pid_param_t RadarAngle_param;
  pid_param_t RadarSpeed_param;	
  pid_param_t SickVx_param;
  pid_param_t SickVy_param;	
  pid_param_t SickVw_param;

  pid_param_t M3508_param;	
  pid_param_t C6020pitAngle_param;
  pid_param_t C6020pitOmega_param;		
}Chassis_Param_t;
/*该结构体用于底盘的期望运动向量*/
typedef struct 
{
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;
	 fp32 mul;//油门倍率
}ChassisMove_Vec;


/**
 *  @brief  
 * 
 */
typedef struct{

	CMD_Chassis_mode mode;   
  CMD_Chassis_navi_pos pos;     	

  ChassisMove_Vec move_vec;         //最终输入速度 
	
	fp32 radar_angle;                 //雷达纠正角度输出值(锁框)
	fp32 radar_yaw;                   //雷达纠正角度输出值(锁车)
	
  /*sick跑点数据*/
	fp32 sick[3];
	fp32 sick_set[3];   
	ChassisMove_Vec move_sick;        //sick跑点速度		
	uint8_t radar_reset_flag;         //雷达校准标志位
	
	/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
	struct{
	fp32 rotor5065_jiesuan_1[4];
	fp32 rotor5065_jiesuan_2[4];
	fp32 rotor6020_jiesuan_1[4];
	fp32 rotor6020_jiesuan_2[4];
	fp32 rotor6020_elur_yaw;	 	
		
	fp32 motor6020_target[4];
	fp32 motor5065_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];
		
	  fp32 rotor_rpm5065[4];
	  fp32 torque_current5065[4];
		fp32 rotor_5065pos[4];	
	}motorfeedback;
	
		/*pid*/
	struct{
  	pid_type_def chassis_6020anglePid[4];                       
	  pid_type_def chassis_6020OmegaPid[4];  
    pid_type_def Chassis_AngleAdjust;		
	  pid_type_def chassis_RadaranglePID[2]; 
    pid_type_def chassis_RadarspeedPID[2];  			
	  pid_type_def chassis_SickVx;
	  pid_type_def chassis_SickVy;		
	  pid_type_def chassis_SickVw;
		
	  pid_type_def chassis_3508VecPID[4];
	  pid_type_def chassis_pitAngle6020;
	  pid_type_def 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校准数据
  const	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;

/**
 * @brief 
 *
 * @param c 
 * @param param 
 * @param mech_zero 
 * @param wheelPolar 
 * @return 
 */
 int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);


/**
 * \brief 

 */
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);

void radar_sick_calibration(Chassis_t *c,CMD_t *ctrl);
/**
 * \brief 

 */
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out, Action_POS_t*pos);

#endif
