#include <stdbool.h>  //定义布尔
#include <string.h>

#include "board.h"
//
#include "zes8p5066lib/basic.h"
#include "zes8p5066lib/gpio.h"
#include "zes8p5066lib/key.h"
#include "zes8p5066lib/systicket.h"
#include "zes8p5066lib/uart0.h"
//
#include "service/light_control_service.h"
#include "service/thisdevice.h"
#include "test.h"
#include "zsimple_timer/zsimple_timer.h"

/***********************************************************************************************************************
 * =====================================================函数声明====================================================== *
 ***********************************************************************************************************************/
void onkey(zkey_t* key, zkey_state_t key_state);
/***********************************************************************************************************************
 * =====================================================全局变量====================================================== *
 ***********************************************************************************************************************/
static zkey_t s_keys[] = {
    ZKEY_INIT("powerkey", port_gpio_get_power_key_state),        //电源按键
    ZKEY_INIT("levelkey", port_gpio_get_level_key_state),        //左1
    ZKEY_INIT("timerkey", port_gpio_get_timer_key_state),        //左2
    ZKEY_INIT("intervalkey", port_gpio_get_interval_key_state),  //左3
};
zkey_module_t key_module = ZMODULE_INIT(s_keys, onkey);
static zsimple_timer_t zsimple_timer_mem[10];  //最多同时存在10个定时器
zsimple_timer_t* debuglighttimer;

/***********************************************************************************************************************
 * =======================================================方法======================================================== *
 ***********************************************************************************************************************/

static uint32_t compute_countdown_num(int countdowns) {
  if (countdowns) return countdowns / kconst_countdown_step_s + !!(countdowns % kconst_countdown_step_s);
}

static void shutdwon() {
  printf("power off\n");
  thisDevice.poweron = false;
  port_fan_set(false);
  //@TODO:关闭臭氧
}

static void increase_and_assign_countdonwnum() {
  if (thisDevice.countdonwnum == 4) {
    thisDevice.countdonwnum = 0;
    thisDevice.countdonwnum_s = 0;
    thisDevice.countdonw_setting_num = 0;
    lcs_active_input(knone_active);
  } else {
    thisDevice.countdonwnum_s = (thisDevice.countdonwnum + 1) * kconst_countdown_step_s;
    thisDevice.countdonwnum = thisDevice.countdonwnum + 1;
    thisDevice.countdonw_setting_num = thisDevice.countdonwnum;
    thisDevice.countdonw_start_ticket = systicket_get_now_ms();
  }
}

static void set_countdown(int countdownnum) {
  thisDevice.countdonwnum = countdownnum;
  thisDevice.countdonwnum_s = thisDevice.countdonwnum * kconst_countdown_step_s;
  thisDevice.countdonw_setting_num = thisDevice.countdonwnum;
  thisDevice.countdonw_start_ticket = systicket_get_now_ms();
}

static void onkey(zkey_t* key, zkey_state_t key_state) {
  if /*  */ (strcmp(key->name, "powerkey") == 0 && zks_rising_edge == key_state) {
    printf("on %s \n", key->name);
    if (!thisDevice.poweron) {
      printf("power on\n");
      thisDevice.poweron = true;
      thisDevice.level = klevel1;
      thisDevice.mode = knormal;

      //设置灯光效果
      port_fan_set(true);
      //@TODO:启动臭氧
    } else {
      shutdwon();
    }
    return;
  }
  // levelkey
  if (strcmp(key->name, "levelkey") == 0 && zks_rising_edge == key_state) {
    if (!thisDevice.poweron) return;
    printf("on %s \n", key->name);

    /**
     * @brief 当前在不在修改状态，不在，则进入修改状态
     */
    if (!lcs_input_is_active(kchange_level_input)) {
      lcs_active_input(kchange_level_input);
      return;
    }

    if (thisDevice.level == klevel1) {
      printf("changet level to level2\n");
      thisDevice.level = klevel2;
      //更改臭氧状态
    } else if (thisDevice.level == klevel2) {
      printf("changet level to level1\n");
      thisDevice.level = klevel1;
      //更改臭氧状态
    }
    lcs_active_input(kchange_level_input);
    return;
  }
  // timerkey
  if (strcmp(key->name, "timerkey") == 0 && zks_rising_edge == key_state) {
    if (!thisDevice.poweron) return;
    printf("on %s \n", key->name);
    if (!lcs_input_is_active(kchange_countdonw_time_input)) {
      lcs_active_input(kchange_countdonw_time_input);
      if (thisDevice.mode != ktimingMode || thisDevice.countdonwnum_s == 0) {
        set_countdown(1);
        thisDevice.mode = ktimingMode;
      }
      return;
    }

    lcs_active_input(kchange_countdonw_time_input);
    increase_and_assign_countdonwnum();
  }
  if (strcmp(key->name, "intervalkey") == 0 && zks_rising_edge == key_state) {
    if (!thisDevice.poweron) return;
    printf("on %s \n", key->name);

    if (!lcs_input_is_active(kchange_intermittentmode_time_input)) {
      lcs_active_input(kchange_intermittentmode_time_input);
      if (thisDevice.mode != kintermittentMode || thisDevice.countdonwnum_s == 0) {
        set_countdown(1);
        thisDevice.mode = kintermittentMode;
      }
      return;
    }

    lcs_active_input(kchange_intermittentmode_time_input);
    increase_and_assign_countdonwnum();
  }
}

