#include "Receiver.h"
#include "Log.h"
#include <math.h>
#if !OPTICAL_USE_MCPWM_CAPTURE
#include <esp_cpu.h>
#include <esp32-hal-cpu.h>
#endif

uint32_t PulseReceiver::tickHz() const {
#if OPTICAL_USE_MCPWM_CAPTURE
  return captureResolutionHz_;
#else
  return cpuTickHz_;
#endif
}

uint32_t PulseReceiver::plannedTickHz(uint32_t expectedHz, float expectedDutyPct) const {
  if (!expectedHz || expectedDutyPct <= 0.0f || expectedDutyPct >= 100.0f) return 0;
#if OPTICAL_USE_MCPWM_CAPTURE
  return captureResolutionHz_ ? captureResolutionHz_ :
    MCPWM_CAPTURE_RESOLUTION_HZ;
#else
  return cpuTickHz_;
#endif
}

bool PulseReceiver::begin() {
  queue_ = xQueueCreate(512, sizeof(Edge));
  if (!queue_) return false;
#if OPTICAL_USE_MCPWM_CAPTURE
  // PWM generation uses MCPWM group 0. Group 1 is dedicated to input capture,
  // so RX cannot exhaust or conflict with the generator's resources.
  mcpwm_capture_timer_config_t timerConfig = {};
  timerConfig.group_id = 1;
  timerConfig.clk_src = MCPWM_CAPTURE_CLK_SRC_DEFAULT;
  timerConfig.resolution_hz = MCPWM_CAPTURE_RESOLUTION_HZ;
  if (mcpwm_new_capture_timer(&timerConfig, &captureTimer_) != ESP_OK) return false;
  if (mcpwm_capture_timer_get_resolution(captureTimer_, &captureResolutionHz_) != ESP_OK ||
      !captureResolutionHz_) return false;

  mcpwm_capture_channel_config_t channelConfig = {};
  channelConfig.gpio_num = GPIO_RX;
  channelConfig.prescale = 1;
  channelConfig.flags.pos_edge = true;
  channelConfig.flags.neg_edge = false;
  if (mcpwm_new_capture_channel(captureTimer_, &channelConfig, &risingChannel_) != ESP_OK)
    return false;
  mcpwm_capture_event_callbacks_t callbacks = {};
  callbacks.on_cap = onCapture;
  if (mcpwm_capture_channel_register_event_callbacks(
      risingChannel_, &callbacks, this) != ESP_OK) return false;

  channelConfig.flags.pos_edge = false;
  channelConfig.flags.neg_edge = true;
  if (mcpwm_new_capture_channel(captureTimer_, &channelConfig, &fallingChannel_) != ESP_OK)
    return false;
  return mcpwm_capture_channel_register_event_callbacks(
    fallingChannel_, &callbacks, this) == ESP_OK;
#else
  pinMode(GPIO_RX, INPUT);
  cpuTickHz_ = getCpuFrequencyMhz() * 1000000UL;
  attachInterruptArg(GPIO_RX, onGpio, this, CHANGE);
  return cpuTickHz_ != 0;
#endif
}

bool PulseReceiver::start(uint32_t expectedHz, float expectedDutyPct) {
  if (!plannedTickHz(expectedHz, expectedDutyPct)) return false;
  expectedHz_ = expectedHz;
  expectedDutyPct_ = expectedDutyPct;
#if !OPTICAL_USE_MCPWM_CAPTURE
  cpuTickHz_ = getCpuFrequencyMhz() * 1000000UL;
  if (!cpuTickHz_) return false;
#endif
  resetStream();
#if OPTICAL_USE_MCPWM_CAPTURE
  // Progress updates keep one capture session alive. Pulse-width stages stop
  // capture only after PWM is quiet, so resetStream never races the ISR.
  if (running_) return true;
  if (mcpwm_capture_timer_enable(captureTimer_) != ESP_OK) return false;
  if (mcpwm_capture_channel_enable(risingChannel_) != ESP_OK) {
    mcpwm_capture_timer_disable(captureTimer_);
    return false;
  }
  if (mcpwm_capture_channel_enable(fallingChannel_) != ESP_OK) {
    mcpwm_capture_channel_disable(risingChannel_);
    mcpwm_capture_timer_disable(captureTimer_);
    return false;
  }
  running_ = true;
  if (mcpwm_capture_timer_start(captureTimer_) != ESP_OK) {
    running_ = false;
    mcpwm_capture_channel_disable(fallingChannel_);
    mcpwm_capture_channel_disable(risingChannel_);
    mcpwm_capture_timer_disable(captureTimer_);
    return false;
  }
#else
  running_ = true;
#endif
  return true;
}

