// ─────────────────────────────────────────────────────────────────────
// FM Test Sketch v6 — improved task-based sampling, no timer ISR
//
// Built on v4's proven approach (busy-wait task on core 0), with
// three substantive improvements:
//
//   1. Bigger chunks (256 samples = 8 ms) so i2s_write blocks much
//      less frequently — drastically reduces catch-up jitter.
//   2. DC offset auto-tracking — a slow IIR filter learns the ADC's
//      real baseline instead of assuming 2048, removing low-freq drift
//      that sounds like rumble or noise.
//   3. More aggressive scaling (* 32 with hard clip), so audio fills
//      more of the int16 range and the signal-to-noise ratio improves.
//
// Sample rate dropped to 32 kHz — still covers all of FM's audio
// bandwidth (~15 kHz) and gives more CPU margin per sample.
//
// Starts on WBUR 90.9 (strongest station in Cambridge) at 75% volume
// so you can verify quickly without fiddling.
//
// Commands:
//   t<freq>  tune
//   u / d    ±0.1 MHz
//   s        signal status
//   m        mute toggle
//   + / -    volume up / down
// ─────────────────────────────────────────────────────────────────────

#include <Wire.h>
#include "driver/i2s.h"
#include "driver/adc.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 = f;     // WBUR — strongest in Cambridge
volatile int volumeQ7 = 96;       // 75% — louder default than v4
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 — port 0 (where the amp is happy)
// ─────────────────────────────────────────────────────────────────────

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 — bigger chunks, DC tracker, harder gain
//
// The DC tracker is a Q8 fixed-point IIR filter with a time constant
// of 4096 samples (~128 ms at 32 kHz). It only removes drift below
// ~1 Hz, so audio content is preserved.
// ─────────────────────────────────────────────────────────────────────

void fmAudioTask(void* param) {
  const int chunkSamples = 256;
  static int16_t stereoBuf[256 * 2];      // 1 KB — static to stay off the stack

  const unsigned long intervalUs = 1000000 / SAMPLE_RATE;   // ~31 µs
  unsigned long nextSampleTime = micros();

  int32_t dcAvgQ8 = 2048 << 8;            // initial guess: mid-rail

  while (true) {
    for (int i = 0; i < chunkSamples; i++) {
      // Busy-wait until time for next sample
      while ((long)(micros() - nextSampleTime) < 0) { /* spin */ }
      nextSampleTime += intervalUs;

      int raw = adc1_get_raw(FM_ADC_CHANNEL);

      // DC tracker:  dcAvg -= dcAvg/4096;  dcAvg += sample/4096
      dcAvgQ8 -= dcAvgQ8 >> 12;
      dcAvgQ8 += raw >> 4;
      int dc = dcAvgQ8 >> 8;

      int16_t sample = 0;
      if (!fmMuted) {
        int32_t centred = (int32_t)raw - dc;
        int32_t scaled = centred * 32;             // 12-bit → near full int16
        scaled = (scaled * volumeQ7) >> 7;         // volume in Q7
        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);

    // Resync if we've drifted far (only happens on startup / after delays)
    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 v6 — task, no timer ISR");
  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 ✓");

  // Quick signal check (300 ms)
  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("%");
  }
}
