#ifndef NAVI_H
#define NAVI_H


#include "Action.h"
#include "pid.h"
#include "ahrs.h"
#include "bmi088.h"
#include "map.h"
#include "can_use.h"
#include "bsp_delay.h"

#define PI 3.1415926535f

typedef struct{
	float pos_x;
	float pos_y;
	float last_pos_x;
  float	last_pos_y;
}chassis_position_t;

typedef struct {
		fp32 vx;
		fp32 vy;
		fp32 vz;
}action_out_t;
typedef struct {
		fp32 vx;
		fp32 vy;
		fp32 vz;
}sick_out_t;
	 
typedef struct{

   pid_param_t path_speed_pid_param; 
   pid_param_t pid_PosSpeed_xy_param;                                                                   
   pid_param_t pid_OutAngle_param;                    
   pid_param_t pid_InnerAngle_param;                      
   pid_param_t pid_OutAngle_hold_param;                   
   pid_param_t pid_InnerAngle_hold_param;                                     
   pid_param_t pid_pos_xy_inner_param; 
   pid_param_t pid_pos_xy_outer_param;  
	 pid_param_t pid_sick_out_param; 
   pid_param_t pid_sick_inner_param;  

}ops_pid_param_t;

typedef struct {
	ops_pid_param_t ops_pid;
	point_t *path;
	int8_t path_num;
}ops_param_t;

typedef enum {
    normal = 0,    // sick正常
    blocked,       // sick被阻挡
    max_reached    // sick测距已达最大值
} sick_status_t;

typedef struct {
    
    int32_t sick_distance[4];       // 当前距离值
    int32_t sick_distance_last[4];  // 上一次的距离值
	  int32_t sick_distance_diff[4];  //距离差值
    fp32  sick_derivative[4];       // 存储微分结果
	  sick_status_t status[4];        // 每个距离传感器的状态标志位
} sick_d_t;

/**
 * @brief 
 *
*/
typedef struct {
	
	float chassis_speed_set;           //解算出来的底盘速度
	
	struct 
	 {
		action_out_t action;
		sick_out_t sick; 
	 }final_out;

	 int16_t sick_distance[4];	      
	
  sick_d_t sick_d;
	  
  int navi_flag; 		
	int sick_flag;	
	 
	Action_POS_t *chassis_pos;         //码盘数据
	
 /*sick纠正后码盘的数据*/
 struct 
	 {
		fp32 x;
		fp32 y;
	 }sick_offest;
	 
	Action_POS_t *chassis_pos_offest; 
	
  Path_t *chassis_map;	              //点位数据
	 
  const ops_param_t *param;	
	
  AHRS_Gyro_t ops_gyro;

	struct{	 
	  pid_type_def path_speed_pid;
	  pid_type_def pid_PosSpeed_xy[2];                       
	  pid_type_def pid_OutAngle;
	  pid_type_def pid_InnerAngle;
	  pid_type_def pid_OutAngle_hold;
	  pid_type_def pid_InnerAngle_hold;	
	  pid_type_def pid_pos_xy_outer;
	  pid_type_def pid_pos_xy_inner;
	  pid_type_def pid_sick_outer;
	  pid_type_def pid_sick_inner;
	}pid;
	 
}ops_t;

int8_t Navi_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
bool isArrive(point_t p,ops_t *o);
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set);
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle,pid_type_def *outer_yaw_pid_set,pid_type_def *inner_yaw_pid_set);
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set);
void sick_point(ops_t *o);
void sick_calculate_d(ops_t *o,const CAN_t *can);
void go_path(ops_t *o,CMD_ACTION_t *ops_out);
#endif

