#ifndef __WP_DATATYPE_H
#define __WP_DATATYPE_H


#include "math.h"
#include "wp_math.h"

enum 
{
	_UP=0,
	_EAST,
	_NORTH
};

enum 
{
	_ROL=0,
	_PIT,
	_YAW
};


#define DEG2RAD (PI / 180.0f)
#define RAD2DEG (180.0f / PI)


#define int16  short
#define uint16 unsigned short
#define int32  int
#define uint32 unsigned int
#define uint8  unsigned char
#define s32    int32	
#define u32    uint32_t	

#define Int_Sort (int16_t)
#define TRUE  true
#define FALSE false
	
typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus;


typedef union
{
	unsigned char Bdata[4];
	float Fdata;
	uint32_t Idata;
}FloatByteInt;



typedef struct
{
	int16_t x;
	int16_t y;
	int16_t z;	
}vector3i;



//typedef struct
//{
//  volatile float Last_Time;
//  volatile  float current_time;
//  volatile  float period;
//  volatile  uint16_t period_INT;//单位ms
//}systime;


typedef struct
{
  volatile float last_time;
  volatile float current_time;
  volatile float period;
  volatile uint16_t period_int;//单位ms
}systime;



typedef struct
{
  int32_t x;
  int32_t y;
}vector2i;

typedef struct
{
  float x;
  float y;
}vector2f;


typedef struct
{
  float x;
  float y;
  float z;
}vector3f;

typedef struct
{
  float pos;
  float vel;
  float acc;
}vector3f_s;


typedef struct
{
  float E;
  float N;
  float U;
}vector3f_nav;

typedef struct
{
  float E;
  float N;
}vector2f_nav;



typedef struct
{
  int32_t lat;
  int32_t lng;
}vector2i_nav;





typedef struct
{
  float Pit;
  float Rol;
}vector2f_ang;



typedef struct
{
  float Pit;
  float Rol;
  float Yaw;
}vector3f_b;


typedef struct
{
  vector3f a;
  vector3f b;
  vector3f c;
}Matrix3f;




typedef struct
{
  vector3f gyro_raw;
  vector3f accel_raw;
  vector3f mag_raw;
  vector3f last_mag_raw;
	float gyro_mode;
	float mag_intensity;
	float acce_filter[3];
	float acce_filter_fb[3];
	vector3f gyro_raw_filter;
	float temperature[2];
	float last_temperature;
  float baro_temp_raw;
	float baro_pressure_raw;
	float baro_presure_offset;
	float baro_altitude;
	float last_baro_altitude;
	float baro_altitude_div;
	float last_baro_altitude_div;
	float baro_altitude_acc;
	uint8_t imu_updtate_flag;
	uint8_t baro_updtate_flag;
	uint8_t us100_updtate_flag;
	uint8_t tofsensor_updtate_flag;
	uint8_t vl53l1x_updtate_flag;
  float _temperature;
	float gyro_scale_raw_to_dps;
	float accel_scale_range_g;
	
	float distance_3d_cm;
			//
	float slam_yaw_init,slam_yaw;//slam定位初始偏航角零偏
	uint8_t slam_yaw_setup;//slam定位初始偏航角设定标志位
}Sensor;

typedef struct
{
	float rMat[9];
	float sin_rpy[3];
	float cos_rpy[3];
	float q[4]; 				//不含磁力计修正的四元数
	float quaternion[4];//含磁力计修正的四元数
	float pitch,roll,yaw;	
	float rpy_gyro_dps[3];
	float rpy_gyro_rps[3];
	vector3f accel_mpss;
	float ef_yaw_gyro_dps;
	vector3f_b direct_integration_angle;
	vector3f accel_scale;
	vector3f accel_offset;
	/***************************************************/
	float rpy_contrast_deg[3];
	float rpy_obs_deg[3];//观测量姿态角度
	float rpy_deg[3];//融合输出姿态角度
	vector3f gyro_dps;//单位为deg/s
	vector3f accel_g; //单位为g
	vector3f mag;
	vector3f accel_earth_g;
	uint8_t quaternion_init_ok;
	float quaternion_init[4];//初始四元数
	uint8_t temperature_stable_flag;
	uint8_t player_level;
}AHRS;



