#include <Arduino.h>;
#include <math.h>;

// =====================================================
// НАЛАШТУВАННЯ ДЛЯ ВАРІАНТІВ (ЗМІНЮВАТИ ТУТ)
// =====================================================

// ID варіанту/ПІБ для маркування виводу
const char* VARIANT_ID = "Dmytro Buriak, V4";

// "Аналогова" частота внутрішнього моделювання (Гц)
// ВАЖЛИВО: повинна ділитися на Fs без остачі (Fref % Fs == 0)
const uint32_t Fref = 20000;

// Частота дискретизації (Гц) – міняйте для експериментів
const uint16_t Fs = 2000; // напр. 200, 500, 2000

// Тривалість експерименту (сек)
const float Tobs = 3.0f;

// Корисний сигнал
const float A_sig = 1.0f;
const float f_sig = 30.0f; // Гц

// Повільний дрейф (опційно; якщо не потрібно – A_drift = 0)
const float A_drift = 0.15f;
const float f_drift = 0.2f; // Гц

// Високочастотна завада для aliasing
const float A_hf = 0.6f;
const float f_hf = 775.0f; // Гц (для Fs=200 має бути > 100 Гц)

// Шум (гаусівський)
const float noise_sigma = 0.18f;

// Антиаліасинг (RC-подібний НЧ-фільтр до вибірки)
const bool AA_ENABLED = false;

// Частота зрізу Fc як частка від Fs: Fc = AA_ratio * Fs
const float AA_ratio = 0.45f; // 0.35..0.49

// Квантування (бітність "АЦП")
const uint8_t ADC_BITS = 12; // напр. 8 / 12 / 16

// Діапазон квантування
const float Vmin = -2.0f;
const float Vmax = 2.0f;

// Скільки кожну N-ту точку виводити у CSV (щоб не “заспамити” серіал)
const uint16_t CSV_EVERY_NTH = 3; // для Fs=2000 можна поставити 2..5

// =====================================================
// СЛУЖБОВЕ (НЕ ОБОВ'ЯЗКОВО ЧІПАТИ)
// =====================================================

static uint32_t rng_state = 42;

