#ifndef UP_H
#define UP_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 "vofa.h"	
#include "GO_M8010_6_Driver.h"
#include "bsp_usart.h"

	

typedef enum
{
	 	  Not_started_Pit=0,//未开始
	    Launch_Ready=1, //准备发射
	    Launch_complete=2,//发射完成
		  Done_Pit=3       //已完成

}Pitch_flag_t;

typedef	enum{
	  
	  Not_started_dri=0,//未开始
	  No_ball =1,  //抓上无球    
    Have_ball_F=2,  //刚开始有球
    Have_ball_S=3,  //中途有球
	  Done_dri =4         //已完成
	
}Dribble_flag_t;

/*运行控制中的控制*/
typedef struct{
	
	/*投球过程*/	
	Pitch_flag_t Pitch_flag;
	/*运球过程*/
	Dribble_flag_t Dribble_flag;
 
	int last_state;
	
} Oper_control_state_t;

typedef struct {

 BMI088_t bmi088;

 AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
	
}UP_Imu_t;
	
/**
	*@input[in]		rev 暂时不知道干啥用的，github为回答issue
	*@input[in]		T	力矩，单位N·m
	*@input[in]		Pos 目标位置，单位rad
	*@input[in]		W 目标速度，单位rad/s
	*@input[in]		K_P 位置环环kp
	*@input[in]		K_W 速度环kp
	*@note				控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
	*/
typedef struct {
                       
	int rev;         //暂时不知何用
	float T;        
	float W;
	float K_P;
	float K_W;
	
}GO_param_t;
/*角度环控制电机类型*/
typedef enum {
    M2006 = 1,
    M3508
	
} MotorType_t;



typedef struct
{	

	pid_param_t VESC_5065_M1_param;
	pid_param_t VESC_5065_M2_param;
	
	pid_param_t UP_GM6020_speed_param;
	pid_param_t UP_GM6020_angle_param;
	
	pid_param_t M2006_speed_param;	
	pid_param_t M2006_angle_param;	
	
	pid_param_t M3508_speed_param;	
	pid_param_t M3508_angle_param;	
	
	GO_param_t go_param;
	
}UP_Param_t;

typedef struct
{		
        MotorType_t motor;
	
        float orig_angle;     
        float last_angle;     
        int32_t round_cnt;    
        uint16_t init_cnt;   
				float total_angle;
	
}motor_angle_data;


typedef struct{
	
   uint8_t up_task_run; 
   const	UP_Param_t *param;  
	
	 UP_Imu_t pos088; 
	  
	/*控制及状态*/
	 CMD_t *cmd;	
	 Oper_control_state_t state;//上层机构的运行状态

	 struct{					
	  fp32 rotor_pit6020ecd;
	  fp32 rotor_pit6020rpm;

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

    motor_angle_data M2006;
		motor_angle_data M3508;
		 
		GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据

	  fp32 M3508_angle[4];
	  fp32 M3508_rpm  [4];
		 
		 int flag;
		
	}motorfeedback;
 

	
	 struct{
					
	  fp32 rotor_pit6020angle;

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  fp32 M2006_angle;
		fp32 M3508_angle;
		 
		fp32 go_shoot;
		fp32 go_spin;

	}motor_target;
  
	 struct{
		 
		 pid_type_def GM6020_speed;
		 pid_type_def GM6020_angle;
		 
		 pid_type_def VESC_5065_M1;
		 pid_type_def VESC_5065_M2;
		 
		 pid_type_def M2006_angle;
		 pid_type_def M2006_speed;
		 
		  pid_type_def M3508_angle;
		  pid_type_def M3508_speed;
				
	}pid;
 
	
	
		/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 final_3508out[4];
		fp32 final_pitchout;
		fp32 final_VESC_5065_M1out;
		fp32 final_VESC_5065_M2out;
		

	}final_out;
	
	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
	
	   fp32 vofa_send[8];
	

	
}	UP_t;


int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);

int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,int id,float pos);
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
int8_t UP_control(UP_t *u,CAN_Output_t *out);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed);



#endif