// ─────────────────────────────────────────────────────────────────────
// FM Test Sketch v9 — cascaded one-pole IIR low-pass at 5 kHz
//
// Replaces v8's brick-wall 4-tap moving average with two cascaded
// one-pole IIR filters. The cutoff is at 5 kHz with 12 dB/octave
// rolloff above, so:
//
//   - Voice fundamentals & formants (200 Hz – 4 kHz): essentially
//     untouched, voice sounds natural
//   - Hiss & high-frequency ADC noise (5 kHz +): cut hard
//
// Each stage:  y[n] = β·x[n] + (1-β)·y[n-1]
// β = 1 - exp(-2π · 5000 / 32000)  ≈  0.625   (Q16 = 40960)
//
// Tuning: change LPF_BETA. Higher (closer to 65536) = brighter / more
// hiss. Lower = duller / quieter.
// ─────────────────────────────────────────────────────────────────────

#include <Wire.h>
#include "driver/i2s.h"
#include "driver/adc.h"
#include "esp_task_wdt.h"

#define FM_SDA          21
#define FM_SCL          22
#define FM_ADC_CHANNEL  ADC1_CHANNEL_7
#define TEA5767_ADDR    0x60

#define I2S_BCLK        13
#define I2S_LRC         14
#define I2S_DIN         12
#define I2S_OUT_PORT    I2S_NUM_0
#define SAMPLE_RATE     32000

float fmFrequencyMHz = 90.9f;
volatile int volumeQ7 = 96;
volatile bool fmMuted = false;
TaskHandle_t fmAudioTaskHandle = NULL;

// ── TEA5767 ──────────────────────────────────────────────────────────

bool tea5767Write(float freqMHz) {
  unsigned long freqHz = (unsigned long)(freqMHz * 1000000.0f);
  unsigned long pll = (4UL * (freqHz + 225000UL)) / 32768UL;
  uint8_t b[5];
  b[0] = (pll >> 8) & 0x3F;
  b[1] =  pll & 0xFF;
  b[2] = 0x90;
  b[3] = 0x10;
  b[4] = 0x00;
  Wire.beginTransmission(TEA5767_ADDR);
  Wire.write(b, 5);
  return Wire.endTransmission() == 0;
}

bool tea5767ReadStatus(uint8_t* outSignal, bool* outStereo, float* outFreq) {
  uint8_t b[5];
  if (Wire.requestFrom((uint8_t)TEA5767_ADDR, (uint8_t)5) != 5) return false;
  for (int i = 0; i < 5; i++) b[i] = Wire.read();
  *outSignal = (b[3] >> 4) & 0x0F;
  *outStereo = (b[2] & 0x80) != 0;
  unsigned long pll = ((unsigned long)(b[0] & 0x3F) << 8) | b[1];
  unsigned long freqHz = (pll * 32768UL) / 4UL - 225000UL;
  *outFreq = freqHz / 1000000.0f;
  return true;
}

void printStatus() {
  uint8_t signal = 0; bool stereo = false; float freq = 0;
  if (tea5767ReadStatus(&signal, &stereo, &freq)) {
    Serial.print("  "); Serial.print(freq, 2); Serial.print(" MHz, signal ");
    Serial.print(signal); Serial.print("/15, ");
    Serial.println(stereo ? "stereo" : "mono");
  }
}

// ── Output I2S ───────────────────────────────────────────────────────

bool setupOutputI2S() {
  i2s_config_t cfg = {};
  cfg.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
  cfg.sample_rate = SAMPLE_RATE;
  cfg.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
  cfg.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT;
  cfg.communication_format = (i2s_comm_format_t)I2S_COMM_FORMAT_STAND_I2S;
  cfg.intr_alloc_flags = 0;
  cfg.dma_buf_count = 8;
  cfg.dma_buf_len = 256;
  cfg.use_apll = false;
  if (i2s_driver_install(I2S_OUT_PORT, &cfg, 0, NULL) != ESP_OK) return false;
  i2s_pin_config_t pins = {};
  pins.bck_io_num = I2S_BCLK;
  pins.ws_io_num = I2S_LRC;
  pins.data_out_num = I2S_DIN;
  pins.data_in_num = I2S_PIN_NO_CHANGE;
  return i2s_set_pin(I2S_OUT_PORT, &pins) == ESP_OK;
}

// ── Audio task ───────────────────────────────────────────────────────

