#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <Arduino.h>

// =====================================================
// Лабораторна 3: Edge-обробка на потоці
// raw -> filtered -> features -> events
// Рекомендований режим: USE_SYNTHETIC_SENSOR = true
// =====================================================

const uint8_t VARIANT = 4; // змінювати тільки номер варіанту: 1..12
const bool USE_SYNTHETIC_SENSOR = true;
const uint16_t Fs = 200; // Гц
const float Tobs = 6.0f; // секунди
const uint16_t BUF_CAP = 16; // буфер потоку (спадок ЛР2)
const uint16_t CSV_EVERY_NTH = 1; // для великих Fs можна збільшити

const float PI_F = 3.1415926535f;

enum FilterKind {
  FILTER_AUTO = -1,
  FILTER_MA = 0,
  FILTER_EMA = 1,
  FILTER_MEDIAN = 2
};

// Для експериментів можна змінювати лише ці override-параметри:
const int8_t FILTER_OVERRIDE = FILTER_AUTO; // -1 = взяти з варіанту,0=MA,1=EMA,2=MEDIAN
const uint8_t FILTER_N_OVERRIDE = 0; // 0 = взяти з варіанту
const float EMA_ALPHA_OVERRIDE = -1.0f; // <0 = взяти з варіанту
const uint8_t FEATURE_WIN_OVERRIDE = 16; // 0 = взяти з варіанту
const float TH_HIGH_SCALE = 1.0f; // 0.85..1.15 для аналізу чутливості
const float TH_LOW_SCALE = 1.0f;
const int16_t HOLD_OFF_OVERRIDE = 20; // <0 = взяти з варіанту
const uint8_t MAX_FILTER_WIN = 11;
const uint8_t MAX_FEATURE_WIN = 24;

struct VariantConfig {
  const char* id;
  FilterKind base_filter;
  uint8_t filter_n;
  float ema_alpha;
  uint8_t feature_win;
  float th_high;
  float th_low;
  uint16_t hold_off_samples;
  float event_amp;
  float noise_sigma;
  uint16_t event_period_samples;
  uint8_t event_width_samples;
  float drift_amp;
};

struct Sample {
  uint32_t seq;
  uint32_t t_us;
  float x_obs;
  float x_true;
  uint8_t truth_event;
};

struct RingBuffer {
  Sample data[BUF_CAP];
  uint16_t head = 0;
  uint16_t tail = 0;
  uint16_t count = 0;
};

struct FilterState {
  FilterKind kind = FILTER_MA;
  uint8_t n = 5;
  float alpha = 0.2f;

  float fbuf[MAX_FILTER_WIN];
  uint8_t fcount = 0;
  uint8_t findex = 0;
  float fsum = 0.0f;
  float ema = 0.0f;
  bool ema_init = false;

  float featbuf[MAX_FEATURE_WIN];
  uint8_t feat_count = 0;
  uint8_t feat_index = 0;
  float prev_filtered = 0.0f;
  bool prev_init = false;

  bool event_state = false;
  uint16_t holdoff_left = 0;
  uint32_t event_count = 0;
};

struct Metrics {
  uint32_t N = 0;
  double abs_err = 0.0;
  double sq_err = 0.0;
  double sig_sq = 0.0;
  uint32_t tp = 0;
  uint32_t fp = 0;
  uint32_t fn = 0;
  uint32_t tn = 0;
};

static RingBuffer rb;
static uint32_t rng_state = 12345UL;

float clipf(float x, float a, float b) {
  if (x < a) return a;
  if (x > b) return b;
  return x;
}

float randu01() {
  rng_state = 1664525UL * rng_state + 1013904223UL;
  return (rng_state >> 8) * (1.0f / 16777216.0f);
}

float randn01_approx() {
  float s = 0.0f;
  for (int i = 0; i < 12; i++) s += randu01();
  return s - 6.0f;
}

