#include "Measurement.h"
#include "Config.h"
#include <string.h>

bool Measurement::start(float hz, float duty, float tolerance, uint32_t timeMs,
                        uint16_t averagingPeriods, uint8_t settleCycles) {
  if (!task_ && xTaskCreate(taskEntry, "optical-rx", 4096, this, 4, &task_) != pdPASS) return false;
  expectedHz_ = static_cast<uint32_t>(hz + 0.5f);
  expectedDutyPct_ = duty;
  tolerance = effectiveTolerancePct(tolerance);
  if (!expectedHz_ || !timeMs || !averagingPeriods ||
      !receiver_.start(expectedHz_, expectedDutyPct_)) return false;
  settleCycles_ = settleCycles; settleLeft_ = settleCycles;
  tolerancePct_ = tolerance;
  averagingPeriods_ = averagingPeriods;
  resetAveragingWindow();
  stepTimeMs_ = (timeMs + MEASUREMENT_PROGRESS_STEPS - 1U) / MEASUREMENT_PROGRESS_STEPS;
  stepTicks_ = static_cast<uint64_t>(receiver_.tickHz()) * timeMs /
    (1000ULL * MEASUREMENT_PROGRESS_STEPS);
  if (!stepTicks_) stepTicks_ = 1;
  currentStep_ = 0;
  stats_.reset();
  publishStats();
  measurementStartTick_ = deadlineTick_ = 0; startedMs_ = millis();
  measurementStartMs_ = lastPeriodMs_ = 0;
  expectedPeriodMs_ = static_cast<uint32_t>((1000ULL + expectedHz_ - 1U) / expectedHz_);
  if (!expectedPeriodMs_) expectedPeriodMs_ = 1;
  state_ = MeasureState::SETTLING;
  xTaskNotifyGive(task_);
  return true;
}

void Measurement::resetAveragingWindow() {
  windowPeriodCount_ = 0;
  windowPeriodSum_ = windowActiveSum_ = 0;
  windowMinPeriod_ = UINT32_MAX;
  windowMaxPeriod_ = 0;
}

void Measurement::taskEntry(void *context) {
  static_cast<Measurement *>(context)->taskLoop();
}

void Measurement::taskLoop() {
  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    while (state_ == MeasureState::SETTLING || state_ == MeasureState::RUNNING) processOnce();
  }
}

void Measurement::fail(FailReason reason) {
  if (stats_.reason == FailReason::NONE) stats_.reason = reason;
  publishStats();
  receiver_.stop(); state_ = MeasureState::FAIL;
}

void Measurement::completeMeasurement() {
  receiver_.stop();
  stats_.droppedItems += receiver_.takeDroppedItems();
  if (receiver_.overflowed()) { fail(FailReason::GLITCH); return; }
  if (windowPeriodCount_) {
    const FailReason result = evaluatePeriodWindow(
      windowPeriodSum_, windowActiveSum_, windowPeriodCount_, receiver_.tickHz(),
      expectedHz_, expectedDutyPct_, tolerancePct_,
      windowMinPeriod_, windowMaxPeriod_,
      currentStep_ + 1U, stats_);
    if (result != FailReason::NONE) { fail(result); return; }
  }
  publishStats();
  if (++currentStep_ < MEASUREMENT_PROGRESS_STEPS) {
    state_ = MeasureState::STEP_READY;
    return;
  }
  if (!stats_.periods) {
    fail(FailReason::DATA_LOSS); return;
  }
  state_ = MeasureState::PASS;
}

void Measurement::publishStats() {
  portENTER_CRITICAL(&statsMux_);
  publishedStats_ = stats_;
  portEXIT_CRITICAL(&statsMux_);
}

bool Measurement::statsSnapshot(StageStats &out) const {
  portENTER_CRITICAL(&statsMux_);
  out = publishedStats_;
  portEXIT_CRITICAL(&statsMux_);
  return out.periods && out.periodSum;
}