void PulseReceiver::stop() {
  const bool wasRunning = running_;
  running_ = false;
#if OPTICAL_USE_MCPWM_CAPTURE
  if (wasRunning) {
    // Mask both edge interrupts before stopping the shared capture timer.
    mcpwm_capture_channel_disable(fallingChannel_);
    mcpwm_capture_channel_disable(risingChannel_);
    mcpwm_capture_timer_stop(captureTimer_);
    mcpwm_capture_timer_disable(captureTimer_);
  }
#else
  (void)wasRunning;
#endif
}

void PulseReceiver::resetStream() {
  if (queue_) xQueueReset(queue_);
  haveReorderEdge_ = false;
  droppedItems_ = 0;
  polarityKnown_ = false;
  activeStartRising_ = false;
  syncEdgeCount_ = 0;
  waitingForActiveEnd_ = true;
  activeStart_ = activeEnd_ = 0;
  haveRawTick_ = false;
  lastRawTick_ = 0;
  tickEpoch_ = 0;
}

PulseReceiver::TimedEdge PulseReceiver::extendEdge(const Edge &e) {
  if (haveRawTick_ && e.tick < lastRawTick_ && lastRawTick_ - e.tick > 0x80000000UL)
    tickEpoch_ += 0x100000000ULL;
  haveRawTick_ = true; lastRawTick_ = e.tick;
  return {tickEpoch_ + e.tick, e.rising != 0};
}

bool PulseReceiver::consumeEdge(const Edge &rawEdge, PulsePeriod &out) {
  const TimedEdge edge = extendEdge(rawEdge);
  if (polarityKnown_) {
    // Deliberately ignore edge type after synchronization. A PWM waveform is
    // just alternating intervals: active, inactive, active, inactive. An
    // extra or missing edge therefore becomes a concrete wrong pulse/period
    // instead of an ambiguous GLITCH state.
    if (waitingForActiveEnd_) {
      activeEnd_ = edge.tick;
      waitingForActiveEnd_ = false;
      return false;
    }
    const uint64_t periodTicks = edge.tick - activeStart_;
    const uint64_t activeTicks = activeEnd_ - activeStart_;
    out = {activeStart_, static_cast<uint32_t>(periodTicks),
      static_cast<uint32_t>(activeTicks), tickHz()};
    activeStart_ = edge.tick;
    waitingForActiveEnd_ = true;
    return true;
  }

  syncEdges_[syncEdgeCount_++] = edge;
  if (syncEdgeCount_ < 3U) return false;

  const uint64_t firstTicks = syncEdges_[1].tick - syncEdges_[0].tick;
  const uint64_t secondTicks = syncEdges_[2].tick - syncEdges_[1].tick;
  const double expectedTicks = static_cast<double>(tickHz()) * expectedDutyPct_ /
    (100.0 * expectedHz_);
  const double firstError = fabs(static_cast<double>(firstTicks) - expectedTicks);
  const double secondError = fabs(static_cast<double>(secondTicks) - expectedTicks);
  activeStartRising_ = firstError <= secondError ? syncEdges_[0].rising : syncEdges_[1].rising;
  polarityKnown_ = true;
  Log::printf("CAPTURE", "RX polarity auto: active starts on %s, first=%lluns second=%lluns",
    activeStartRising_ ? "RISING" : "FALLING",
    static_cast<unsigned long long>(firstTicks * 1000000000ULL / tickHz()),
    static_cast<unsigned long long>(secondTicks * 1000000000ULL / tickHz()));

  bool produced = false;
  if (firstError <= secondError) {
    activeStart_ = syncEdges_[0].tick;
    activeEnd_ = syncEdges_[1].tick;
    const uint64_t periodTicks = syncEdges_[2].tick - activeStart_;
    out = {activeStart_, static_cast<uint32_t>(periodTicks),
      static_cast<uint32_t>(activeEnd_ - activeStart_), tickHz()};
    activeStart_ = syncEdges_[2].tick;
    waitingForActiveEnd_ = true;
    produced = true;
  } else {
    activeStart_ = syncEdges_[1].tick;
    activeEnd_ = syncEdges_[2].tick;
    waitingForActiveEnd_ = false;
  }
  syncEdgeCount_ = 0;
  return produced;
}