void loadVariant(uint8_t v, VariantConfig &cfg) {
  switch (v) {
    case 1: cfg = {"V1", FILTER_MA, 5, 0.25f, 8, 1.20f, 0.75f, 10, 150.0f, 8.0f, 95, 9, 20.0f}; break;
    case 2: cfg = {"V2", FILTER_EMA, 5, 0.22f, 10, 1.15f, 0.72f, 12, 170.0f, 10.0f, 110, 10, 25.0f}; break;
    case 3: cfg = {"V3", FILTER_MEDIAN, 5, 0.30f, 8, 1.25f, 0.80f, 10, 160.0f, 14.0f, 90, 8, 18.0f}; break;
    case 4: cfg = {"V4", FILTER_MA, 7, 0.25f, 12, 1.25f, 0.78f, 14, 180.0f, 12.0f, 120, 12, 20.0f}; break;
    case 5: cfg = {"V5", FILTER_EMA, 5, 0.18f, 14, 1.05f, 0.68f, 14, 140.0f, 6.0f, 130, 13, 16.0f}; break;
    case 6: cfg = {"V6", FILTER_MEDIAN, 7, 0.28f, 10, 1.30f, 0.85f, 12, 200.0f, 18.0f, 100, 11, 22.0f}; break;
    case 7: cfg = {"V7", FILTER_MA, 9, 0.25f, 14, 1.22f, 0.78f, 16, 190.0f, 10.0f, 140, 14, 24.0f}; break;
    case 8: cfg = {"V8", FILTER_EMA, 7, 0.15f, 16, 1.02f, 0.65f, 16, 130.0f, 7.0f, 150, 15, 14.0f}; break;
    case 9: cfg = {"V9", FILTER_MEDIAN, 5, 0.26f, 12, 1.35f, 0.90f, 10, 210.0f, 20.0f, 105, 9, 28.0f}; break;
    case 10: cfg = {"V10", FILTER_MA, 5, 0.24f, 8, 1.15f, 0.72f, 8, 155.0f, 9.0f, 85, 8, 18.0f}; break;
    case 11: cfg = {"V11", FILTER_EMA, 9, 0.12f, 18, 0.98f, 0.60f, 18, 145.0f, 11.0f, 160, 16, 16.0f}; break;
    case 12: cfg = {"V12", FILTER_MEDIAN, 7, 0.20f, 10, 1.38f, 0.92f, 14, 220.0f, 15.0f, 115, 10, 26.0f}; break;
    default: cfg = {"V1", FILTER_MA, 5, 0.25f, 8, 1.20f, 0.75f, 10, 150.0f, 8.0f, 95, 9, 20.0f}; break;
  }
}

void applyOverrides(const VariantConfig &src, VariantConfig &dst) {
  dst = src;
  if (FILTER_OVERRIDE >= 0) dst.base_filter = (FilterKind)FILTER_OVERRIDE;
  if (FILTER_N_OVERRIDE > 0) dst.filter_n = FILTER_N_OVERRIDE;
  if (EMA_ALPHA_OVERRIDE >= 0.0f) dst.ema_alpha = EMA_ALPHA_OVERRIDE;
  if (FEATURE_WIN_OVERRIDE > 0) dst.feature_win = FEATURE_WIN_OVERRIDE;
  if (HOLD_OFF_OVERRIDE >= 0) dst.hold_off_samples = (uint16_t)HOLD_OFF_OVERRIDE;
  dst.th_high *= TH_HIGH_SCALE;
  dst.th_low *= TH_LOW_SCALE;
  if (dst.filter_n > MAX_FILTER_WIN) dst.filter_n = MAX_FILTER_WIN;
  if (dst.feature_win > MAX_FEATURE_WIN) dst.feature_win = MAX_FEATURE_WIN;
}

const char* filterName(FilterKind k) {
  if (k == FILTER_MA) return "MA";
  if (k == FILTER_EMA) return "EMA";
  return "MEDIAN";
}

