#include "zble_module.h"
#include "zdatachannel_service.h"
#include "znordic.h"
//

#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//
#include <math.h>

#include "app_scheduler.h"
#include "znordic.h"

#if defined(UART_PRESENT)
#include "nrf_uart.h"
#endif
#if defined(UARTE_PRESENT)
#include "nrf_uarte.h"
#endif

#if 0
int main() {
  one_conduction_main();
  return 0;
}
#else
#include "nrfx_timer.h"

ZDATACHANNEL_DEF(m_zhrs, 2 /*优先级*/, 1 /*client num*/);   // 蓝牙服务
static const nrfx_timer_t m_timer = NRFX_TIMER_INSTANCE(1); /**< Timer used for channel sweeps and tx with duty cycle. */
#define PI 3.14159265358979323846
/********************************************************************************************************
 *                              LOW PASS FILTER
 ********************************************************************************************************/
typedef struct {
  float coef[2];
  float v_out[2];
} LPFilter;

void  LPFilter_Init(LPFilter *filter, float cutoffFreqHz, float sampleTimeS);
float LPFilter_Update(LPFilter *filter, float v_in);

/********************************************************************************************************
 *                              HIGH PASS FILTER
 ********************************************************************************************************/
typedef struct {
  float coef;
  float v_out[2];
  float v_in[2];

} HPFilter;

void  HPFilter_Init(HPFilter *filter, float cutoffFreqHz, float sampleTimeS);
float HPFilter_Update(HPFilter *filter, float v_in);

/********************************************************************************************************
 *                              BAND PASS FILTER
 ********************************************************************************************************/

typedef struct {
  LPFilter lpf;
  HPFilter hpf;
  float    out_in;
} PBFilter;

void  PBFilter_Init(PBFilter *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS);
float PBFilter_Update(PBFilter *filter, float v_in);

/********************************************************************************************************
 *                              NOTCH FILTER
 ********************************************************************************************************/

typedef struct {
  float alpha;
  float beta;

  float vin[3];
  float vout[3];

} NOTCHFilter;

void  NOTCHFilter_Init(NOTCHFilter *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS);
float NOTCHFilter_Update(NOTCHFilter *filter, float vin);

#define PI 3.141592653

/********************************************************************************************************
 *                              LOW PASS FILTER
 ********************************************************************************************************/

void LPFilter_Init(LPFilter *filter, float cutoffFreqHz, float sampleTimeS) {
  float RC        = 0.0;
  RC              = 1.0 / (2 * PI * cutoffFreqHz);
  filter->coef[0] = sampleTimeS / (sampleTimeS + RC);
  filter->coef[1] = RC / (sampleTimeS + RC);

  filter->v_out[0] = 0.0;
  filter->v_out[1] = 0.0;
}

float LPFilter_Update(LPFilter *filter, float v_in) {
  filter->v_out[1] = filter->v_out[0];
  filter->v_out[0] = (filter->coef[0] * v_in) + (filter->coef[1] * filter->v_out[1]);

  return (filter->v_out[0]);
}

/********************************************************************************************************
 *                              HIGH PASS FILTER
 ********************************************************************************************************/
void HPFilter_Init(HPFilter *filter, float cutoffFreqHz, float sampleTimeS) {
  float RC = 0.0;
  RC       = 1.0 / (2 * PI * cutoffFreqHz);

  filter->coef = RC / (sampleTimeS + RC);

  filter->v_in[0] = 0.0;
  filter->v_in[1] = 0.0;

  filter->v_out[0] = 0.0;
  filter->v_out[1] = 0.0;
}

float HPFilter_Update(HPFilter *filter, float v_in) {
  filter->v_in[1] = filter->v_in[0];
  filter->v_in[0] = v_in;

  filter->v_out[1] = filter->v_out[0];

  filter->v_out[0] = filter->coef * (filter->v_in[0] - filter->v_in[1] + filter->v_out[1]);

  return (filter->v_out[0]);
}

/********************************************************************************************************
 *                              BAND PASS FILTER
 ********************************************************************************************************/

void PBFilter_Init(PBFilter *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS) {
  LPFilter_Init(&filter->lpf, LPF_cutoffFreqHz, sampleTimeS);
  HPFilter_Init(&filter->hpf, HPF_cutoffFreqHz, sampleTimeS);
  filter->out_in = 0.0;
}