uint32_t PulseReceiver::takeDroppedItems() {
  return __atomic_exchange_n(&droppedItems_, 0, __ATOMIC_RELAXED);
}

#if OPTICAL_USE_MCPWM_CAPTURE
bool IRAM_ATTR PulseReceiver::onCapture(mcpwm_cap_channel_handle_t,
                                        const mcpwm_capture_event_data_t *data,
                                        void *ctx) {
  PulseReceiver *self = static_cast<PulseReceiver *>(ctx);
  if (!self->running_) return false;
  const bool rawRising = data->cap_edge == MCPWM_CAP_EDGE_POS;
  const Edge edge = {data->cap_value, static_cast<uint8_t>(rawRising)};
  BaseType_t wake = pdFALSE;
  if (xQueueSendFromISR(self->queue_, &edge, &wake) != pdTRUE)
    __atomic_fetch_add(&self->droppedItems_, 1U, __ATOMIC_RELAXED);
  return wake == pdTRUE;
}
#else
void IRAM_ATTR PulseReceiver::onGpio(void *ctx) {
  PulseReceiver *self = static_cast<PulseReceiver *>(ctx);
  if (!self->running_) return;
  bool level = gpio_get_level(static_cast<gpio_num_t>(GPIO_RX));
  if (RX_ACTIVE_LEVEL == LOW) level = !level;
  const Edge edge = {esp_cpu_get_cycle_count(), static_cast<uint8_t>(level)};
  BaseType_t wake = pdFALSE;
  if (xQueueSendFromISR(self->queue_, &edge, &wake) != pdTRUE)
    __atomic_fetch_add(&self->droppedItems_, 1U, __ATOMIC_RELAXED);
  if (wake) portYIELD_FROM_ISR();
}
#endif

bool PulseReceiver::nextOrderedEdge(Edge &edge, TickType_t waitTicks) {
  if (!haveReorderEdge_) {
    if (xQueueReceive(queue_, &reorderEdge_, waitTicks) != pdTRUE) return false;
    haveReorderEdge_ = true;
  }
  Edge next = {};
  // Keep one-event look-ahead. If both channel interrupts were pending while
  // OLED/I2C ran, the MCPWM driver may dispatch them by channel number rather
  // than timestamp. The signed modular comparison restores their real order.
  if (xQueueReceive(queue_, &next, waitTicks) != pdTRUE) return false;
  if (static_cast<int32_t>(next.tick - reorderEdge_.tick) < 0) {
    edge = next;
  } else {
    edge = reorderEdge_;
    reorderEdge_ = next;
  }
  return true;
}

size_t PulseReceiver::readPeriods(PulsePeriod *periods, size_t capacity,
                                  TickType_t waitTicks) {
  size_t count = 0;
  Edge edge = {};
  while (count < capacity && nextOrderedEdge(edge, count ? 0 : waitTicks)) {
    if (consumeEdge(edge, periods[count])) {
      periods[count].activeTickHz = tickHz();
      ++count;
    }
  }
  return count;
}