void fmAudioTask(void* param) {
  esp_task_wdt_delete(NULL);

  const int chunkSamples = 256;
  static int16_t stereoBuf[256 * 2];

  const unsigned long intervalUs = 1000000 / SAMPLE_RATE;
  unsigned long nextSampleTime = micros();

  int32_t dcAvgQ8 = 2048 << 8;

  // Cascaded one-pole IIR low-pass at 5 kHz (Q16 fixed point)
  const int32_t LPF_BETA  = 40960;        // β  ≈ 0.625
  const int32_t LPF_GAMMA = 65536 - 40960; // 1-β
  int32_t lpf1 = 0, lpf2 = 0;

  while (true) {
    for (int i = 0; i < chunkSamples; i++) {
      while ((long)(micros() - nextSampleTime) < 0) { /* spin */ }
      nextSampleTime += intervalUs;

      int raw = adc1_get_raw(FM_ADC_CHANNEL);

      // DC tracker (very slow IIR, ~1 Hz cutoff)
      dcAvgQ8 -= dcAvgQ8 >> 12;
      dcAvgQ8 += raw >> 4;
      int dc = dcAvgQ8 >> 8;

      int32_t centred = (int32_t)raw - dc;

      // Two-stage low-pass cascade
      lpf1 = (LPF_BETA * centred + LPF_GAMMA * lpf1) >> 16;
      lpf2 = (LPF_BETA * lpf1    + LPF_GAMMA * lpf2) >> 16;

      int16_t sample = 0;
      if (!fmMuted) {
        int32_t scaled = lpf2 * 32;
        scaled = (scaled * volumeQ7) >> 7;
        if (scaled >  32767) scaled =  32767;
        if (scaled < -32768) scaled = -32768;
        sample = (int16_t)scaled;
      }
      stereoBuf[i*2]     = sample;
      stereoBuf[i*2 + 1] = sample;
    }

    size_t written;
    i2s_write(I2S_OUT_PORT, stereoBuf,
              sizeof(int16_t) * 2 * chunkSamples,
              &written, portMAX_DELAY);

    // Yield 1 ms so the OS can run housekeeping (keeps the chip happy).
    vTaskDelay(1);

    long drift = (long)micros() - (long)nextSampleTime;
    if (drift > 50000 || drift < -50000) {
      nextSampleTime = micros();
    }
  }
}

// ─────────────────────────────────────────────────────────────────────

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

  Serial.println();
  Serial.println("══════════════════════════════════════════");
  Serial.println("  FM v9 — IIR low-pass at 5 kHz");
  Serial.print  ("  Sample rate: "); Serial.print(SAMPLE_RATE); Serial.println(" Hz");
  Serial.println("══════════════════════════════════════════");

  Wire.begin(FM_SDA, FM_SCL);
  Wire.setClock(100000);
  delay(50);

  if (!tea5767Write(fmFrequencyMHz)) {
    Serial.println("[1] ✗ TEA5767 did not respond");
    return;
  }
  Serial.print("[1] Tuned to "); Serial.print(fmFrequencyMHz, 1); Serial.println(" MHz ✓");
  delay(200);
  printStatus();

  adc1_config_width(ADC_WIDTH_BIT_12);
  adc1_config_channel_atten(FM_ADC_CHANNEL, ADC_ATTEN_DB_11);
  Serial.println("[2] ADC configured ✓");

  int minV = 4095, maxV = 0;
  long sumV = 0;
  int count = 0;
  unsigned long t0 = millis();
  while (millis() - t0 < 300) {
    int v = adc1_get_raw(FM_ADC_CHANNEL);
    if (v < minV) minV = v;
    if (v > maxV) maxV = v;
    sumV += v;
    count++;
  }
  Serial.print("[3] Signal check: min="); Serial.print(minV);
  Serial.print(" max="); Serial.print(maxV);
  Serial.print(" avg="); Serial.print((int)(sumV / count));
  Serial.print(" swing="); Serial.println(maxV - minV);

  if (!setupOutputI2S()) {
    Serial.println("[4] ✗ Output I2S failed");
    return;
  }
  Serial.println("[4] Output I2S ready ✓");

  xTaskCreatePinnedToCore(
    fmAudioTask, "fmAudio", 4096, NULL, 5, &fmAudioTaskHandle, 0
  );
  Serial.println("[5] Audio task on core 0 ✓");

  Serial.println();
  Serial.println("══════════════════════════════════════════");
  Serial.println("  Audio should be playing");
  Serial.println("══════════════════════════════════════════");
  Serial.println("Commands: t<freq>, u, d, s, m, +, -");
}

void loop() {
  if (!Serial.available()) return;
  char cmd = Serial.read();

  if (cmd == 't') {
    String s = Serial.readStringUntil('\n'); s.trim();
    float f = s.toFloat();
    if (f >= 87.5f && f <= 108.0f) {
      fmFrequencyMHz = f;
      tea5767Write(f);
      delay(150);
      Serial.print("Tuned to "); Serial.print(f, 1); Serial.println(" MHz");
      printStatus();
    }
  } else if (cmd == 'u') {
    fmFrequencyMHz = min(108.0f, fmFrequencyMHz + 0.1f);
    tea5767Write(fmFrequencyMHz);
    delay(150);
    Serial.print("Up to "); Serial.print(fmFrequencyMHz, 1); Serial.println(" MHz");
    printStatus();
  } else if (cmd == 'd') {
    fmFrequencyMHz = max(87.5f, fmFrequencyMHz - 0.1f);
    tea5767Write(fmFrequencyMHz);
    delay(150);
    Serial.print("Down to "); Serial.print(fmFrequencyMHz, 1); Serial.println(" MHz");
    printStatus();
  } else if (cmd == 's') {
    printStatus();
  } else if (cmd == 'm') {
    fmMuted = !fmMuted;
    Serial.println(fmMuted ? "Muted" : "Unmuted");
  } else if (cmd == '+') {
    volumeQ7 = min(128, volumeQ7 + 12);
    Serial.print("Volume: "); Serial.print((volumeQ7 * 100) / 128); Serial.println("%");
  } else if (cmd == '-') {
    volumeQ7 = max(0, volumeQ7 - 12);
    Serial.print("Volume: "); Serial.print((volumeQ7 * 100) / 128); Serial.println("%");
  }
}
