#include "frequency_sweep_service.h"

#include "zes8p5066lib/basic.h"
#include "zes8p5066lib/systicket.h"

/***********************************************************************************************************************
 * ====================================================卡尔曼滤波===================================================== *
 ***********************************************************************************************************************/
typedef struct {
  float LastP;  //上次估算协方差 初始化值为0.02
  float Now_P;  //当前估算协方差 初始化值为0
  float out;    //卡尔曼滤波器输出 初始化值为0
  float Kg;     //卡尔曼增益 初始化值为0
  float Q;      //过程噪声协方差 初始化值为0.001
  float R;      //观测噪声协方差 初始化值为0.543
} KFP;          // Kalman Filter parameter

// 2. 以高度为例 定义卡尔曼结构体并初始化参数
/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float kalmanFilter(KFP* kfp, float input) {
  //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
  kfp->Now_P = kfp->LastP + kfp->Q;
  //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
  //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
  kfp->out = kfp->out + kfp->Kg * (input - kfp->out);  //因为这一次的预测值就是上一次的输出值
  //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
  kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
  return kfp->out;
}

/***********************************************************************************************************************
 * ====================================================THIS_MODULE==================================================== *
 ***********************************************************************************************************************/

struct {
  float power_table[350];
  uint16_t startfreq;
  uint16_t endfreq;
  uint16_t step;
  bool is_schedule;
  bool firstloop;
  uint16_t nowfreq;
  uint32_t startticket;
  uint32_t dutyns;
} this;

KFP KFPConfig = {0.02, 0, 0, 0, 0.05, 0.543};

/***********************************************************************************************************************
 * ====================================================PowerTable===================================================== *
 ***********************************************************************************************************************/
static void mf_setpower(uint16_t freq, float power) {
  uint16_t index = (freq - this.startfreq) / this.step;

  if (index >= ZARR_SIZE(this.power_table)) return;
  this.power_table[index] = power;
}
static float mf_getpower(uint16_t freq) {
  uint16_t index = (freq - this.startfreq) / this.step;

  if (index >= ZARR_SIZE(this.power_table)) return 0;
  if (index > (this.endfreq - this.startfreq) / this.step) {
    return 0;
  }
  return this.power_table[index];
}

static float mf_get_ozone_power() {
  float powersum = 0;
  for (size_t i = 0; i < 10; i++) {
    powersum += port_adc_get_ozone_generator_power();
  }
  return powersum / 10;
}
/***********************************************************************************************************************
 * ======================================================Extern======================================================= *
 ***********************************************************************************************************************/
void frequency_sweep_start(uint32_t startfreq, uint32_t step, uint32_t endfreq, uint16_t dutyns) {
  this.startfreq = startfreq;
  this.endfreq = endfreq;
  this.step = step;
  this.dutyns = dutyns;

  this.firstloop = true;
  this.is_schedule = true;
  this.startticket = systicket_get_now_ms();
  this.nowfreq = this.startfreq;
  printf("cls\n");
}

bool frequency_sweep_is_finished() { return !this.is_schedule; }
float frequency_sweep_get_power(uint16_t freq) { return mf_getpower(freq); }
void frequency_sweep_schedule() {
  if (!this.is_schedule) {
    return;
  }

  /// loop
  if (systicket_haspassedms(this.startticket) > 1) {
    /**
     * @brief 设置频率和占空比
     */
    port_ozone_pwm_set_duty(this.nowfreq, this.dutyns);
    port_ozone_pwm_start();
    systicket_delay_ms(3);
    /**
     * @brief 读取当前功率
     */
    float power = mf_get_ozone_power();
    if (this.firstloop) {
      KFPConfig.LastP = power;
    }

    float afterfileter = kalmanFilter(&KFPConfig, power);
    mf_setpower(this.nowfreq, afterfileter);
    printf("%d,%f,%f\n", this.nowfreq, afterfileter, power);
    /**
     * @brief 更新频率
     */
    this.nowfreq += this.step;
    if (this.nowfreq > this.endfreq) {
      this.is_schedule = false;
    }
    port_ozone_pwm_stop();
    //
    this.firstloop = false;
    this.startticket = systicket_get_now_ms();
  }
}