#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"
#include "up_utils.h"

/*
 状态机

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

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


*/
/*共用发射，重复部分*/
typedef struct {
	  fp32 m2006_init;   // M2006初始角度
	  fp32 m2006_trig;   // M2006触发角度0,用来合并扳机
	  fp32 go_init;
	  fp32 go_pull_pos; // go上升去合并扳机，扳机位置  
    fp32 go_up_speed;	
	  fp32 go_down_speed;
	 
	  fp32 Pitch_angle;
} LaunchCfg_t;
typedef enum  {
	  Launch_Stop,
    Launch_PREPARE,        //
	  Launch_START,          //启动，从指定位置上升，扣动扳机
    Launch_TRIGGER,        //拉动到指定位置  
    Launch_SHOOT_WAIT,           // 发射等待
	  Launch_SHOOT,
	  Launch_DONE,
    
	
}LaunchState_t;
typedef struct {
    LaunchCfg_t LaunchCfg;
	  LaunchState_t LaunchState;
} LaunchContext_t;


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

/* 投球参数配置 */
typedef struct {
    
    fp32 go_init;     // GO电机触发位置,初始位置，后续更改用于发射的位置	  
	  fp32 Pitch_angle;  //俯仰角度，以oid为准	  
	  fp32 go_release_pos;//go松开，发射位置
	
} PitchCfg_t;

typedef struct {
    PitchState_t PitchState;
	  PitchCfg_t PitchConfig;
    CurveType Curve;  //当前函数，雷达距离->pos
    uint8_t is_init ;
} PitchContext_t;

/*传球过程 */
typedef enum {
	  PASS_STOP,         // 停止状态,未进入传球模式
	  PASS_IDLE,         // 空闲状态，进入未开始
    PASS_PREPARE,      // 传球准备
    PASS_START,        // 启动传球,拉动到等球位置
    PASS_POS_PREPARE,  // 传球位置准备，对准篮筐，go的位置
    PASS_COMPLETE      // 发射
} PassState_t;
typedef struct {
	
	fp32 go_wait;     // GO等待位置
	fp32 go_release_pos; // GO电机发射位置
	fp32 Pitch_angle;  // 俯仰角度，以oid为准
	fp32 go_up_speed; // GO电机上升速度
	fp32 go_down_speed; // GO电机下降速度
	
} PassCfg_t;
typedef struct {
    PassState_t PassState;
	  PassCfg_t PassCfg;
    CurveType Curve;  //当前函数，雷达距离->pos
    uint8_t is_init ;
} PassContext_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;

  pid_param_t Pitch_Angle_M2006_speed_param;
	 
	LaunchCfg_t LaunchCfg;
	PitchCfg_t  PitchCfg;
	PassCfg_t   PassCfg;
	
	
	GO_MotorCmd_t go_cmd;
}UP_Param_t;
typedef struct{

	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;

	  fp32 Shoot_M2006_angle;
		 
		fp32 go_shoot;
    fp32 go_pull_speed;
	  fp32 Pitch_angle;  //以oid的角度为准,目标俯仰角
		 
	}up_motor_target_t;

typedef 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_type_def Pitch_Angle_M2006_speed;		 
		 
		 
	}up_pid_t;

typedef struct{
	
   uint8_t up_task_run; 
   const	UP_Param_t *param; 
  LaunchContext_t LaunchContext;	
    /*投篮过程*/
	PitchContext_t PitchContext;
	/*传球过程*/
	PassContext_t PassContext;	  
	 CMD_t *cmd;	

	 struct{	
		 
	 struct{	
	  fp32 VESC_5065_M1_rpm;
	  fp32 VESC_5065_M2_rpm;
	 }VESC;
	  GO_MotorData_t go_data;//存放go电机数据

    DJmotor_feedback_t DJmotor_feedback[6];
		
    struct { 
	   uint32_t ecd;
	   float angle;
     }Encoder;	
	}motorfeedback;
 

	

  up_pid_t pid;
  up_motor_target_t motor_target;

 	GO_MotorCmd_t go_cmd;
	
		/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 DJfinal_out[6];
		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 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 DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);

int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);


#endif