#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"

/*
 状态机

状态结构体，触发标志位结构体

配置层，-->config.c
  |
中间层，-->up.h,统一UP_t
  |
运行函数，switch(状态) 运行相应函数











*/




/* 投球状态定义 */
typedef enum {
    PITCH_PREPARE,          // 准备阶段
	  PITCH_START,            //启动，拉扳机
    PITCH_PULL_TRIGGER,  
    PITCH_LAUNCHING,        // 发射中
    PITCH_COMPLETE          // 完成
} PitchState_t;

/* 投球参数配置 */
typedef struct {
    fp32 m2006_init_angle;      // M2006初始角度-140
	  fp32 m2006_trigger_angle;   // M2006触发角度0,用来合并扳机
    fp32 go1_init_position;     // GO电机触发位置,0,初始位置
	  fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
	  fp32 m2006_Screw_init;
} PitchConfig_t;

/* 投球控制上下文 */
typedef struct {
    PitchState_t PitchState;
	  PitchConfig_t PitchConfig;

    uint8_t is_initialized ;
} PitchContext_t;








/*运球*/
typedef enum {
	  Dribble_PREPARE,
    STATE_GRAB_BALL,        // 夹球升起阶段
    STATE_RELEASE_BALL,     // 释放球体
    STATE_CATCH_PREP,       // 接球准备
    STATE_CATCH_BALL,       // 接球动作
	  STATE_TRANSFER,         //转移球
	  STATE_CATCH_DONE        //完成
} DribbleState_t;

/* 参数配置结构体 */
typedef struct {
    fp32 m3508_init_angle;     // 3508初始角度
    fp32 m3508_high_angle;     // 3508升起角度
    fp32 go2_init_angle;       // GO2初始角度
    fp32 go2_flip_angle;       // GO2翻转角度
    fp32 flip_timing;       // 翻转触发时机
    fp32 go2_release_threshold;    // 释放球
} DribbleConfig_t;

/* 状态机上下文 */
typedef struct {
    DribbleState_t DribbleState;
	  DribbleConfig_t DribbleConfig;
	   
    uint8_t is_initialized;

} DribbleContext_t;


typedef struct {

 BMI088_t bmi088;

 AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
	
}UP_Imu_t;
	 
typedef struct
{	

	pid_param_t VESC_5065_M1_param;
	pid_param_t VESC_5065_M2_param;
	
	pid_param_t Shoot_M2006_speed_param;	
	pid_param_t Shoot_M2006_angle_param;	
	
	pid_param_t Pitch_M2006_speed_param;	
	pid_param_t Pitch_M2006_angle_param;	
	
	DribbleConfig_t DribbleConfig_Config;
	PitchConfig_t  PitchConfig_Config;
	
	GO_MotorCmd_t go_cmd;
}UP_Param_t;

typedef struct
{	
	  fp32 ecd;
	  fp32 rpm;
		 
    fp32 orig_angle;     
    fp32 last_angle;     
    int32_t round_cnt;    
    uint16_t init_cnt;   
	  fp32 total_angle;	
}DJmotor_feedback_t;


typedef struct{
	
   uint8_t up_task_run; 
   const	UP_Param_t *param;  
		/*运球过程*/
  DribbleContext_t DribbleContext;
    /*投篮过程*/
	PitchContext_t PitchContext;
	
	
	 UP_Imu_t pos088; 
	  
	 CMD_t *cmd;	

	 struct{					

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  GO_MotorData_t *go_data;//存放go电机数据

    DJmotor_feedback_t DJmotor_feedback[4];
		 
		
	}motorfeedback;
 

	
	 struct{

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  fp32 Shoot_M2006_angle;
		fp32 Pitch_M2006_angle;
		 
		fp32 go_shoot;

	}motor_target;
  
	 struct{
		 
		 
		 pid_type_def VESC_5065_M1;
		 pid_type_def VESC_5065_M2;
		 
		 pid_type_def Shoot_M2006angle;
		 pid_type_def Shoot_M2006speed;
		 
		 pid_type_def Pitch_M2006_angle;
		 pid_type_def Pitch_M2006_speed;
		 
	}pid;
 	   GO_MotorCmd_t go_cmd;

		 

	
		/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 DJfinal_out[4];
		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 VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,float pos,float limit);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);


#endif