// простий RNG -> приблизний N(0,1) через суму U(0,1)
float randu01() {
  rng_state = 1664525UL * rng_state + 1013904223UL;
  return (rng_state >> 8) * (1.0f / 16777216.0f); // 24 біти
}
float randn01_approx() {
  // сума 12 рівномірних ~ N(0,1) (грубо)
  float s = 0;

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

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

float quantize(float x, float vmin, float vmax, uint8_t bits) {
  x = clipf(x, vmin, vmax);
  uint32_t levels = 1UL << bits; // 2^bits
  float delta = (vmax - vmin) / (levels - 1);
  float code = roundf((x - vmin) / delta);
  return code * delta + vmin;
}

void update_metrics(float x_true, float x_est, double &sum_abs, double &sum_sq, double &sum_true_sq) {
  double e = (double)x_est - (double)x_true;
  sum_abs += fabs(e);
  sum_sq += e * e;
  sum_true_sq += (double)x_true * (double)x_true;
}

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

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

  // Перевірка ділимості
  if (Fref % Fs != 0) {
    Serial.println("#ERROR: Fref must be divisible by Fs (Fref % Fs == 0).");
    while (true) delay(1000);
  }

  const uint32_t decim = Fref / Fs;
  const uint32_t total_ref_steps = (uint32_t)(Tobs * (float)Fref);
  const uint32_t total_samples = (uint32_t)(Tobs * (float)Fs);

  // RC-подібний фільтр: y += alpha*(x - y)
  const float dt = 1.0f / (float)Fref;
  const float Fc = AA_ratio * (float)Fs;
  const float alpha = (2.0f * (float)M_PI * Fc * dt) / (1.0f + 2.0f * (float)M_PI * Fc * dt);

  // Метрики (окремо для raw і AA)
  double abs_raw = 0, sq_raw = 0, true_sq_raw = 0;
  double abs_aa = 0, sq_aa = 0, true_sq_aa = 0;

  // CSV заголовок
  Serial.print("#VARIANT,"); Serial.print(VARIANT_ID);
  Serial.print(",Fs="); Serial.print(Fs);
  Serial.print(",ADC_BITS="); Serial.print(ADC_BITS);
  Serial.print(",AA_ENABLED="); Serial.println(AA_ENABLED ? "1" : "0");
  Serial.println("#CSV_BEGIN");
  Serial.println("t,x_true,x_raw,x_aa,x_q_raw,x_q_aa");

  // Стан фільтра
  float y = 0.0f;

  uint32_t sample_idx = 0;

  for (uint32_t k = 0; k < total_ref_steps; k++) {
    float t = (float)k / (float)Fref;

    // "Істина" (корисне + дрейф)
    float x_true = A_sig * sinf(2.0f * (float)M_PI * f_sig * t);
    if (A_drift != 0.0f) {
      x_true += A_drift * sinf(2.0f * (float)M_PI * f_drift * t);
    }

    // Спостереження (додаємо HF + шум)
    float x_obs = x_true + A_hf * sinf(2.0f * (float)M_PI * f_hf * t) + noise_sigma * randn01_approx();

    // антиаліасинг (на високій Fref, як аналог RC)
    if (AA_ENABLED) {
      y = y + alpha * (x_obs - y);
    } else {
      y = x_obs;
    }

    // момент "вибірки" кожні decim кроків
    if ((k % decim) == 0 && sample_idx < total_samples) {
      float t_s = (float)sample_idx / (float)Fs;

      // x_raw – це значення x_obs у моменті вибірки
      float x_raw = x_obs;
      // x_aa – це відфільтроване до вибірки
      float x_aa = y;

      // квантування
      float x_q_raw = quantize(x_raw, Vmin, Vmax, ADC_BITS);
      float x_q_aa = quantize(x_aa, Vmin, Vmax, ADC_BITS);

      // метрики відносно істини (істина у моменті t_s)
      float x_true_s = A_sig * sinf(2.0f * (float)M_PI * f_sig * t_s);
      if (A_drift != 0.0f) {
        x_true_s += A_drift * sinf(2.0f * (float)M_PI * f_drift * t_s);
      }

      update_metrics(x_true_s, x_q_raw, abs_raw, sq_raw, true_sq_raw);
      update_metrics(x_true_s, x_q_aa, abs_aa, sq_aa, true_sq_aa);

      // CSV вивід (можна проріджувати)
      if ((sample_idx % CSV_EVERY_NTH) == 0) {
        Serial.print(t_s, 6); Serial.print(",");
        Serial.print(x_true_s, 6); Serial.print(",");
        Serial.print(x_raw, 6); Serial.print(",");
        Serial.print(x_aa, 6); Serial.print(",");
        Serial.print(x_q_raw, 6); Serial.print(",");
        Serial.println(x_q_aa, 6);
      }

      sample_idx++;
    }
  }

  Serial.println("#CSV_END");

  // Підсумкові метрики
  double N = (double)total_samples;
  double mae_raw = abs_raw / N;
  double rmse_raw = sqrt(sq_raw / N);
  double mae_aa = abs_aa / N;
  double rmse_aa = sqrt(sq_aa / N);

  double ps_raw = true_sq_raw / N;
  double pn_raw = sq_raw / N;
  double ps_aa = true_sq_aa / N;
  double pn_aa = sq_aa / N;
  
  Serial.println("#SUMMARY");
  Serial.print("MAE_raw="); Serial.println(mae_raw, 6);
  Serial.print("RMSE_raw="); Serial.println(rmse_raw, 6);
  Serial.print("SNR_raw_dB="); Serial.println(snr_db(ps_raw, pn_raw), 3);

  Serial.print("MAE_AA="); Serial.println(mae_aa, 6);
  Serial.print("RMSE_AA="); Serial.println(rmse_aa, 6);

  Serial.print("SNR_AA_dB="); Serial.println(snr_db(ps_aa, pn_aa), 3);

  Serial.println("#DONE");
}

void loop() {
  delay(1000);
}