#ifndef NAVI_H
#define NAVI_H


#include "Action.h"
#include "pid.h"
#include "ahrs.h"
#include "bmi088.h"
#include "map.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;
	Action_POS_t *chassis_pos;   

 struct 
	 {
		fp32 x;
		fp32 y;
	 }sick_offest;
	 
	Action_POS_t *chassis_pos_offest; 
	
  Path_t *chassis_map;	

   point_t transformed_target;
	 
  const ops_param_t *param;	
	
	 AHRS_Eulr_t ops_imu_pos;
   AHRS_Gyro_t ops_gyro;

	  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;
	 
	pid_type_def path_speed_pid;
	pid_type_def pid_PosSpeed_xy;                       
	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_inner;
	pid_type_def pid_pos_xy_outer;	
	pid_type_def pid_sick_outer;
	pid_type_def pid_sick_inner;
	 
}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);
void go_path(ops_t *o,CMD_ACTION_t *ops_out);
#endif