#include "qrs_time_domain_zh.h"

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#define HEART_RATE_FILTER_SIZE 10

typedef struct {
  uint16_t data[HEART_RATE_FILTER_SIZE];
  uint16_t data_process_buf[HEART_RATE_FILTER_SIZE];
  uint32_t cnt;
  uint32_t index;
} HeartRateMedianFilter_t;  // 中值滤波器

typedef struct {
  uint16_t data[HEART_RATE_FILTER_SIZE];
  uint32_t cnt;
  uint32_t index;
  uint32_t sum;
} HeartRateMeanFilter_t;  // 均值滤波器

HeartRateMedianFilter_t m_heart_rate_median_filter;
HeartRateMeanFilter_t   m_heart_rate_mean_filter;

static void HeartRateMedianFilter_reset() {
  memset(m_heart_rate_median_filter.data, 0, sizeof(m_heart_rate_median_filter.data));
  m_heart_rate_median_filter.cnt   = 0;
  m_heart_rate_median_filter.index = 0;
}
static uint16_t HeartRateMedianFilter_process(uint16_t data) {
  HeartRateMedianFilter_t* pfilter = &m_heart_rate_median_filter;

  pfilter->data[pfilter->index] = data;
  pfilter->index++;
  pfilter->cnt++;
  if (pfilter->index >=HEART_RATE_FILTER_SIZE) {
    pfilter->index = 0;
  }

  if (pfilter->cnt <HEART_RATE_FILTER_SIZE) {
    return data;
  }

  memcpy(pfilter->data_process_buf, pfilter->data, HEART_RATE_FILTER_SIZE * sizeof(uint16_t));
  for (uint8_t i = 0; i < HEART_RATE_FILTER_SIZE; i++) {
    for (uint8_t j = i + 1; j < HEART_RATE_FILTER_SIZE; j++) {
      if (pfilter->data_process_buf[i] > pfilter->data_process_buf[j]) {
        uint16_t temp                = pfilter->data_process_buf[i];
        pfilter->data_process_buf[i] = pfilter->data_process_buf[j];
        pfilter->data_process_buf[j] = temp;
      }
    }
  }
  return pfilter->data_process_buf[2];
}
static void HeartRateMeanFilter_reset() {
  memset(m_heart_rate_mean_filter.data, 0, sizeof(m_heart_rate_mean_filter.data));
  m_heart_rate_mean_filter.cnt   = 0;
  m_heart_rate_mean_filter.index = 0;
  m_heart_rate_mean_filter.sum   = 0;
}
static uint16_t HeartRateMeanFilter_process(uint16_t data) {
  HeartRateMeanFilter_t* pfilter = &m_heart_rate_mean_filter;

  pfilter->sum -= pfilter->data[pfilter->index];
  pfilter->data[pfilter->index] = data;
  pfilter->sum += data;

  pfilter->index++;
  pfilter->cnt++;

  if (pfilter->index >= HEART_RATE_FILTER_SIZE) {
    pfilter->index = 0;
  }

  if (pfilter->cnt < HEART_RATE_FILTER_SIZE) {
    return data;
  }

  return pfilter->sum / HEART_RATE_FILTER_SIZE;
}

static uint16_t m_data[TABLE_SIZE];
static uint32_t m_ndata     = 0;
static uint32_t m_dataindex = 0;
static uint32_t m_data_cnt  = 0;
static uint16_t m_heartrate = 0;

static uint32_t m_datasum = 0;
static float    m_avg     = 0;
static uint32_t m_max_val_in_m_data;

static bool m_findpeak = false;

static uint16_t pQRS_median_filter_cache[HEART_RATE_FILTER_SIZE];
static uint16_t pQRS_median_filter_cache_index = 0;
static uint16_t pQRS_median_filter_cache_cnt   = 0;

static uint32_t m_last_peak_pos = 0;
static uint32_t m_peakcnt       = 0;

