// ─────────────────────────────────────────────────────────────────────
// FM Test Sketch v5 — hardware timer + ring buffer
//
// Sample timing comes from a hardware timer interrupt (precise, no
// jitter). The ISR reads the ADC and pushes one sample into a ring
// buffer. A FreeRTOS task drains the ring into I2S in chunks.
//
// This eliminates the audio-rate jitter that the busy-wait approach
// can introduce when I2S backpressure stalls the sampling loop.
//
// Works on both Arduino-ESP32 2.x and 3.x — version detected at
// compile time.
//
// Commands once running:
//   t<freq>  tune to a frequency
//   u / d    ±0.1 MHz
//   s        print status
//   m        mute toggle
//   + / -    volume up / down
// ─────────────────────────────────────────────────────────────────────

#include <Wire.h>
#include "driver/i2s.h"
#include "driver/adc.h"
#include "esp_arduino_version.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     32000            // try 32 kHz first — easier on the timer

#define RING_SIZE       2048             // must be power of 2 for fast wrap

float fmFrequencyMHz = 101.7f;
volatile int  volumeQ7 = 64;
volatile bool fmMuted = false;

// ── Ring buffer (mono int16 samples) ─────────────────────────────────

volatile int16_t  ring[RING_SIZE];
volatile uint32_t ringWrite = 0;   // ISR advances
volatile uint32_t ringRead  = 0;   // task advances
volatile uint32_t ringOverruns = 0;

hw_timer_t* sampleTimer = NULL;

// ─────────────────────────────────────────────────────────────────────
// Timer ISR — runs at exactly SAMPLE_RATE Hz
// ─────────────────────────────────────────────────────────────────────

void IRAM_ATTR onSampleTimer() {
  int raw = adc1_get_raw(FM_ADC_CHANNEL);

  // Ring buffer full?  Drop the sample and bump overrun counter.
  uint32_t next = (ringWrite + 1) & (RING_SIZE - 1);
  if (next == ringRead) {
    ringOverruns++;
    return;
  }

  int16_t sample = 0;
  if (!fmMuted) {
    int32_t centred = (int32_t)raw - 2048;
    int32_t scaled = centred * 16;          // 12-bit → 16-bit
    scaled = (scaled * volumeQ7) >> 7;
    if (scaled >  32767) scaled =  32767;
    if (scaled < -32768) scaled = -32768;
    sample = (int16_t)scaled;
  }
  ring[ringWrite] = sample;
  ringWrite = next;
}

bool setupSampleTimer() {
#if ESP_ARDUINO_VERSION_MAJOR >= 3
  // New API (Arduino-ESP32 3.x)
  sampleTimer = timerBegin(1000000);   // 1 MHz base frequency
  if (!sampleTimer) return false;
  timerAttachInterrupt(sampleTimer, &onSampleTimer);
  timerAlarm(sampleTimer, 1000000 / SAMPLE_RATE, true, 0);
#else
  // Old API (Arduino-ESP32 2.x)
  sampleTimer = timerBegin(0, 80, true);   // timer 0, prescaler 80 → 1 µs tick
  if (!sampleTimer) return false;
  timerAttachInterrupt(sampleTimer, &onSampleTimer, true);
  timerAlarmWrite(sampleTimer, 1000000 / SAMPLE_RATE, true);
  timerAlarmEnable(sampleTimer);
#endif
  return true;
}

// ─────────────────────────────────────────────────────────────────────
// Audio flushing task — drains ring buffer into I2S
// ─────────────────────────────────────────────────────────────────────

TaskHandle_t flushTaskHandle = NULL;

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

  while (true) {
    int collected = 0;
    while (collected < chunkSamples) {
      // How many samples are available?
      uint32_t w = ringWrite;
      uint32_t r = ringRead;
      uint32_t available = (w - r) & (RING_SIZE - 1);
      if (available == 0) {
        // Briefly yield while waiting for more samples
        vTaskDelay(1);
        continue;
      }
      int n = (int)available;
      if (n > chunkSamples - collected) n = chunkSamples - collected;
      for (int i = 0; i < n; i++) {
        int16_t s = ring[ringRead];
        ringRead = (ringRead + 1) & (RING_SIZE - 1);
        stereoBuf[(collected + i)*2]     = s;
        stereoBuf[(collected + i)*2 + 1] = s;
      }
      collected += n;
    }

    size_t written;
    i2s_write(I2S_OUT_PORT, stereoBuf, sizeof(stereoBuf), &written, portMAX_DELAY);
  }
}

// ─────────────────────────────────────────────────────────────────────
// 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;       // match the ADC 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;
}

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

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

  Serial.println();
  Serial.println("══════════════════════════════════════════");
  Serial.println("  FM v5 — hw timer ISR + ring buffer");
  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 ✓");

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

  xTaskCreatePinnedToCore(
    fmFlushTask, "fmFlush", 4096, NULL, 5, &flushTaskHandle, 1
  );
  Serial.println("[4] Flush task running on core 1 ✓");

  if (!setupSampleTimer()) {
    Serial.println("[5] ✗ Sample timer failed to start");
    return;
  }
  Serial.println("[5] Sample timer firing at precise rate ✓");

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

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("%");
  } else if (cmd == 'o') {
    Serial.print("Ring overruns since boot: "); Serial.println(ringOverruns);
  }
}
