#pragma once
#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>

// Tiny, efficient RTTTL player for ESP32 LEDC (non-blocking, priority-aware)
class RtttlPlayer {
public:
  // pin: GPIO to output tone, channel: LEDC channel [0..15]
  // timer: LEDC timer index [0..3], resolutionBits typically 10-13
  RtttlPlayer(uint8_t pin, uint8_t channel, uint8_t timer=0, uint8_t resolutionBits=10,
              uint16_t queueDepth=4)
  : _pin(pin), _ch(channel), _timer(timer), _resBits(resolutionBits) {

    // LEDC init
    ledcSetup(_ch, /*freq*/ 1000, _resBits);
    ledcAttachPin(_pin, _ch);
    ledcWrite(_ch, 0); // duty 0 initially

    // Sync primitives
    _queue = xQueueCreate(queueDepth, sizeof(PlayItem));
    _mtx   = xSemaphoreCreateMutex();
    _stopFlag = false;
    _playing  = false;
    _curPrio  = 0;
    _preempt.item = nullptr; // Initialize preempt slot

    // Playback task
    xTaskCreatePinnedToCore(_taskThunk, "rtttl_task", 2048, this, /*prio*/ 1, &_taskHandle, 1);
  }

  ~RtttlPlayer() {
    if (_taskHandle) vTaskDelete(_taskHandle);
    if (_queue) vQueueDelete(_queue);
    if (_mtx) vSemaphoreDelete(_mtx);
  }

  // Non-blocking request to play a tune at 'priority'.
  // Copies the RTTTL string internally (heap); returns false if queue full or alloc fails.
  bool play(const char* rtttl, uint8_t priority) {
    if (!rtttl) return false;

    const size_t len = strnlen(rtttl, 512); // Reduce memory cap for efficiency
    char* buf = (char*)malloc(len + 1);
    if (!buf) return false;
    memcpy(buf, rtttl, len);
    buf[len] = '\0';

    PlayItem item{buf, priority};

    // Fast path: preempt if strictly higher priority than current
    xSemaphoreTake(_mtx, portMAX_DELAY);
    const bool shouldPreempt = _playing && (priority > _curPrio);
    if (shouldPreempt) {
      // Put new item into the front by sending to a small high-prio queue slot
      // Approach: set preempt slot; playback loop will pick it ASAP.
      if (_preempt.item) {
        // Drop older preempt request to avoid leaks, keep newest
        free((void*)_preempt.item->tune);
        delete _preempt.item;
      }
      _preempt.item = new PlayItem(item); // copy
      xSemaphoreGive(_mtx);
      // we own original 'buf' no longer (copied into new PlayItem); free the temporary
      free(buf);
      // Signal stop to current note so task can switch between notes (cheap, cooperative)
      _stopFlag = true;
      return true;
    }
    xSemaphoreGive(_mtx);

    // Otherwise enqueue and return (will play after current/earlier)
    if (xQueueSend(_queue, &item, 0) == pdTRUE) {
      return true;
    } else {
      free(buf);
      return false;
    }
  }

  // Stop playback immediately and flush queue (non-blocking).
  void stopAll() {
    xSemaphoreTake(_mtx, portMAX_DELAY);
    _stopFlag = true;
    // Drain queue
    PlayItem tmp;
    while (xQueueReceive(_queue, &tmp, 0) == pdTRUE) {
      if (tmp.tune) free((void*)tmp.tune);
    }
    // Drop any pending preempt
    if (_preempt.item) {
      free((void*)_preempt.item->tune);
      delete _preempt.item;
      _preempt.item = nullptr;
    }
    xSemaphoreGive(_mtx);
  }

private:
  struct PlayItem {
    const char* tune; // heap-allocated copy
    uint8_t priority;
  };
  struct PreemptSlot {
    PlayItem* item = nullptr; // one-slot "front of line"
  };

  // ===== LEDC helpers =====
  inline void toneOn(uint32_t freq) {
    if (freq == 0) { // pause
      ledcWrite(_ch, 0);
      return;
    }
    // Update LEDC frequency efficiently
    // Use ledcSetup to set frequency (works on all Arduino-ESP32 versions)
    ledcSetup(_ch, freq, _resBits);
    // Duty: ~50% for square-like tone
    const uint32_t dutyMax = (1U << _resBits) - 1U;
    ledcWrite(_ch, dutyMax / 2);
  }
  inline void toneOff() {
    ledcWrite(_ch, 0);
  }

  // ===== Playback task =====
  static void _taskThunk(void* arg) {
    ((RtttlPlayer*)arg)->_taskLoop();
  }

  void _taskLoop() {
    for (;;) {
      // First check preempt slot, then queue
      PlayItem item{};
      if (_takePreempt(item) || xQueueReceive(_queue, &item, portMAX_DELAY) == pdTRUE) {
        _playing = true;
        _curPrio = item.priority;
        _stopFlag = false;
        _playOne(item.tune);
        // cleanup
        free((void*)item.tune);
        _playing = false;
        _curPrio = 0;
      }
    }
  }

  bool _takePreempt(PlayItem& out) {
    bool got = false;
    xSemaphoreTake(_mtx, portMAX_DELAY);
    if (_preempt.item) {
      out = *_preempt.item;
      delete _preempt.item;
      _preempt.item = nullptr;
      got = true;
    }
    xSemaphoreGive(_mtx);
    return got;
  }

  // ===== RTTTL parsing & playback =====
  struct Defaults { uint16_t dur = 4; uint8_t oct = 5; uint16_t bpm = 63; };

