/*
 * far♂蛇模块
 */

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "main.h"
#include <stdbool.h>
#include "component/pid.h"
#include "device/motor_rm.h"


/* Exported constants ------------------------------------------------------- */
#define SHOOT_OK (0)        /* 运行正常 */
#define SHOOT_ERR (-1)      /* 运行时发现了其他错误 */
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */

#define SHOOT_FRIC_NUM (6) /* 摩擦轮数量 */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 5000.0f
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
    SHOOT_JAMFSM_STATE_NORMAL = 0,      // 常规状态
    SHOOT_JAMFSM_STATE_SUSPECTED,       // 怀疑状态
    SHOOT_JAMFSM_STATE_CONFIRMED,       // 确认状态
    SHOOT_JAMFSM_STATE_DEAL             // 处理状态
} Shoot_JamDetectionFSM_State_t;
typedef enum {
    SHOOT_STATE_IDLE = 0,      // 熄火
    SHOOT_STATE_READY,         // 准备射击
    SHOOT_STATE_FIRE           // 射击
} Shoot_Running_State_t;

typedef enum {
    SHOOT_MODE_SAFE = 0,      // 安全模式
    SHOOT_MODE_SINGLE,         // 单发模式
    SHOOT_MODE_BURST,          // 多发模式
    SHOOT_MODE_CONTINUE         // 连发模式
} Shoot_Mode_t;
typedef struct {
  bool online;	
	
  bool ready;                    /* 准备射击 */
  bool firecmd;                  /* 射击指令 */

} Shoot_CMD_t;
typedef struct {
  MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
  MOTOR_Feedback_t trig; /* 拨弹电机反馈 */

  float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
  float fil_trig_rpm; /* 滤波后的拨弹电机转速*/

  float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
  float fric_avgrpm; /* 归一化摩擦轮平均转速*/
  float trig_rpm; /* 归一化拨弹电机转速*/

}Shoot_Feedback_t;

typedef struct{
  float   time_last_shoot;
  uint8_t num_to_shoot;
  uint8_t num_shooted;
}Shoot_AngleCalu_t;

typedef struct {
  bool jam_detected; /* 卡弹检测结果 */
  float jam_last_time;/* 用于记录怀疑状态或处理状态的开始时间 */
  Shoot_JamDetectionFSM_State_t jamfsm_state; /* 卡弹检测状态机 */
}Shoot_JamDetection_t;
typedef struct {
  float out_follow[SHOOT_FRIC_NUM];
  float out_err[SHOOT_FRIC_NUM];
  float out_fric[SHOOT_FRIC_NUM];
  float lpfout_fric[SHOOT_FRIC_NUM];

  
  float outagl_trig;
  float outomg_trig;
  float outlpf_trig;
}Shoot_Output_t;


/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
  float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
  float shot_delay_time; /* 射击间隔时间，单位秒 */
  uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */

  float jam_threshold; /* 卡弹检测阈值，单位A */
  float jam_suspected_time; /* 卡弹怀疑时间，单位秒 */

  MOTOR_RM_Param_t          fric_motor_param[SHOOT_FRIC_NUM];
  MOTOR_RM_Param_t trig_motor_param;
  

  KPID_Params_t fric_follow;               /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
  KPID_Params_t fric_err;               /* 摩擦轮电机PID控制参数，用于消除转速误差 */
  KPID_Params_t trig;              /* 拨弹电机PID控制参数 */
  KPID_Params_t trig_omg;              /* 拨弹电机PID控制参数 */

  /* 低通滤波器截止频率 */
  struct {
    struct{
    float in;  /* 反馈值滤波器 */
    float out; /* 输出值滤波器 */
    }fric;
    struct{
    float in;  /* 反馈值滤波器 */
    float out; /* 输出值滤波器 */
    }trig;
  } filter;
} Shoot_Params_t;

/*
 * 运行的主结构体，所有这个文件里的函数都在操作这个结构体
 * 包含了初始化参数，中间变量，输出变量
 */
typedef struct {
  bool                      online;//待完善，电机或遥控器在线检测

  float                     now;                           /* 当前时间，单位秒 */
  uint64_t                  lask_wakeup;                   /* 上次唤醒时间，单位微秒 */
  float                     dt;                            /* 两次唤醒间隔时间，单位秒 */
  Shoot_Params_t            *param;                        /* 发射参数 */
  /* 模块通用 */           
  Shoot_Running_State_t     running_state;                 /* 运行状态机 */
  Shoot_Mode_t              mode;                          /* 射击模式 */
  /* 反馈信息 */           
  Shoot_Feedback_t          feedback;                      /* 反馈信息 */
  /* 控制信息*/          
  Shoot_JamDetection_t      jamdetection;          
  Shoot_AngleCalu_t         anglecalu;           
  Shoot_Output_t            output;          
  /* 目标控制量 */           
  struct {           
    float                   target_rpm;                    /* 目标摩擦轮转速 */
    float                   target_angle;                  /* 目标拨弹位置 */
  }target_variable;

  /* 反馈控制用的PID */
  struct {
    KPID_t                  fric_follow[SHOOT_FRIC_NUM];   /* */
    KPID_t                  fric_err[SHOOT_FRIC_NUM];      /*  */
    KPID_t                  trig;
    KPID_t                  trig_omg;
  } pid;

  /* 滤波器 */
  struct {
    struct{
    LowPassFilter2p_t       in[SHOOT_FRIC_NUM];            /* 反馈值滤波器 */
    LowPassFilter2p_t       out[SHOOT_FRIC_NUM];           /* 输出值滤波器 */
    }fric;
    struct{
    LowPassFilter2p_t       in;                            /* 反馈值滤波器 */
    LowPassFilter2p_t       out;                          /* 输出值滤波器 */
    }trig;
  } filter;

  float errtosee; /*调试用*/

} Shoot_t;

/* Exported functions prototypes -------------------------------------------- */

/**
 * \brief 初始化发射
 *
 * \param s 包含发射数据的结构体
 * \param param 包含发射参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);

/**
 * \brief 更新反馈
 *
 * \param s 包含发射数据的结构体
 *
 * \return 函数运行结果
 */
int8_t Chassis_UpdateFeedback(Shoot_t *s);

/**
 * \brief 初始化发射
 *
 * \param s 包含发射数据的结构体
 * \param cmd 包含发射命令的结构体
 *
 * \return 函数运行结果
 */
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);



#ifdef __cplusplus
}
#endif