static uint16_t pQRS_median_filter(uint16_t indata) {
  //   memcpy(pQRS_median_filter_cache + 1, pQRS_median_filter_cache, 4 * sizeof(uint16_t));
  pQRS_median_filter_cache[pQRS_median_filter_cache_index] = indata;
  pQRS_median_filter_cache_index++;
  pQRS_median_filter_cache_cnt++;
  if (pQRS_median_filter_cache_index >= HEART_RATE_FILTER_SIZE) {
    pQRS_median_filter_cache_index = 0;
  }

  if (pQRS_median_filter_cache_cnt < HEART_RATE_FILTER_SIZE) {
    return indata;
  }

  static uint16_t process_cache[HEART_RATE_FILTER_SIZE];
  memcpy(process_cache, pQRS_median_filter_cache, HEART_RATE_FILTER_SIZE * sizeof(uint16_t));
  for (uint8_t i = 0; i < HEART_RATE_FILTER_SIZE; i++) {
    for (uint8_t j = i + 1; j < HEART_RATE_FILTER_SIZE; j++) {
      if (process_cache[i] > process_cache[j]) {
        uint16_t temp    = process_cache[i];
        process_cache[i] = process_cache[j];
        process_cache[j] = temp;
      }
    }
  }
  return process_cache[2];
}

static uint32_t pQRS_findMaxValue() {
  uint32_t max_val = 0;
  for (uint32_t i = 0; i < TABLE_SIZE; i++) {
    if (m_data[i] > max_val) {
      max_val = m_data[i];
    }
  }
  return max_val;
}

void QRS_resetBuf() {  //
  m_ndata     = 0;
  m_dataindex = 0;
  m_heartrate = 0;
  m_data_cnt  = 0;
  memset(m_data, 0, sizeof(m_data));
  m_datasum                      = 0;
  m_findpeak                     = false;
  pQRS_median_filter_cache_index = 0;
  pQRS_median_filter_cache_cnt   = 0;
  m_peakcnt                      = 0;

  HeartRateMedianFilter_reset();
  HeartRateMeanFilter_reset();
}

void QRS_processData(uint16_t _data) {
  uint16_t data = pQRS_median_filter(_data);
  /*******************************************************************************
   *                                   填充BUF                                   *
   *******************************************************************************/
  m_datasum -= m_data[m_dataindex];
  m_data[m_dataindex] = data;
  m_datasum += data;

  m_data_cnt++;

  if (m_dataindex < TABLE_SIZE) {
    m_dataindex++;
  } else {
    m_dataindex = 0;
  }

  m_ndata++;
  if (m_ndata > TABLE_SIZE) {
    m_ndata = TABLE_SIZE;
  }

  /*******************************************************************************
   *                            求BUF的平均值和最大值                            *
   *******************************************************************************/
  if (m_ndata == TABLE_SIZE) {
    m_avg               = (float)m_datasum / m_ndata;
    m_max_val_in_m_data = pQRS_findMaxValue();
  }

  /*******************************************************************************
   *                            寻找QRS波峰和波谷                             *
   *******************************************************************************/

  if (!m_findpeak) {
    uint16_t thresholdValue = (m_max_val_in_m_data - m_avg) * 0.666 + m_avg;
    if (data > thresholdValue) {
      m_findpeak = true;
      m_peakcnt++;

      if (m_last_peak_pos != 0) {
        uint16_t diff_peak_pos = m_data_cnt - m_last_peak_pos;
        if (diff_peak_pos > 0) {
          //
          // m_heartrate = 60 * 500 / diff_peak_pos;

          uint16_t diff_peak_ms = diff_peak_pos * 2;  // 500Hz
          uint16_t heart_rate   = 60 * 1000 / diff_peak_ms;

          m_heartrate = HeartRateMeanFilter_process(HeartRateMedianFilter_process(heart_rate));
        }
      }

      m_last_peak_pos = m_data_cnt;
    }
  } else {
    if (data < m_avg) {
      m_findpeak = false;
    }
  }
}
uint16_t QRS_getHeartRate() {
  __disable_fiq();
  uint16_t heartrate = m_heartrate;
  __enable_fiq();

  if (heartrate > 200) return 0;
  if (heartrate < 55) return 0;
  return heartrate;
}

uint16_t QRS_getMaxValueLastVal() { return m_max_val_in_m_data; }

uint16_t QRS_getAvgValueVal() {  //
  return m_avg;
}