  // Note frequencies for octave 4 (rounded). Others are scaled by powers of two.
  // Index by semitone: C, C#, D, D#, E, F, F#, G, G#, A, A#, B
  static constexpr uint16_t baseA4 = 440;
  // We’ll derive semitone freq using integer math: f = 440 * 2^((n)/12)
  // To avoid floating point in the loop, we precompute a small LUT for octave 4.
  static constexpr uint16_t LUT4[12] = {
    262, 277, 294, 311, 330, 349, 370, 392, 415, 440, 466, 494
  };

  static uint32_t freqFor(uint8_t noteIndex, int8_t octave) {
    // noteIndex 0..11; octave typical 3..7
    // Get octave 4 freq and scale by 2^(oct-4)
    uint32_t f = LUT4[noteIndex];
    if (octave > 4)      f <<= (octave - 4);
    else if (octave < 4) f >>= (4 - octave);
    return f;
  }

  // Returns next token start or nullptr when finished
  static const char* skipSpaces(const char* p) {
    while (p && *p && (*p == ' ' || *p == ',')) ++p;
    return p;
  }

  void _playOne(const char* rtttl) {
    if (!rtttl) return;
    Defaults def;
    // Format: name:d=4,o=5,b=120: note[,note...]
    const char* p = strchr(rtttl, ':');
    if (!p) return;
    const char* p2 = strchr(p + 1, ':');
    if (!p2) return;

    // Parse defaults between first and second ':'
    parseDefaults(p + 1, p2, def);

    // Whole note duration in ms
    uint32_t wholenote = (60000UL / def.bpm) * 4;

    // Notes section
    p = p2 + 1;
    p = skipSpaces(p);
    while (p && *p) {
      // Cooperative preemption between notes
      if (_checkPreempt()) break;

      uint16_t duration = 0;
      // 1) Optional duration number (e.g., 16)
      while (*p >= '0' && *p <= '9') {
        duration = duration * 10 + (*p - '0');
        ++p;
      }
      if (duration == 0) duration = def.dur;

      // 2) Note letter or pause
      bool pause = false;
      uint8_t noteIndex = 0xFF;
      switch (tolower(*p)) {
        case 'c': noteIndex = 0; break;
        case 'd': noteIndex = 2; break;
        case 'e': noteIndex = 4; break;
        case 'f': noteIndex = 5; break;
        case 'g': noteIndex = 7; break;
        case 'a': noteIndex = 9; break;
        case 'b': noteIndex = 11; break;
        case 'p': pause = true; break;
        default: break;
      }
      if (*p) ++p;

      // 3) Optional sharp '#'
      if (!pause && *p == '#') {
        if (noteIndex != 0xFF) noteIndex++;
        ++p;
      }

      // 4) Optional dotted note '.'
      bool dotted = false;
      if (*p == '.') { dotted = true; ++p; }

      // 5) Optional octave number
      int8_t octave = def.oct;
      if (*p >= '4' && *p <= '7') { octave = (*p - '0'); ++p; }

      // Compute duration in ms
      uint32_t noteDur = wholenote / duration;
      if (dotted) noteDur += noteDur / 2;

      // Play the note
      if (pause || noteIndex == 0xFF) {
        toneOff();
        vTaskDelay(pdMS_TO_TICKS(noteDur));
      } else {
        const uint32_t f = freqFor(noteIndex % 12, octave);
        toneOn(f);
        // Shorten a little to add a tiny gap (staccato for clarity & queue responsiveness)
        uint32_t onMs  = (noteDur >= 4) ? (noteDur - 2) : noteDur;
        uint32_t offMs = noteDur - onMs;
        vTaskDelay(pdMS_TO_TICKS(onMs));
        toneOff();
        if (offMs) vTaskDelay(pdMS_TO_TICKS(offMs));
      }

      p = skipSpaces(p);
      // Optional trailing comma already handled by skipSpaces
    }

    toneOff();
  }

  void parseDefaults(const char* beg, const char* end, Defaults& def) {
    const char* p = beg;
    while (p < end && *p) {
      // key=value (d,o,b)
      char key = tolower(*p);
      const char* eq = (const char*)memchr(p, '=', end - p);
      if (!eq) break;
      const char* val = eq + 1;
      const char* nxt = (const char*)memchr(val, ',', end - val);
      if (!nxt) nxt = end;

      uint32_t v = 0;
      for (const char* t = val; t < nxt; ++t) {
        if (*t >= '0' && *t <= '9') v = v * 10 + (*t - '0');
      }
      if (key == 'd' && v) def.dur = v;
      else if (key == 'o' && v) def.oct = (uint8_t)v;
      else if (key == 'b' && v) def.bpm = v;

      p = (nxt < end) ? (nxt + 1) : end;
    }
    // Clamp sane ranges
    if (def.dur == 0) def.dur = 4;
    if (def.oct < 3 || def.oct > 7) def.oct = 5;
    if (def.bpm < 20) def.bpm = 20;
    if (def.bpm > 400) def.bpm = 400;
  }

  bool _checkPreempt() {
    if (!_stopFlag) return false;
    // Grab preempt immediately if present, otherwise just stop current tune
    PlayItem nxt{};
    if (_takePreempt(nxt)) {
      // Play preempted tune immediately (no recursion - direct call)
      _curPrio = nxt.priority;
      _stopFlag = false;
      _playOne(nxt.tune);
      free((void*)nxt.tune);
    }
    // Indicate current tune should finish early
    return true;
  }

  // ===== members =====
  uint8_t _pin, _ch, _timer, _resBits;
  TaskHandle_t _taskHandle = nullptr;
  QueueHandle_t _queue = nullptr;
  SemaphoreHandle_t _mtx = nullptr;
  volatile bool _stopFlag;
  volatile bool _playing;
  volatile uint8_t _curPrio;
  PreemptSlot _preempt;
};