float PBFilter_Update(PBFilter *filter, float v_in) {
  filter->out_in = HPFilter_Update(&filter->hpf, v_in);

  filter->out_in = LPFilter_Update(&filter->lpf, filter->out_in);

  return (filter->out_in);
}

/********************************************************************************************************
 *                              NOTCH FILTER
 ********************************************************************************************************/

void NOTCHFilter_Init(NOTCHFilter *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS) {
  // filter frequency to angular (rad/s)
  float w0_rps = 2.0 * PI * centerFreqHz;
  float ww_rps = 2.0 * PI * notchWidthHz;

  // pre warp center frequency
  float w0_pw_rps = (2.0 / sampleTimeS) * tanf(0.5 * w0_rps * sampleTimeS);

  // computing filter coefficients

  filter->alpha = 4.0 + w0_rps * w0_pw_rps * sampleTimeS * sampleTimeS;
  filter->beta  = 2.0 * ww_rps * sampleTimeS;

  // clearing input and output  buffers

  for (uint8_t n = 0; n < 3; n++) {
    filter->vin[n]  = 0;
    filter->vout[n] = 0;
  }
}

float NOTCHFilter_Update(NOTCHFilter *filter, float vin) {
  // shifting samples
  filter->vin[2] = filter->vin[1];
  filter->vin[1] = filter->vin[0];

  filter->vout[2] = filter->vout[1];
  filter->vout[1] = filter->vout[0];

  filter->vin[0] = vin;

  // compute new output
  filter->vout[0] = (filter->alpha * filter->vin[0] + 2.0 * (filter->alpha - 8.0) * filter->vin[1] + filter->alpha * filter->vin[2] - (2.0f * (filter->alpha - 8.0) * filter->vout[1] + (filter->alpha - filter->beta) * filter->vout[2])) / (filter->alpha + filter->beta);

  return (filter->vout[0]);
}

/*******************************************************************************
 *                                    MAIN                                     *
 *******************************************************************************/

static void zdatachannel_data_handler(zdatachannel_evt_t *p_evt) {
  /**
   * @brief
   */
  if (p_evt->type == ZDATACHANNEL_EVT_RX_DATA) {
  }
}

static void on_service_init(void) {
  ZLOGI("init zdatachannel service");
  zdatachannel_init_t zdatachannle_init;
  memset(&zdatachannle_init, 0, sizeof(zdatachannle_init));
  zdatachannle_init.data_handler = zdatachannel_data_handler;
  ZERROR_CHECK(zdatachannel_init(&m_zhrs, &zdatachannle_init));
}

int16_t adc_val_cache0[5] = {0};
int16_t adc_val_cache1[5] = {0};
int16_t adc_val_cache2[5] = {0};
int16_t adc_val_cache3[5] = {0};
int     adc_val_index     = 0;

void sendpacket_to_pc() {
  uint8_t data[255];
  // adc_val_cache[0] = 1;
  // adc_val_cache[1] = 2;
  // adc_val_cache[2] = 3;
  // adc_val_cache[3] = 4;
  // adc_val_cache[4] = 5;
  for (int i = 0; i < adc_val_index; i++) {
    if (adc_val_cache0[i] > 4096) {
      adc_val_cache0[i] = 4096;
    }
    if (adc_val_cache1[i] > 4096) {
      adc_val_cache1[i] = 4096;
    }
    if (adc_val_cache2[i] > 4096) {
      adc_val_cache2[i] = 4096;
    }
    if (adc_val_cache3[i] > 4096) {
      adc_val_cache3[i] = 4096;
    }

    if (adc_val_cache0[i] < 0) {
      adc_val_cache0[i] = 0;
    }
    if (adc_val_cache1[i] < 0) {
      adc_val_cache1[i] = 0;
    }
    if (adc_val_cache2[i] < 0) {
      adc_val_cache2[i] = 0;
    }
    if (adc_val_cache3[i] < 0) {
      adc_val_cache3[i] = 0;
    }
  }

  for (int i = 0; i < adc_val_index; i++) {
    data[i * 12 + 0]  = 0xA2;
    data[i * 12 + 1]  = 0x2;
    data[i * 12 + 2]  = adc_val_cache0[i] & 0xff;
    data[i * 12 + 3]  = adc_val_cache0[i] >> 8 & 0xff;
    data[i * 12 + 4]  = adc_val_cache1[i] & 0xff;
    data[i * 12 + 5]  = adc_val_cache1[i] >> 8 & 0xff;
    data[i * 12 + 6]  = adc_val_cache2[i] & 0xff;
    data[i * 12 + 7]  = adc_val_cache2[i] >> 8 & 0xff;
    data[i * 12 + 8]  = adc_val_cache3[i] & 0xff;
    data[i * 12 + 9]  = adc_val_cache3[i] >> 8 & 0xff;
    data[i * 12 + 10] = 0x2;
    data[i * 12 + 11] = 0xA2;
  }
  zdatachannel_data_send2(data, 12 * adc_val_index);
}
#if 1