bool rb_push(const Sample &s) {
  if (rb.count >= BUF_CAP) return false;
  rb.data[rb.head] = s;
  rb.head = (rb.head + 1) % BUF_CAP;
  rb.count++;
  return true;
}

bool rb_pop(Sample &s) {
  if (rb.count == 0) return false;
  s = rb.data[rb.tail];
  rb.tail = (rb.tail + 1) % BUF_CAP;
  rb.count--;
  return true;
}

void filterInit(FilterState &st, const VariantConfig &cfg) {
  st.kind = cfg.base_filter;
  st.n = cfg.filter_n;
  st.alpha = cfg.ema_alpha;
  st.fcount = 0;
  st.findex = 0;
  st.fsum = 0.0f;
  st.ema = 0.0f;
  st.ema_init = false;
  st.feat_count = 0;
  st.feat_index = 0;
  st.prev_filtered = 0.0f;
  st.prev_init = false;
  st.event_state = false;
  st.holdoff_left = 0;
  st.event_count = 0;
}

float filterUpdate(FilterState &st, float x) {
  if (st.kind == FILTER_MA) {
    if (st.fcount < st.n) {
      st.fbuf[st.findex] = x;
      st.fsum += x;
      st.fcount++;
      st.findex = (st.findex + 1) % st.n;
    } else {
      st.fsum -= st.fbuf[st.findex];
      st.fbuf[st.findex] = x;
      st.fsum += x;
      st.findex = (st.findex + 1) % st.n;
    }
    return st.fsum / (float)st.fcount;
  }

  if (st.kind == FILTER_EMA) {
    if (!st.ema_init) {
      st.ema = x;
      st.ema_init = true;
    } else {
      st.ema = st.alpha * x + (1.0f - st.alpha) * st.ema;
    }
    return st.ema;
  }

  // median
  if (st.fcount < st.n) {
    st.fbuf[st.fcount++] = x;
  } else {
    st.fbuf[st.findex] = x;
    st.findex = (st.findex + 1) % st.n;
  }
  float tmp[MAX_FILTER_WIN];
  for (uint8_t i = 0; i < st.fcount; i++) tmp[i] = st.fbuf[i];
  for (uint8_t i = 1; i < st.fcount; i++) {
    float key = tmp[i];
    int8_t j = i - 1;
    while (j >= 0 && tmp[j] > key) {
      tmp[j + 1] = tmp[j];
      j--;
    }
    tmp[j + 1] = key;
  }
  return tmp[st.fcount / 2];
}

void featurePush(FilterState &st, float x, uint8_t maxwin) {
  if (st.feat_count < maxwin) {
    st.featbuf[st.feat_count++] = x;
  } else {
    st.featbuf[st.feat_index] = x;
    st.feat_index = (st.feat_index + 1) % maxwin;
  }
}

void computeFeatures(
    FilterState &st,
    uint8_t maxwin,
    float filtered,
    float &mean_w,
    float &pos_dev,
    float &pos_slope,
    float &range_w,
    float &mad_w) {
  featurePush(st, filtered, maxwin);
  mean_w = 0.0f;
  float minv = st.featbuf[0];
  float maxv = st.featbuf[0];
  for (uint8_t i = 0; i < st.feat_count; i++) {
    float v = st.featbuf[i];
    mean_w += v;
    if (v < minv) minv = v;
    if (v > maxv) maxv = v;
  }
  mean_w /= (float)st.feat_count;
  range_w = maxv - minv;

  mad_w = 0.0f;
  for (uint8_t i = 0; i < st.feat_count; i++) {
    mad_w += fabsf(st.featbuf[i] - mean_w);
  }
  mad_w /= (float)st.feat_count;

  pos_dev = filtered - mean_w;
  if (pos_dev < 0.0f) pos_dev = 0.0f;

  float slope = 0.0f;
  if (st.prev_init) slope = filtered - st.prev_filtered;
  st.prev_filtered = filtered;
  st.prev_init = true;

  pos_slope = slope;
  if (pos_slope < 0.0f) pos_slope = 0.0f;
}

