/*
   Modified from
   https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp

   参考资料：
   https://github.com/PX4/Firmware/issues/12362
   https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
   https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
   https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
   https://en.wikipedia.org/wiki/PID_controller
   http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/

#include "pid.h"

#define SIGMA 0.000001f

/**
 * @brief 初始化PID
 *
 * @param pid PID结构体
 * @param mode PID模式
 * @param sample_freq 采样频率
 * @param param PID参数
 * @return int8_t 0对应没有错误
 */
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
                const KPID_Params_t *param) {
  if (pid == NULL) return -1;

  if (!isfinite(param->p)) return -1;
  if (!isfinite(param->i)) return -1;
  if (!isfinite(param->d)) return -1;
  if (!isfinite(param->i_limit)) return -1;
  if (!isfinite(param->out_limit)) return -1;
  pid->param = param;

  float dt_min = 1.0f / sample_freq;
  if (isfinite(dt_min))
    pid->dt_min = dt_min;
  else
    return -1;

  LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);

  pid->mode = mode;
  PID_Reset(pid);
  return 0;
}

/**
 * @brief PID计算
 *
 * @param pid PID结构体
 * @param sp 设定值
 * @param fb 反馈值
 * @param fb_dot 反馈值微分
 * @param dt 间隔时间
 * @return float 计算的输出
 */
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
  if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
    return pid->last.out;
  }

  /* 计算误差值 */
  const float err = CircleError(sp, fb, pid->param->range);

  /* 计算P项 */
  const float k_err = err * pid->param->k;

  /* 计算D项 */
  const float k_fb = pid->param->k * fb;
  const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);

  float d;
  switch (pid->mode) {
    case KPID_MODE_CALC_D:
      /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
      /* 当sp不变时，err的微分等于负的fb的微分 */
      d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
      break;

    case KPID_MODE_SET_D:
      d = fb_dot;
      break;

    case KPID_MODE_NO_D:
      d = 0.0f;
      break;
  }
  pid->last.err = err;
  pid->last.k_fb = filtered_k_fb;

  if (!isfinite(d)) d = 0.0f;

  /* 计算PD输出 */
  float output = (k_err * pid->param->p) - (d * pid->param->d);

  /* 计算I项 */
  const float i = pid->i + (k_err * dt);
  const float i_out = i * pid->param->i;
  
  if (pid->param->i > SIGMA) {
    /* 检查是否饱和 */
    if (isfinite(i)) {
      if ((fabsf(output + i_out) <= pid->param->out_limit) &&
          (fabsf(i) <= pid->param->i_limit)) {
        /* 未饱和，使用新积分 */
        pid->i = i;
      }
    }
  }
  
  /* 计算PID输出 */
  output += i_out;
 
  /* 限制输出 */
  if (isfinite(output)) {
    if (pid->param->out_limit > SIGMA) {
      output = AbsClip(output, pid->param->out_limit);
    }
    pid->last.out = output;
  }
  return pid->last.out;
}

/**
 * @brief 重置微分项
 *
 * @param pid PID结构体
 * @return int8_t 0对应没有错误
 */
int8_t PID_ResetIntegral(KPID_t *pid) {
  if (pid == NULL) return -1;

  pid->i = 0.0f;

  return 0;
}

/**
 * @brief 重置PID
 *
 * @param pid PID结构体
 * @return int8_t 0对应没有错误
 */
int8_t PID_Reset(KPID_t *pid) {
  if (pid == NULL) return -1;

  pid->i = 0.0f;
  pid->last.err = 0.0f;
  pid->last.k_fb = 0.0f;
  pid->last.out = 0.0f;
  LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);

  return 0;
}