#include "ozone_control_service.h"

#include "../zes8p5066lib/basic.h"
#include "../zes8p5066lib/systicket.h"
#include "frequency_sweep_service.h"
#include "kalmanFilter.h"
#include "state_machine.h"
//
#include "least_square_method.h"

/**
 * @brief
 *
 *  状态机:
 *            不工作 ------> 扫描频率 ----> 工作
 *
 **/

#define PRV_START_EVENT (STATE_MACHINE_PUBLIC_EVENT + 1)
#define PRV_STOP_EVENT (STATE_MACHINE_PUBLIC_EVENT + 2)

state_machine_state_t m_states[] = {
    [kIdleState] =
        {
            .name = "kIdleState",
            .stateId = kIdleState,
        },
    [kBeforeWorkingStateSweepFrequency] =
        {
            .name = "kBeforeWorkingStateSweepFrequency",
            .stateId = kBeforeWorkingStateSweepFrequency,
        },
    [kWorkingState] =  //
    {
        .name = "kWorkingState",
        .stateId = kWorkingState,
    }
    //
};
state_machine_t m_statemachine;

static ozone_control_working_state_t s_workingstate;

static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
  /**
   * @brief
   *    计算谐振频率，频率和功率的图像接近下图，由此可知，当设备工作正常时，在一定频率范围内,查找功率最小的点即可。
   *
   * http://192.168.1.3:3000/project_ozone_generator/doc/src/branch/master/ref/20220815频率-功率关系图.png
   */
  float minpower = 0;
  uint16_t retfreq = 0;
  bool inited = false;
  for (uint16_t freq = startfreq; freq <= endfreq; freq += step) {
    float power = frequency_sweep_get_power(freq);
    if (!inited) {
      minpower = power;
      retfreq = freq;
      inited = true;
      continue;
    }

    if (power < minpower) {
      minpower = power;
      retfreq = freq;
    }
  }
  return retfreq;
}

static KFP KFPConfig = KALMAN_FILTER_PARA;
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;
}

static float mf_fan_get_power() {
  float fanpower = 0;
  for (size_t i = 0; i < 20; i++) {
    fanpower += port_adc_get_fan_power();
  }
  return fanpower / 20;
}

static void mf_get_ozone_power_reset_filter() { KFPConfig.LastP = __mf_get_ozone_power(); }
static float mf_get_ozone_power() { return kalmanFilter(&KFPConfig, __mf_get_ozone_power()); }

float get_expect_power() {
  float expectpower = 0;
  if (thisDevice.level == klevel1) {
    expectpower = LEVEL1_EXPECT_POWER;
  } else if (thisDevice.level == klevel2) {
    expectpower = LEVEL2_EXPECT_POWER;
  }
  return expectpower;
}

static float gety(int x) { return frequency_sweep_get_power((uint16_t)x); }
static float compute_slope(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
  float k = 0;
  float c = 0;
  least_square_method(startfreq, step, endfreq, gety, &c, &k);
  return k;
}
static float compute_avarage_power(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
  float sumpower = 0;
  int count = 0;
  for (uint16_t freq = startfreq; freq <= endfreq; freq += step) {
    count++;
    float power = frequency_sweep_get_power(freq);
    sumpower += power;
  }
  return sumpower / count;
}

static state_machine_state_t* processBeforeWorkingStateSweepFrequency(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
  /**
   * @brief 扫频
   */
  if (event == ENTER_STATE) {
    frequency_sweep_start(20000, 100, 41000, 3000);
  } else if (event == TIME_EVENT) {
    if (frequency_sweep_is_finished()) {
      s_workingstate.resonant_frequency = get_resonant_frequency(25000, 100, 35000);  //谐振频率点
      s_workingstate.slope_when_freq40k = compute_slope(39000, 100, 41000);           //利用最小二乘法计算出45K频率的斜率
      s_workingstate.avarage_power = compute_avarage_power(20000, 100, 41000);        //平均功率
      printf("----------Summarize--------\n");
      printf("-resonant_frequency: %d\n", s_workingstate.resonant_frequency);
      printf("-slope_when_freq40k: %f\n", s_workingstate.slope_when_freq40k);
      printf("-avarage_power     : %f\n", s_workingstate.avarage_power);
      s_workingstate.nowfreq = s_workingstate.resonant_frequency;
      return &m_states[kWorkingState];
    }
  } else if (event == EXIT_STATE) {
    frequency_sweep_stop();
  }
  return NULL;
}