float computeScore(
    const VariantConfig &cfg,
    float pos_dev,
    float pos_slope,
    float range_w,
    float mad_w) {
  float s1 = pos_dev / (cfg.noise_sigma * 1.7f + 6.0f);
  float s2 = pos_slope / (cfg.noise_sigma * 1.3f + 4.0f);
  float s3 = range_w / (cfg.event_amp * 0.22f + 8.0f);
  float s4 = mad_w / (cfg.noise_sigma * 1.5f + 5.0f);
  return 0.45f * s1 + 0.25f * s2 + 0.20f * s3 + 0.10f * s4;
}

uint8_t updateDetector(FilterState &st, const VariantConfig &cfg, float score) {
  if (st.holdoff_left > 0) st.holdoff_left--;

  if (!st.event_state && st.holdoff_left == 0 && score >= cfg.th_high) {
    st.event_state = true;
    st.event_count++;
  } else if (st.event_state && score <= cfg.th_low) {
    st.event_state = false;
    st.holdoff_left = cfg.hold_off_samples;
  }
  return st.event_state ? 1 : 0;
}

void updateMetrics(Metrics &m, float truth_signal, float filtered, uint8_t truth_event, uint8_t
    det_event) {
  double e = (double)filtered - (double)truth_signal;
  m.N++;
  m.abs_err += fabs(e);
  m.sq_err += e * e;
  m.sig_sq += (double)truth_signal * (double)truth_signal;

  if (det_event && truth_event) m.tp++;
  else if (det_event && !truth_event) m.fp++;
  else if (!det_event && truth_event) m.fn++;
  else m.tn++;
}

float synthTruthSignal(uint32_t n, const VariantConfig &cfg, uint8_t &truth_event) {
  float t = (float)n / (float)Fs;
  float pulse = 0.0f;
  uint16_t pos = n % cfg.event_period_samples;
  if (pos < cfg.event_width_samples) {
    float phase = ((float)pos + 0.5f) / (float)cfg.event_width_samples;
    pulse = cfg.event_amp * sinf(PI_F * phase);
  }
  truth_event = (pulse > 0.25f * cfg.event_amp) ? 1 : 0;

  float base = 512.0f
    + 70.0f * sinf(2.0f * PI_F * 0.35f * t)
    + cfg.drift_amp * sinf(2.0f * PI_F * 0.07f * t + 0.4f)
    + pulse;
  return clipf(base, 0.0f, 1023.0f);
}

float sampleObserved(uint32_t n, const VariantConfig &cfg, float &truth_signal, uint8_t
    &truth_event) {
  if (!USE_SYNTHETIC_SENSOR) {
    truth_event = 0;
    truth_signal = (float)analogRead(A0);
    return truth_signal;
  }

  truth_signal = synthTruthSignal(n, cfg, truth_event);
  float noisy = truth_signal + cfg.noise_sigma * randn01_approx();
  return clipf(noisy, 0.0f, 1023.0f);
}

double snr_db(double ps, double pn) {
  const double eps = 1e-12;
  return 10.0 * log10((ps + eps) / (pn + eps));
}