void compute_countdown() {
  if /*  */ (thisDevice.mode == knormal) {
    thisDevice.countdonwnum = 0;
    thisDevice.countdonwnum_s = 0;
  } else if (thisDevice.mode == ktimingMode || thisDevice.mode == kintermittentMode) {
    if (thisDevice.countdonwnum_s != 0) {
      thisDevice.countdonwnum_s =  //
          thisDevice.countdonw_setting_num * kconst_countdown_step_s - systicket_haspassedms(thisDevice.countdonw_start_ticket) / 1000;
      thisDevice.countdonwnum = compute_countdown_num(thisDevice.countdonwnum_s);
    }
  }
}

void process_countdwonevent() {
  //计算countdown
  compute_countdown();
  /**
   * @brief 处理倒计时事件发生时的事件
   */
  if (thisDevice.mode == ktimingMode) {
    //定时自动关机
    if (thisDevice.poweron) {
      if (thisDevice.countdonwnum_s == 0) {
        shutdwon();
      }
    }
  } else if (thisDevice.mode == kintermittentMode) {
    /**
     * @brief 如果当前处于间歇模式下
     * 定时时间到,关闭风扇,关闭臭氧,等待大周期过去后,开启风扇,开启臭氧，并重新计时
     */

    if (!thisDevice.intermittentMode_idle) {
      if (thisDevice.countdonwnum_s == 0) {
        //切换状态到待机状态
        // TODO:关闭臭氧
        thisDevice.intermittentMode_idle = true;
        port_fan_set(false);
      }
    } else {
      if (systicket_haspassedms(thisDevice.countdonw_start_ticket) / 1000 > INTERMITTENTMODE_PERIOD_S) {
        thisDevice.intermittentMode_idle = false;
        set_countdown(thisDevice.countdonw_setting_num);
        //启动设备
        port_fan_set(true);
      }
    }
  }
};

void do_debug_light_state(zsimple_timer_t* handler) {
  static uint8_t debug_led_state = 1;
  debug_led_state = !debug_led_state;
  port_debug_set(debug_led_state);
}

float get_ozone_power() {
  float powersum = 0;
  for (size_t i = 0; i < 5; i++) {
    powersum += port_adc_get_ozone_generator_power();
  }
  return powersum / 5;
}

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. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height = {0.02, 0, 0, 0, 0.03, 0.543};

/**
 *卡尔曼滤波器
 *@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;
}

float derivative(float now) {
  static float last = 0;
  float diff = now - last;
  last = now;
  return diff;
}

#if 0
void printf_ozone_freq_and_power_table() {
  /**
   * @brief 打印臭氧发生器和频率之间的关系
   */

  float startfreq = 10 * 1000;
  float stepfreq = 100;
  float nowfreq = startfreq;

  port_ozone_pwm_set_duty(nowfreq, 1000);
  port_ozone_pwm_start();
  systicket_delay_ms(10);

  bool first = true;

  while (true) {
    
    port_ozone_pwm_set_duty(nowfreq, 3000);
    systicket_delay_ms(2);
    float power = get_ozone_power();



    // printf("%f %f\n", nowfreq, freq);
    // printf("{freq power:%f,%f}\r\n", nowfreq, power);
    // printf("{freq power:%f,%f}\r\n", nowfreq, power);
    if (first) {
      // kalmanFilter(&KFP_height, power);
      KFP_height.LastP = power;
      first = false;
    }
    float afterfilter = kalmanFilter(&KFP_height, power);
    printf("%f,%f,%f,%f\r\n", nowfreq, power, afterfilter,derivative(afterfilter)*10);

    // printf("{power:%f}\r\n", power);
    nowfreq += stepfreq;
    if (nowfreq >= 40 * 1000) break;
  }
  port_ozone_pwm_set_duty(33000, 3000);
  port_fan_set(true);
  while (true) {
  }
  port_ozone_pwm_stop();

  while (true) {
    systicket_delay_ms(2);
    float power = get_ozone_power();
    float afterfilter = kalmanFilter(&KFP_height, power);
    nowfreq+=50;
    printf("%f,%f,%f\r\n", nowfreq, power, afterfilter);
  }
}
#endif

