#include "ozone_control_service.h"

#include "frequency_sweep_service.h"
#if 0

float get_ozone_power() {
  float powersum = 0;
  for (size_t i = 0; i < 20; i++) {
    powersum += port_adc_get_ozone_generator_power();
  }
  return powersum / 20;
}
static float mf_set_pwm_and_get_power(uint32_t freq, uint32_t dutyns) {
  port_ozone_pwm_set_duty(freq, dutyns);
  systicket_delay_ms(2);
  float power1 = get_ozone_power();
  return power1;
}



void frequency_sweep(uint32_t startfreq, uint32_t step, uint32_t endfreq, uint16_t dutyns) {
  /**
   * @brief 扫频
   */
  s_power_table_startfreq = startfreq;
  s_frequency_sweep_step = step;
  s_power_table_endfreq = endfreq;
  uint16_t nowfreq = startfreq;

  bool firstloop = true;
  KFP KFPConfig = {0.02, 0, 0, 0, 0.03, 0.543};

  port_ozone_pwm_set_duty(startfreq, dutyns);
  port_ozone_pwm_start();
  systicket_delay_ms(3);
  printf("frequency_sweep:startfreq:%d, step:%d, endfreq:%d, dutyns:%d\n", startfreq, step, endfreq, dutyns);
  for (size_t i = 0; i < ARRAY_SIZE(s_power_table); i++) {
    nowfreq = startfreq + i * s_frequency_sweep_step;
    float power = mf_set_pwm_and_get_power(nowfreq, dutyns);
    if (firstloop) {
      firstloop = false;
      KFPConfig.LastP = power;
    }
    float afterfileter = kalmanFilter(&KFPConfig, power);
    setpower(nowfreq, power);
    /**
     * @brief dumptable
     */
    printf("%d,%f,%f\n", nowfreq, s_power_table[i], afterfileter);
    if (nowfreq >= s_power_table_endfreq) {
      break;
    }
  }
  port_ozone_pwm_stop();
}

float find_resonant_frequency(uint32_t startfreq, uint32_t endfreq) {
  /**
   * @brief
   */
  float minpower = 0;
    

}
#endif
static struct { bool working; } this;

void ozone_control_init(ozone_control_config_t* config) {
  /**
   * @brief 扫描频率
   */

  while (!frequency_sweep_is_finished()) {
  }
}

void ozone_control_set_level(level_t level);
void ozone_control_start() {
  frequency_sweep_start(
      /*startfreq:*/ 20000,  //
      /*step     :*/ 100,    //
      /*endfreq  :*/ 40000,  //
      /*dutyns   :*/ 5000    //
  );
  this.working = true;
}
void ozone_control_stop() {}

void ozone_control_schedule() {
  //
  frequency_sweep_schedule();
  //

  

}