MeasureState Measurement::processOnce() {
  if (state_ != MeasureState::SETTLING && state_ != MeasureState::RUNNING) return state_;
  if (receiver_.overflowed()) { fail(FailReason::GLITCH); return state_; }
  bool receivedPeriod = false;
  for (;;) {
    const size_t periodCount = receiver_.readPeriods(periodBatch_, PERIOD_BATCH_SIZE, pdMS_TO_TICKS(2));
    stats_.droppedItems += receiver_.takeDroppedItems();
    if (!periodCount) break;
    receivedPeriod = true;
    for (size_t periodIndex = 0; periodIndex < periodCount; ++periodIndex) {
      if (state_ != MeasureState::SETTLING && state_ != MeasureState::RUNNING) return state_;
      const PulsePeriod &period = periodBatch_[periodIndex];
      if (state_ == MeasureState::SETTLING) {
        if (settleLeft_) --settleLeft_;
        if (!settleLeft_) {
          measurementStartTick_ = period.startTick + period.periodTicks;
          deadlineTick_ = measurementStartTick_ + stepTicks_;
          measurementStartMs_ = lastPeriodMs_ = millis(); state_ = MeasureState::RUNNING;
        }
        continue;
      }
      const uint64_t endTick = period.startTick + period.periodTicks;
      if (period.startTick < measurementStartTick_) continue; // leading incomplete period
      if (endTick > deadlineTick_) { completeMeasurement(); return state_; } // trailing incomplete period
      if (!period.periodTicks || period.activeTicks >= period.periodTicks) {
        fail(FailReason::EXTRA_EDGE); return state_;
      }

      ++stats_.periods;
      stats_.periodSum += period.periodTicks;
      stats_.activeSum += period.activeTicks;
      if (period.periodTicks < stats_.minPeriod) stats_.minPeriod = period.periodTicks;
      if (period.periodTicks > stats_.maxPeriod) stats_.maxPeriod = period.periodTicks;
      if (period.activeTicks < stats_.minActive) stats_.minActive = period.activeTicks;
      if (period.activeTicks > stats_.maxActive) stats_.maxActive = period.activeTicks;

      if (period.periodTicks < windowMinPeriod_) windowMinPeriod_ = period.periodTicks;
      if (period.periodTicks > windowMaxPeriod_) windowMaxPeriod_ = period.periodTicks;
      ++windowPeriodCount_;
      windowPeriodSum_ += period.periodTicks;
      windowActiveSum_ += period.activeTicks;
      if (windowPeriodCount_ >= averagingPeriods_) {
        const FailReason result = evaluatePeriodWindow(
          windowPeriodSum_, windowActiveSum_, windowPeriodCount_, receiver_.tickHz(),
          expectedHz_, expectedDutyPct_, tolerancePct_,
          windowMinPeriod_, windowMaxPeriod_,
          currentStep_ + 1U, stats_);
        resetAveragingWindow();
        if (result != FailReason::NONE) { fail(result); return state_; }
      }
    }
  }
  if (receivedPeriod && state_ == MeasureState::RUNNING) lastPeriodMs_ = millis();
  const uint64_t edgeBasedTimeout =
    static_cast<uint64_t>(PWM_SETTLE_CYCLES + NO_SIGNAL_TIMEOUT_PERIODS) * expectedPeriodMs_ + 20;
  const uint64_t rmtBatchTimeout =
    static_cast<uint64_t>(RMT_MIN_RECEIVE_SYMBOLS + NO_SIGNAL_TIMEOUT_PERIODS) * expectedPeriodMs_ + 20;
  const uint64_t settleTimeout = edgeBasedTimeout > rmtBatchTimeout ? edgeBasedTimeout : rmtBatchTimeout;
  if (state_ == MeasureState::SETTLING && millis() - startedMs_ > settleTimeout) fail(FailReason::NO_SIGNAL);
  if (state_ == MeasureState::RUNNING && measurementStartTick_) {
    const uint32_t now = millis();
    // RMT reports a block only after its user buffer has filled. At 1 kHz the
    // minimum 48-symbol C3 block contains roughly 48 PWM periods and therefore
    // arrives much later than the old 8-period timeout. Do not call that
    // normal batching delay a lost edge.
    const uint32_t batchPeriods = receiver_.receiveChunkSymbols();
    const uint32_t batchTimeoutMs = expectedPeriodMs_ * (batchPeriods + NO_SIGNAL_TIMEOUT_PERIODS) + 2U;
    const uint32_t edgeTimeoutMs = expectedPeriodMs_ * NO_SIGNAL_TIMEOUT_PERIODS + 2U;
    const uint32_t receiveTimeoutMs = batchTimeoutMs > edgeTimeoutMs ? batchTimeoutMs : edgeTimeoutMs;
    if (now - measurementStartMs_ < stepTimeMs_ && now - lastPeriodMs_ > receiveTimeoutMs) {
      fail(FailReason::LOST_EDGE); return state_;
    }
    if (now - measurementStartMs_ > stepTimeMs_ + expectedPeriodMs_ + 2) completeMeasurement();
  }
  return state_;
}

MeasureState Measurement::update() { return state_; }

bool Measurement::continueAfterDisplay() {
  if (state_ != MeasureState::STEP_READY) return false;
  if (!receiver_.start(expectedHz_, expectedDutyPct_)) {
    fail(FailReason::UNSUPPORTED);
    return false;
  }
  // receiver_.start() starts a new RMT timebase and therefore a new sampling
  // phase.  Start a fresh averaging window for the new continuous capture.
  resetAveragingWindow();
  settleLeft_ = settleCycles_;
  measurementStartTick_ = deadlineTick_ = 0;
  startedMs_ = millis(); measurementStartMs_ = lastPeriodMs_ = 0;
  state_ = MeasureState::SETTLING;
  xTaskNotifyGive(task_);
  return true;
}

void Measurement::abort() {
  if (state_ == MeasureState::SETTLING || state_ == MeasureState::RUNNING ||
      state_ == MeasureState::STEP_READY) fail(FailReason::ABORTED);
}