#if 0
float kalmanFilter1(float input) {
  static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543};
  return kalmanFilter(&kfp, input);
}
float kalmanFilter2(float input) {
  static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543};
  return kalmanFilter(&kfp, input);
}
float kalmanFilter3(float input) {
  static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543};
  return kalmanFilter(&kfp, input);
}
float kalmanFilter4(float input) {
  static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543};
  return kalmanFilter(&kfp, input);
}

float 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 printf_ozone_freq_and_power_table() {
  /**
   * @brief 打印臭氧发生器和频率之间的关系
   */

  uint32_t startfreq = 10 * 1000;
  uint32_t stepfreq = 100;
  uint32_t nowfreq = startfreq;

  port_ozone_pwm_set_duty(nowfreq, 1000);
  // port_ozone_pwm_stop();
  port_ozone_pwm_start(); 
  systicket_delay_ms(10);

  bool first = true;

  while (true) {
    // printf("%d,%f,%f,%f,%f\r\n", nowfreq,                        //
    //        /*kalmanFilter1*/(set_pwm_and_get_power(nowfreq, 2000)),  //
    //        /*kalmanFilter2*/(set_pwm_and_get_power(nowfreq, 4000)),  //
    //        /*kalmanFilter3*/(set_pwm_and_get_power(nowfreq, 6000)),  //
    //        /*kalmanFilter4*/(set_pwm_and_get_power(nowfreq, 8000))   //
    // );

    // printf("{power:%f}\r\n", power);
    nowfreq += stepfreq;
    if (nowfreq >= 40 * 1000) break;
  }
  port_ozone_pwm_set_duty(33000, 3500);
  port_fan_set(true);
  while (true) {
  }
}
#endif

// void printf_ozone_freq_and_power_table() {
//   /**
//    * @brief 打印臭氧发生器和频率之间的关系
//    */

//   float startfreq = 20 * 1000;
//   float stepfreq = 50;
//   float nowfreq = startfreq;

//   // port_ozone_pwm_set_duty(nowfreq, 1000);
//   // port_ozone_pwm_start();
//   // systicket_delay_ms(10);

//   while (true) {
//     // printf("{power:%f}\r\n", nowfreq);
//     printf("%d,%d\r\n", (int)nowfreq,(int)nowfreq);
//     systicket_delay_ms(100);
//     nowfreq += stepfreq;
//     // if (nowfreq >= 40 * 1000) break;
//   }
//   port_ozone_pwm_stop();
// }

void init_all_subdevice_state() {
  port_debug_set(false);
  port_fan_set(false);
  port_led0_set(false);
  port_led1_set(false);
  port_led2_set(false);
  port_led3_set(false);
  port_led_r_set(false);
  port_led_g_set(false);
  port_led_b_set(false);
}

int main(void) {
  SystemInit();            //配置系统时钟
  DeviceClockAllEnable();  //打开所有外设时钟
  systicket_init();

  /*系统初始化*/
  zgpio_init_all_gpio();                            //
  port_init();                                      //
  init_all_subdevice_state();                       //
  printf("==========OZONE_GENERATOR==========\n");  //
  printf("= manufactor: iflytop\n");                //
  printf("= version   : %s\n", VERSION);            //
  printf("=\n");                                    //

  /*组件初始化*/
  zsimple_timer_module_init(zsimple_timer_mem, ZARR_SIZE(zsimple_timer_mem), systicket_get_now_ms);  //定时器模块初始化
  zkey_init(&key_module);                                                                            //按键初始化

  debuglighttimer = zsimple_timer_alloc();
  zsimple_timer_trigger_static(debuglighttimer, 300, INFINITE_TIMES /*触发次数:*/, true, do_debug_light_state);

  /**
   * @brief
   *    频率从20k起步，递增50hz，每次等待100ms计算功率
   */

  printf_ozone_freq_and_power_table();
  
  while (true) {
    //按键扫描逻辑
    DO_IT_EACH_MS(KEY_PERIOD) { zkey_do_loop_in_each_period(NULL); }
    END();

    zsimple_timer_schedule();
    lcs_shcedule();
    process_countdwonevent();
    // printf("countddonw %d\n", thisDevice.countdonwnum_s);
  }
}