#define Axis_Num  3
#define Num  20
typedef struct
{
 float position[Axis_Num];//位置估计量
 float speed[Axis_Num];//速度估计量
 float accel_cmpss[Axis_Num];//加速度估计量
 float pos_backups[Axis_Num][Num];//历史惯导位置
 float vel_backups[Axis_Num][Num];//历史惯导速度
 float accel_backups[Axis_Num][Num];//历史惯导速度
 float acce_bias[Axis_Num];//惯导加速度漂移量估计量
 float acce_bias_All[Axis_Num];//惯导加速度漂移量估计量
 float last_accel_cmpss[Axis_Num];
 float last_speed[Axis_Num];
	
 float ins_accel_cmpss[Axis_Num];
 float _position[3];
 float _speed[3];
}SINS;



typedef struct
{
  uint8_t Mpu_Health;
  uint8_t Baro_Health;
  uint8_t Mag_Health;
  uint8_t Gps_Health;
  uint8_t Ground_Health;
}Sensor_Health;


typedef enum 
{
	NO_SLAM=0,
	LIDAR_2D_SLAM,	//位置导航模式
	T265_SLAM,    //T265定点模式
	LOAM,	         
}slam_sensor_mode;

typedef struct
{
	float position_x;
	float position_y;
	float position_z;
	float velocity_x;
	float velocity_y;
	float velocity_z;
	float q[4];
	float quality;
	uint8_t update_flag;
	uint8_t byte[8];
	float rpy[3];
	
	uint8_t rec_update_flag;
	uint8_t rec_head_update_flag;
	uint8_t valid;
	uint8_t fault,last_fault;
	slam_sensor_mode slam_sensor;
	uint8_t loam_update_flag;
}third_party_state;



typedef enum 
{
  BODY_FRAME=0,//机体坐标系
	MAP_FRAME,	 //导航坐标系
}Navigation_Frame;

typedef enum 
{
  RELATIVE_MODE=0,//相对模式
	GLOBAL_MODE,	  //全局模式
	CMD_VEL_MODE,   //速度模式
	TRANSITION_MODE //过渡模式
}Navigation_Mode;

typedef struct{
	uint16_t number;								//任务编号
	float x;												//x方向位置/速度期望
	float y;												//y方向位置/速度期望
	float z;												//z方向位置/偏航角速度期望
	uint8_t nav_mode;								//导航模式
	uint8_t frame_id;								//坐标系类型
	
	uint8_t update_flag;						//指令更新标志位
	uint8_t ctrl_finish_flag;				//控制完毕标志位
	
	uint16_t cnt; 									//位置控制完毕判断计数器
	float dis_cm;										//总位置偏差
	const float dis_limit_cm;       //位置阈值
	const float cmd_vel_max;        //最大线速度限制
	const float cmd_angular_max;    //最大角速度限制
	
	uint8_t cmd_vel_update;					//速度控制指令更新标志位
	float cmd_vel_x;								//期望x方向速度
	float cmd_vel_y;                //期望y方向速度
	float cmd_vel_angular_z;				//期望偏航方向角速度
	uint32_t cmd_vel_during_cnt;    //速度控制计数器
	uint8_t cmd_vel_suspend_flag;   //控制中止标志位
}nav_ctrl;

typedef struct
{
	float p[3][3]; //协方差矩阵
	float qp,qv;   //加速度计过程噪声
	float	qb;      //加速度计过程偏移噪声
	float rp,rv;   //位置观测噪声、速度观测噪声
	float cp[2],cv[2],cb[2];//修正量
	float k[3][2]; ////增益矩阵
	float err[2][2];
	uint8_t init;
}kalman_filter;

#endif