void printSummary(const VariantConfig &cfg, const FilterState &st, const Metrics &m) {
  double N = (double)m.N;
  double mae = m.abs_err / N;
  double rmse = sqrt(m.sq_err / N);
  double snr = snr_db(m.sig_sq / N, m.sq_err / N);
  double precision = (m.tp + m.fp) ? ((double)m.tp / (double)(m.tp + m.fp)) : 0.0;
  double recall = (m.tp + m.fn) ? ((double)m.tp / (double)(m.tp + m.fn)) : 0.0;

  Serial.println("#SUMMARY");
  Serial.print("variant="); Serial.println(cfg.id);
  Serial.print("filter="); Serial.println(filterName(cfg.base_filter));
  Serial.print("Fs="); Serial.println((int)Fs);
  Serial.print("filter_n="); Serial.println((int)cfg.filter_n);
  Serial.print("feature_win="); Serial.println((int)cfg.feature_win);
  Serial.print("th_high="); Serial.println(cfg.th_high, 3);
  Serial.print("th_low="); Serial.println(cfg.th_low, 3);
  Serial.print("hold_off_samples="); Serial.println((int)cfg.hold_off_samples);
  Serial.print("MAE_filter="); Serial.println(mae, 4);
  Serial.print("RMSE_filter="); Serial.println(rmse, 4);
  Serial.print("SNR_filter_dB="); Serial.println(snr, 3);
  Serial.print("TP="); Serial.println((unsigned long)m.tp);
  Serial.print("FP="); Serial.println((unsigned long)m.fp);
  Serial.print("FN="); Serial.println((unsigned long)m.fn);
  Serial.print("precision="); Serial.println(precision, 4);
  Serial.print("recall="); Serial.println(recall, 4);
  Serial.print("detected_events="); Serial.println((unsigned long)st.event_count);
  Serial.println("#DONE");
}

void setup() {
  Serial.begin(115200);

  VariantConfig base_cfg, cfg;
  loadVariant(VARIANT, base_cfg);
  applyOverrides(base_cfg, cfg);

  FilterState st;
  filterInit(st, cfg);
  Metrics mt;

  uint32_t total_samples = (uint32_t)(Tobs * (float)Fs);
  uint32_t dt_us = 1000000UL / (uint32_t)Fs;
  uint32_t dropped = 0;

  Serial.print("#VARIANT,"); Serial.println(cfg.id);
  Serial.print("#FILTER,"); Serial.println(filterName(cfg.base_filter));
  Serial.println("#CSV_BEGIN");
  Serial.println("t_us,seq,raw,true,filtered,mean_w,pos_dev,pos_slope,range_w,mad_w,score,event,truth_event,occ");

  for (uint32_t n = 0; n < total_samples; n++) {
#ifdef UNIT_TEST
    g_now_us = n * dt_us;
#endif
    Sample s;
    s.seq = n;
    s.t_us = n * dt_us;
    s.x_obs = sampleObserved(n, cfg, s.x_true, s.truth_event);

    if (!rb_push(s)) {
      dropped++;
      continue;
    }

    Sample cur;
    if (rb_pop(cur)) {
      float filtered = filterUpdate(st, cur.x_obs);

      float mean_w = 0.0f, pos_dev = 0.0f, pos_slope = 0.0f, range_w = 0.0f, mad_w = 0.0f;
      computeFeatures(st, cfg.feature_win, filtered, mean_w, pos_dev, pos_slope, range_w,mad_w);
      float score = computeScore(cfg, pos_dev, pos_slope, range_w, mad_w);
      uint8_t event_flag = updateDetector(st, cfg, score);

      updateMetrics(mt, cur.x_true, filtered, cur.truth_event, event_flag);

      if ((cur.seq % CSV_EVERY_NTH) == 0) {
        Serial.print(cur.t_us); Serial.print(",");
        Serial.print(cur.seq); Serial.print(",");
        Serial.print(cur.x_obs, 3); Serial.print(",");
        Serial.print(cur.x_true, 3); Serial.print(",");
        Serial.print(filtered, 3); Serial.print(",");
        Serial.print(mean_w, 3); Serial.print(",");
        Serial.print(pos_dev, 3); Serial.print(",");
        Serial.print(pos_slope, 3); Serial.print(",");
        Serial.print(range_w, 3); Serial.print(",");
        Serial.print(mad_w, 3); Serial.print(",");
        Serial.print(score, 3); Serial.print(",");
        Serial.print((int)event_flag); Serial.print(",");
        Serial.print((int)cur.truth_event); Serial.print(",");
        Serial.println((int)rb.count);
      }
    }
  }

  Serial.println("#CSV_END");
  Serial.print("#dropped="); Serial.println((unsigned long)dropped);
  printSummary(cfg, st, mt);
}

void loop() {
  delay(1000);
}