// ─────────────────────────────────────────────────────────────────────
// FM Test Sketch v4 — analogRead approach
//
// Direct ADC sampling via adc1_get_raw() at ~44 kHz from a FreeRTOS
// task pinned to core 0. Output goes to I2S port 0 (the one we know
// works on this board). FM tuner is the TEA5767 on I2C pins 21/22.
//
// The task busy-waits to enforce sample timing; I2S DMA paces us to
// exactly 44.1 kHz via backpressure.
//
// Commands once running:
//   t<freq>  tune to a frequency (e.g. t101.7)
//   u / d    step up / down 0.1 MHz
//   s        print status (signal, stereo, frequency)
//   m        toggle mute
//   + / -    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   // GPIO 35
#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     44100

float fmFrequencyMHz = 101.7f;
volatile int volumeQ7 = 64;     // 0–128, 64 = 50%
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 on port 0 (confirmed working earlier)
// ─────────────────────────────────────────────────────────────────────

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 — pinned to core 0
//
// Samples the ADC at ~22 µs spacing (slightly faster than 44.1 kHz so
// I2S backpressure paces us to exactly 44.1 kHz). Reads 32 samples per
// chunk, expands mono → stereo, applies volume, writes to I2S.
// ─────────────────────────────────────────────────────────────────────

void fmAudioTask(void* param) {
  const int chunkSamples = 32;
  int16_t stereoBuf[chunkSamples * 2];

  unsigned long nextSampleTime = micros();
  const unsigned long interval = 22;   // µs per sample

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

      int raw = adc1_get_raw(FM_ADC_CHANNEL);
      int16_t sample = 0;
      if (!fmMuted) {
        int32_t centred = (int32_t)raw - 2048;
        int32_t scaled = centred * 16;            // 12-bit → 16-bit range
        scaled = (scaled * volumeQ7) >> 7;        // apply volume (Q7)
        if (scaled >  32767) scaled =  32767;
        if (scaled < -32768) scaled = -32768;
        sample = (int16_t)scaled;
      }
      stereoBuf[i*2]     = sample;
      stereoBuf[i*2 + 1] = sample;
    }

    // Write chunk to I2S. Blocks if DMA is full, which is what gives
    // us precise pacing at the I2S clock rate (44.1 kHz).
    size_t written;
    i2s_write(I2S_OUT_PORT, stereoBuf, sizeof(stereoBuf), &written, portMAX_DELAY);

    // If we've drifted far behind real time (e.g. on startup), resync.
    long drift = (long)micros() - (long)nextSampleTime;
    if (drift > 10000 || drift < -10000) {
      nextSampleTime = micros();
    }
  }
}

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

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

  Serial.println();
  Serial.println("══════════════════════════════════════════");
  Serial.println("  FM via analogRead — port 0 output");
  Serial.println("══════════════════════════════════════════");

  // ── I2C + TEA5767 tune ─────────────────────────────────────────────
  Wire.begin(FM_SDA, FM_SCL);
  Wire.setClock(100000);
  delay(50);

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

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

  // ── Diagnostic: 500 ms of direct ADC samples ───────────────────────
  // This is independent of any I2S magic — it just reads the pin.
  Serial.println();
  Serial.println("[3] Sampling GPIO 35 for 500 ms to verify the audio wire…");
  int minV = 4095, maxV = 0;
  long sumV = 0;
  int count = 0;
  unsigned long t0 = millis();
  while (millis() - t0 < 500) {
    int v = adc1_get_raw(FM_ADC_CHANNEL);
    if (v < minV) minV = v;
    if (v > maxV) maxV = v;
    sumV += v;
    count++;
  }
  int avgV = (int)(sumV / count);
  int swing = maxV - minV;
  Serial.print("    "); Serial.print(count); Serial.print(" samples, ");
  Serial.print("min="); Serial.print(minV);
  Serial.print("  max="); Serial.print(maxV);
  Serial.print("  avg="); Serial.print(avgV);
  Serial.print("  swing="); Serial.println(swing);

  if (swing < 50) {
    Serial.println("    ✗ DEAD signal at GPIO 35.");
    Serial.println("    The audio wire isn't carrying anything, or the chip's");
    Serial.println("    audio output stage is broken.");
    Serial.println("    Try: swap red/black audio wires; try a different audio");
    Serial.println("    output pin on the module if there is one.");
    Serial.println("    Continuing anyway in case you fix it live…");
  } else if (swing < 200) {
    Serial.println("    ~ Weak signal. Could be antenna position or weak station.");
  } else {
    Serial.println("    ✓ Healthy signal — audio is present at GPIO 35");
  }

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

  // ── Audio task on core 0 ───────────────────────────────────────────
  xTaskCreatePinnedToCore(
    fmAudioTask,
    "fmAudio",
    4096,          // stack
    NULL,
    5,             // priority — higher than default loop
    &fmAudioTaskHandle,
    0              // core 0 (loop runs on core 1)
  );
  Serial.println("[5] Audio task started on core 0 ✓");

  Serial.println();
  Serial.println("══════════════════════════════════════════");
  Serial.println("  Audio should be playing now");
  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 {
      Serial.println("Bad frequency");
    }
  } 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("%");
  }
}
