#include "frequency_sweep_service.h"

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

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

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

static KFP KFPConfig = KALMAN_FILTER_PARA;

/***********************************************************************************************************************
 * ====================================================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");
}

void frequency_sweep_stop() { this.is_schedule = false; }

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();
  }
}