static state_machine_state_t* processWorkingState(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
  /**
   * @brief 检查当前功率是否正常
   *
   *    当功率在期望功率窗口之外时候([expertpower+window,expertpower-window])，
   * 开始调整当前频率，直到功率接近期望功率。
   */

  if (event == ENTER_STATE) {
    /**
     * @brief 在谐振点启动，此时臭氧功率最小
     */
    if (s_workingstate.nowfreq < s_workingstate.resonant_frequency) {
      s_workingstate.nowfreq = s_workingstate.resonant_frequency;
    }
    port_ozone_pwm_set_duty(s_workingstate.nowfreq, 5000);
    port_ozone_pwm_start();
    s_workingstate.adjustedToTheProperPower = false;
    mf_get_ozone_power_reset_filter();
    printf("----------start working--------\n");
  } else if (event == TIME_EVENT) {
    float nowpower = mf_get_ozone_power();
    float fanpower = mf_fan_get_power();
    if (s_workingstate.adjustedToTheProperPower) {
      /**
       *  检查当前功率是否在期望功率窗口之外时候([expertpower+window,expertpower-window])，
       */
      if (nowpower < (get_expect_power() - EXPECT_POWER_WINDONWS) ||  //
          nowpower > (get_expect_power() + EXPECT_POWER_WINDONWS)) {
        s_workingstate.adjustedToTheProperPower = false;
        printf("change freq [ no ],freq %d, ozonePower %f-->%f  fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
        if (nowpower < get_expect_power()) {
          s_workingstate.changefreqdirection = true;
        } else {
          s_workingstate.changefreqdirection = false;
        }
      }
    } else {
      /**
       * 调整功率到期望功率
       */
      if (s_workingstate.changefreqdirection) {
        if (nowpower < get_expect_power()) {
          s_workingstate.nowfreq += 25;
          if (s_workingstate.nowfreq > MAX_FREQ) s_workingstate.nowfreq = MAX_FREQ;
          printf("change freq [ up ],freq %d, ozonePower %f-->%f  fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
          port_ozone_pwm_set_duty(s_workingstate.nowfreq, kconst_pwm_work_dutyns);
        } else {
          // printf("reach %f->%f\n", nowpower, get_expect_power());
          s_workingstate.adjustedToTheProperPower = true;
        }
      } else {
        if (nowpower > get_expect_power()) {
          s_workingstate.nowfreq -= 25;
          if (s_workingstate.nowfreq < s_workingstate.resonant_frequency) s_workingstate.nowfreq = s_workingstate.resonant_frequency;
          printf("change freq [down],freq %d, ozonePower %f-->%f  fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
          port_ozone_pwm_set_duty(s_workingstate.nowfreq, kconst_pwm_work_dutyns);
        } else {
          // printf("reach %f->%f\n", nowpower, get_expect_power());
          s_workingstate.adjustedToTheProperPower = true;
        }
      }
    }
  } else if (event == EXIT_STATE) {
    port_ozone_pwm_stop();
  }
  return NULL;
}

static state_machine_state_t* state_machine_process_event(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
  /**
   * @brief 处理Start和Stop事件
   */
  if (event == PRV_START_EVENT || event == PRV_STOP_EVENT) {
    if (event == PRV_START_EVENT) {
      printf("start\n");
      return &m_states[kBeforeWorkingStateSweepFrequency];
    } else {
      printf("stop\n");
      return &m_states[kIdleState];
    }
  }

  /**
   * @brief 处理其他事件
   */
  if /*  */ (nowstate == &m_states[kIdleState]) {
  } else if (nowstate == &m_states[kBeforeWorkingStateSweepFrequency]) {
    return processBeforeWorkingStateSweepFrequency(machine, nowstate, event);
  } else if (nowstate == &m_states[kWorkingState]) {
    return processWorkingState(machine, nowstate, event);
  }
  return NULL;
}
/***********************************************************************************************************************
 * ======================================================Export======================================================= *
 ***********************************************************************************************************************/

void ozone_control_init() { state_machine_init(&m_statemachine, m_states, ZARR_SIZE(m_states), state_machine_process_event); }
void ozone_control_start() { state_machine_trigger_event(&m_statemachine, PRV_START_EVENT); }
void ozone_control_stop() { state_machine_trigger_event(&m_statemachine, PRV_STOP_EVENT); }
void ozone_control_schedule() {
  static uint32_t ticket = 0;
  if (systicket_haspassedms(ticket) > 10) {
    ticket = systicket_get_now_ms();
    state_machine_schedule_each10ms(&m_statemachine);
  }
  frequency_sweep_schedule();
}
ozone_control_working_state_id_t ozone_control_get_working_state_id() {
  if (state_machine_get_now_state(&m_statemachine) == &m_states[kIdleState]) {
    return kIdleState;
  }
  if (state_machine_get_now_state(&m_statemachine) == &m_states[kBeforeWorkingStateSweepFrequency]) {
    return kBeforeWorkingStateSweepFrequency;
  }
  if (state_machine_get_now_state(&m_statemachine) == &m_states[kWorkingState]) {
    return kWorkingState;
  }
  return kIdleState;
}
ozone_control_working_state_t* ozone_control_get_working_state() { return &s_workingstate; }

float ozone_control_get_ozone_power() { return mf_get_ozone_power(); }

float ozone_control_get_expect_power() { return get_expect_power(); }

float ozone_control_get_fan_power() { return mf_fan_get_power(); }