#if 0
// 定义一阶高通滤波器结构体
typedef struct {
  float alpha;            // 时间常数
  float previous_output;  // 上一时刻的输出
} HighPassFilter;

HighPassFilter myFilter;

// 初始化滤波器
void initializeFilter(HighPassFilter* filter, float alpha) {
  filter->alpha           = alpha;
  filter->previous_output = 0.0;
}

// 一阶高通滤波函数
float filterValue(HighPassFilter* filter, float input) {
  // 计算输出
  float output = filter->alpha * (input - filter->previous_output) + filter->previous_output;

  // 更新上一次的输出
  filter->previous_output = output;

  return output;
}
#endif

#endif

NOTCHFilter notchfilter;
LPFilter    lowpassfilter;
static void nrfx_timer_event_handler(nrf_timer_event_t event_type, void *p_context) {  //
  // raw data
  int16_t val                   = SingleLeadECG_ecg_plod_get_ecg_val();  //
  adc_val_cache0[adc_val_index] = val;

  int16_t notchf_val            = NOTCHFilter_Update(&notchfilter, val);
  adc_val_cache1[adc_val_index] = notchf_val;

  int16_t lowpassf_val          = LPFilter_Update(&lowpassfilter, notchf_val);
  adc_val_cache2[adc_val_index] = lowpassf_val;

  // val = low_pass_filter(val);
  /*******************************************************************************
   *                             显示数据计算并赋值                              *
   *******************************************************************************/
  adc_val_index++;
  if (adc_val_index >= 5) {
    if (zdatachannel_is_connected()) {
      sendpacket_to_pc();
    } else {
    }
    adc_val_index = 0;
  }

  static int  cnt;
  static bool state;
  cnt++;
  if (zdatachannel_is_connected()) {
    SingleLeadECG_led_green_set_state(1);
  } else {
    if (cnt % 20 == 0) {
      SingleLeadECG_led_green_set_state(state);
      state = !state;
    }
  }
}
int main() {  //
  APP_SCHED_INIT(APP_TIMER_SCHED_EVENT_DATA_SIZE, 20);
  // low_pass_filter_init();

  // initializeFilter(&myFilter, alpha);

  NOTCHFilter_Init(&notchfilter, 50, 30, 0.002);
  LPFilter_Init(&lowpassfilter, 200, 0.002);

  znordic_init();
  NRF_LOG_INFO("compile time :%s", __TIME__);

  ztm_t                    tm;
  static zble_module_cfg_t cfg =  //
      {
          .deviceName      = "OneLeadTest",
          .on_service_init = on_service_init,
      };
  zble_module_init(&cfg);
  SingleLeadECG_adc_module_init();
  SingleLeadECG_led_init();
  SingleLeadECG_led_green_set_state(0);

  zble_module_start_adv();

  /*******************************************************************************
   *                                定时器初始化                                 *
   *******************************************************************************/
  /**
   * @brief 初始化定时器
   */
  nrfx_err_t          err;
  nrfx_timer_config_t timer_cfg = {
      .frequency          = NRF_TIMER_FREQ_500kHz,
      .mode               = NRF_TIMER_MODE_TIMER,
      .bit_width          = NRF_TIMER_BIT_WIDTH_24,
      .p_context          = NULL,
      .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
  };

  err = nrfx_timer_init(&m_timer, &timer_cfg, nrfx_timer_event_handler);
  if (err != NRFX_SUCCESS) {
    NRF_LOG_ERROR("nrfx_timer_init failed with: %d\n", err);
  }
  uint32_t timer_ticks = nrfx_timer_ms_to_ticks(&m_timer, 2);  // 500HZ
  nrfx_timer_extended_compare(&m_timer, NRF_TIMER_CC_CHANNEL0, timer_ticks, NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK, true);
  nrfx_timer_enable(&m_timer);
  znordic_loop();
}

#endif