/*
 * 射击模组
 */

/* Includes ----------------------------------------------------------------- */
#include "shoot.h"

#include "bsp/pwm.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "component/pid.h"
#include "component/limiter.h"
#include "component/user_math.h"
#include "device/motor_rm.h"
#include <stdint.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */

#define BULLET_SPEED_LIMIT_17MM (25.0)

/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */

/**
 * \brief 设置射击模式
 *
 * \param c 包含射击数据的结构体
 * \param mode 要设置的模式
 *
 * \return 函数运行结果
 */
static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) {
  if (s == NULL) return -1;

  if (mode == s->mode) return SHOOT_OK;

  /* 切换模式后重置PID和滤波器 */
//   for (uint8_t i = 0; i < 2; i++) {
//     PID_Reset(s->pid.fric + i);
//     LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f);
//     LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f);
//   }
//   PID_Reset(&(s->pid.trig));
//   LowPassFilter2p_Reset(&(s->filter.in.trig), 0.0f);
//   LowPassFilter2p_Reset(&(s->filter.out.trig), 0.0f);
    PID_Reset(&(s->pid.fric[0]));
    PID_Reset(&(s->pid.fric[1]));
    LowPassFilter2p_Reset(&(s->filter.in.fric[0]), 0.0f);
    LowPassFilter2p_Reset(&(s->filter.in.fric[1]), 0.0f);
    LowPassFilter2p_Reset(&(s->filter.out.fric[0]), 0.0f);
    LowPassFilter2p_Reset(&(s->filter.out.fric[1]), 0.0f);
    PID_Reset(&(s->pid.trig));
    LowPassFilter2p_Reset(&(s->filter.in.trig), 0.0f);
    LowPassFilter2p_Reset(&(s->filter.out.trig), 0.0f);

//   while (fabsf(CircleError(s->setpoint.trig_angle, s->feedback.trig_angle,
//                            M_2PI)) >= M_2PI / s->param->num_trig_tooth / 2.0f) {
//     CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
//               M_2PI);
//   }

  if (mode == SHOOT_MODE_LOADED) s->fire_ctrl.to_shoot = 0;
  s->mode = mode;
  return 0;
}

// static float Shoot_CalcRPMFromSpeed(float speed, float radius, float *rpm) {
//   if (rpm == NULL) return -1;
//   if (radius <= 0.0f) return -2;

//   /* 限制弹丸速度 */
//   speed = Limit_Abs(speed, BULLET_SPEED_LIMIT_17MM);

//   /* 计算转速 */
//   *rpm = speed / (2.0f * M_PI * radius) * 60.0f;

//   return 0;
// }

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

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

  s->param = param;           /* 初始化参数 */
  s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */

  PID_Init(&(s->pid.fric[0]), KPID_MODE_NO_D, target_freq,
           &(param->fric_pid_param));
  PID_Init(&(s->pid.fric[1]), KPID_MODE_NO_D, target_freq,
           &(param->fric_pid_param));
  PID_Init(&(s->pid.trig), KPID_MODE_CALC_D, target_freq,
           &(param->trig_pid_param));  

    LowPassFilter2p_Init(&(s->filter.in.fric[0]), target_freq,
                         param->low_pass_cutoff_freq.in.fric);      
    LowPassFilter2p_Init(&(s->filter.in.fric[1]), target_freq,
                         param->low_pass_cutoff_freq.in.fric);      
    LowPassFilter2p_Init(&(s->filter.out.fric[0]), target_freq,
                         param->low_pass_cutoff_freq.out.fric);      
    LowPassFilter2p_Init(&(s->filter.out.fric[1]), target_freq,
                         param->low_pass_cutoff_freq.out.fric);      
    LowPassFilter2p_Init(&(s->filter.in.trig), target_freq,
                         param->low_pass_cutoff_freq.in.trig);
    LowPassFilter2p_Init(&(s->filter.out.trig), target_freq,
                         param->low_pass_cutoff_freq.out.trig);

    BSP_CAN_Init();
    MOTOR_RM_Register(s->param->fric_motor_param[0]);
    MOTOR_RM_Register(s->param->fric_motor_param[1]);
    MOTOR_RM_Register(s->param->trig_motor_param);

  return SHOOT_OK;
}

/**
 * \brief 更新射击的反馈信息
 *
 * \param s 包含射击数据的结构体
 *
 * \return 函数运行结果
 */
int8_t Shoot_UpdateFeedback(Shoot_t *s) {
  if (s == NULL) return -1;
    MOTOR_RM_Update(s->param->fric_motor_param[0]);
    MOTOR_RM_Update(s->param->fric_motor_param[1]);
    MOTOR_RM_Update(s->param->trig_motor_param);

    MOTOR_RM_t *motor;
    motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[0]);
    s->feedback.fric[0] = motor->feedback;
    motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[1]);
    s->feedback.fric[1] = motor->feedback;
    motor = MOTOR_RM_GetMotor(s->param->trig_motor_param);
    s->feedback.trig = motor->feedback;

  return SHOOT_OK;
}

/**
 * \brief 运行射击控制逻辑
 *
 * \param s 包含射击数据的结构体
 * \param s_cmd 射击控制指令
 *
 * \return 函数运行结果
 */
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *s_cmd) {
  if (s == NULL) return -1;
  if (s_cmd == NULL) return -1;

  Shoot_SetMode(s, s_cmd->mode); /* 设置射击模式 */

  /* 计算摩擦轮转速的目标值 */
  // 根据弹丸射速计算转速，这里需要实现具体的计算逻辑
  // s->setpoint.fric_rpm[0] = calculate_rpm_from_speed(s_cmd->bullet_speed);
  // s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[0];

  switch (s->mode) {
    case SHOOT_MODE_RELAX:
      MOTOR_RM_SetOutput(s->param->fric_motor_param[0], 0.0f);
      MOTOR_RM_SetOutput(s->param->fric_motor_param[1], 0.0f);
      MOTOR_RM_SetOutput(s->param->trig_motor_param, 0.0f);
      break;
    case SHOOT_MODE_SAFE:
    /*摩擦轮速度环到0，拨弹位置环到设定位置*/
        s->setpoint.fric_rpm[0] = 0.0f;
        s->setpoint.fric_rpm[1] = 0.0f;


      
      break;
      
    case SHOOT_MODE_LOADED:
      // TODO: 实现上膛模式的控制逻辑
      break;
  }
  return SHOOT_OK;
}

/**
 * \brief 射击输出值
 *
 * \param s 包含射击数据的结构体
 */
void Shoot_Output(Shoot_t *s) {
  if (s == NULL) return;
  MOTOR_RM_Ctrl(s->param->fric_motor_param[0]);
}