#ifndef NAVI_H
#define NAVI_H


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

typedef enum {
    AUTO_MODE, // 自动跑点模式
    MANUAL_MODE ,  // 控制模式
	  NO_MODE
} Navi_Mode_t;

typedef struct {
	
   Navi_Mode_t mode; 
		 
	 int Trig_flag;//触发沿，初始值为零，两种控制模式下，这个值变为1，会使cnt+1，为-1，使cnt-1
	
} Navi_COT_MODE_t;


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

typedef enum{
	YES,
	NO,
	NEXT1,
	NEXT2
}is_cpt_e;
typedef struct{
   pid_param_t pid_PosSpeed_x_param;                 
   pid_param_t pid_PosSpeed_y_param;                
   pid_param_t pid_pos_x_out_param;                   
   pid_param_t pid_pos_x_inner_param;                 
   pid_param_t pid_pos_y_out_param;                   
   pid_param_t pid_pos_y_inner_param;                 
   pid_param_t pid_OutAngle_param;                    
   pid_param_t pid_InnerAngle_param;                   
   pid_param_t path_speed_pid_param;                  

}ops_pid_param_t;

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

/**
 * @brief 
 *
*/
typedef struct {
	Action_POS_t *chassis_pos;          
	fp32 current_x;														 	
	fp32 current_y;														

	fp32 next_mapx;
	fp32 next_mapy;
  const point_t *chassis_map;		
  const ops_param_t *param;	
	PathState_t state;
	Navi_COT_MODE_t Navi_Mode; 
	
	 AHRS_Eulr_t ops_imu_pos;
   AHRS_Gyro_t ops_gyro;


	
	pid_type_def pid_PosSpeed_x;              
	pid_type_def pid_PosSpeed_y;	         
	pid_type_def pid_pos_x_out;
	pid_type_def pid_pos_x_inner;
	pid_type_def pid_pos_y_out;
	pid_type_def pid_pos_y_inner;
	pid_type_def pid_OutAngle;
	pid_type_def pid_InnerAngle;
	pid_type_def path_speed_pid;


	is_cpt_e POS_IS_CPT;                     
   
		 struct 
	 {
		fp32 vx;
		fp32 vy;
		fp32 yaw_angle;
	 }final_out;
	 
}ops_t;




int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);

int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
#endif