#include "Core.h"
#include "Config.h"
#include <math.h>
#include <string.h>

const char *roleName(Role r) {
  static const char *names[] = {"SOLO", "MASTER", "SLAVE"};
  const uint8_t i = static_cast<uint8_t>(r);
  return i < 3 ? names[i] : "?";
}

const char *testKindName(TestKind kind) {
  static const char *names[] = {"OPTICAL", "DRIVER"};
  const uint8_t i = static_cast<uint8_t>(kind);
  return i < 2 ? names[i] : "?";
}

const char *lightCodeName(LightCode code) {
  static const char *names[] = {"HH", "HL", "LH", "LL"};
  const uint8_t i = static_cast<uint8_t>(code);
  return i < 4 ? names[i] : "??";
}

const char *failName(FailReason r) {
  static const char *names[] = {"NONE", "NO SIGNAL", "PERIOD OUT", "PULSE OUT",
    "EXTRA EDGE", "GLITCH", "LOST EDGE", "DATA LOSS ERROR", "LINK LOST",
    "UNSUPPORTED", "RESOLUTION", "ABORTED", "ACK MISSING", "ACK TIMING",
    "DRIVER FAULT", "ACK MERGED"};
  const uint8_t i = static_cast<uint8_t>(r);
  return i < (sizeof(names) / sizeof(names[0])) ? names[i] : "UNKNOWN";
}

void StageStats::reset() {
  memset(this, 0, sizeof(*this));
  minPeriod = minActive = UINT32_MAX;
  reason = FailReason::NONE;
}

uint32_t settingsChecksum(const Settings &s) {
  const uint8_t *p = reinterpret_cast<const uint8_t *>(&s);
  const size_t n = offsetof(Settings, checksum);
  uint32_t hash = 2166136261UL;
  for (size_t i = 0; i < n; ++i) { hash ^= p[i]; hash *= 16777619UL; }
  return hash;
}

bool txActiveLightOn(const Settings &s) {
  return static_cast<uint8_t>(s.lightCode) < static_cast<uint8_t>(LightCode::LH);
}

bool rxActiveLightOn(const Settings &s) {
  return (static_cast<uint8_t>(s.lightCode) & 1U) == 0U;
}

uint32_t pulseWidthPointCount(uint32_t maxPulseNs, uint32_t minPulseNs) {
  if (!minPulseNs || maxPulseNs < minPulseNs) return 0;
  uint32_t count = 0;
  for (size_t i = 0; i < countOf(TEST_PULSE_WIDTHS_NS); ++i)
    if (TEST_PULSE_WIDTHS_NS[i] >= minPulseNs && TEST_PULSE_WIDTHS_NS[i] <= maxPulseNs) ++count;
  return count;
}

uint32_t pulseWidthAt(uint32_t maxPulseNs, uint32_t minPulseNs, uint32_t index) {
  for (size_t i = countOf(TEST_PULSE_WIDTHS_NS); i > 0; --i) {
    const uint32_t pulseNs = TEST_PULSE_WIDTHS_NS[i - 1U];
    if (pulseNs < minPulseNs || pulseNs > maxPulseNs) continue;
    if (!index--) return pulseNs;
  }
  return 0;
}

uint64_t nominalStageUs(uint32_t frequencyHz, uint32_t sampleTimeMs, uint32_t settleCycles) {
  if (!frequencyHz || !sampleTimeMs) return 0;
  const uint64_t sampleUs = static_cast<uint64_t>(sampleTimeMs) * 1000ULL;
  // At high frequency the CPU needs longer than the requested sample window
  // to validate every captured period. Use the measured sustained C3 rate.
  const uint64_t processingUs =
    (static_cast<uint64_t>(frequencyHz) * sampleTimeMs * 1000ULL +
      RX_PROCESSING_PERIODS_PER_SECOND - 1U) / RX_PROCESSING_PERIODS_PER_SECOND;
  const uint64_t samplingWallUs = processingUs > sampleUs ? processingUs : sampleUs;

  const uint64_t settleUs =
    (1000000ULL * settleCycles * MEASUREMENT_PROGRESS_STEPS + frequencyHz - 1U) / frequencyHz;
  // Initial stage screen, nine intermediate screens and the final result.
  const uint64_t displayUs = static_cast<uint64_t>(OLED_PROGRESS_UPDATE_MS) * 1000ULL *
    (MEASUREMENT_PROGRESS_STEPS + 1U);
  return samplingWallUs + settleUs + displayUs;
}

uint64_t nominalTotalUs(const TestParams &p, uint32_t settleCycles) {
  return static_cast<uint64_t>(pulseWidthPointCount(p.maxPulseNs, p.minPulseNs)) *
    nominalStageUs(p.frequencyHz, p.testTimeMs, settleCycles);
}

bool periodWithin(float measured, float expected, float tolerance) {
  return expected > 0.0f && fabsf(measured - expected) * 100.0f / expected <= tolerance + 0.0001f;
}

bool dutyWithin(float measured, float expected, float tolerance) {
  return fabsf(measured - expected) <= tolerance + 0.0001f;
}

float effectiveTolerancePct(float configured) {
  return configured;
}

uint8_t choosePwmResolution(uint32_t frequencyHz, uint32_t sourceClockHz,
                            uint8_t maxBits) {
  if (!frequencyHz || !sourceClockHz || !maxBits) return 0;
  uint8_t bits = maxBits;
  while (bits > 1 && static_cast<uint64_t>(frequencyHz) * (1ULL << bits) > sourceClockHz)
    --bits;
  return bits;
}

uint8_t chooseStablePwmResolution(uint32_t frequencyHz, uint32_t sourceClockHz,
                                  uint8_t maxBits, uint8_t dutyPct) {
  const uint8_t fallback = choosePwmResolution(frequencyHz, sourceClockHz, maxBits);
  if (!fallback || dutyPct > 100U) return fallback;
  for (uint8_t bits = fallback; bits > 0; --bits) {
    const uint32_t levels = 1UL << bits;
    const uint64_t denominator = static_cast<uint64_t>(frequencyHz) * levels;
    if (denominator > sourceClockHz || sourceClockHz % denominator) continue;
    const uint32_t divider = static_cast<uint32_t>(sourceClockHz / denominator);
    if (!divider || divider > 1024U) continue;
    if ((static_cast<uint32_t>(levels) * dutyPct) % 100U == 0U) return bits;
  }
  return fallback;
}

bool choosePwmConfig(uint32_t requestedHz, uint32_t requestedPulseNs,
                     uint32_t sourceClockHz, uint8_t maxBits,
                     IntegerPwmConfig &config) {
  if (!requestedHz || !requestedPulseNs || !sourceClockHz || !maxBits) return false;

  constexpr uint32_t FRACTION_SCALE = 256U;
  constexpr uint32_t MAX_DIVIDER_RAW = 1024U * FRACTION_SCALE - 1U;
  bool found = false;
  uint32_t bestFrequencyError = UINT32_MAX;
  uint32_t bestPulseError = UINT32_MAX;

  for (uint8_t bits = 1; bits <= maxBits && bits < 31; ++bits) {
    const uint32_t levels = 1UL << bits;
    const uint64_t dividerNumerator = static_cast<uint64_t>(sourceClockHz) * FRACTION_SCALE;
    const uint64_t dividerDenominator = static_cast<uint64_t>(requestedHz) * levels;
    const uint32_t dividerFloor = static_cast<uint32_t>(dividerNumerator / dividerDenominator);
    const uint32_t candidates[] = {dividerFloor, dividerFloor + 1U};
    for (uint32_t dividerRaw : candidates) {
      if (dividerRaw < FRACTION_SCALE || dividerRaw > MAX_DIVIDER_RAW) continue;
      const uint64_t frequencyDenominator = static_cast<uint64_t>(levels) * dividerRaw;
      const uint32_t actualHz = static_cast<uint32_t>(
        (dividerNumerator + frequencyDenominator / 2U) / frequencyDenominator);
      if (!actualHz) continue;

      const uint64_t dutyNumerator = static_cast<uint64_t>(requestedPulseNs) *
        sourceClockHz * FRACTION_SCALE;
      const uint64_t dutyDenominator = static_cast<uint64_t>(dividerRaw) * 1000000000ULL;
      uint32_t dutyCount = static_cast<uint32_t>((dutyNumerator + dutyDenominator / 2U) /
        dutyDenominator);
      if (!dutyCount) dutyCount = 1U;
      if (dutyCount >= levels) dutyCount = levels - 1U;
      if (!dutyCount) continue;

      const uint32_t actualPulseNs = static_cast<uint32_t>(
        (static_cast<uint64_t>(dutyCount) * dividerRaw * 1000000000ULL +
          static_cast<uint64_t>(sourceClockHz) * FRACTION_SCALE / 2U) /
        (static_cast<uint64_t>(sourceClockHz) * FRACTION_SCALE));
      const uint32_t frequencyError = actualHz > requestedHz ? actualHz - requestedHz : requestedHz - actualHz;
      const uint32_t pulseError = actualPulseNs > requestedPulseNs
        ? actualPulseNs - requestedPulseNs : requestedPulseNs - actualPulseNs;
      if (found && (frequencyError > bestFrequencyError ||
          (frequencyError == bestFrequencyError && pulseError > bestPulseError) ||
          (frequencyError == bestFrequencyError && pulseError == bestPulseError && bits <= config.bits)))
        continue;

      config = {actualHz, dividerRaw, dutyCount, actualPulseNs, bits};
      bestFrequencyError = frequencyError;
      bestPulseError = pulseError;
      found = true;
    }
  }
  return found;
}

FailReason validateResolution(uint32_t frequencyHz, float dutyPct, float accuracyPct,
                              uint32_t periodCaptureHz, uint32_t pulseCaptureHz,
                              uint8_t pwmBits,
                              uint16_t averagingPeriods) {
  if (!frequencyHz || !periodCaptureHz || !pulseCaptureHz || !pwmBits || !averagingPeriods)
    return FailReason::RESOLUTION;
  const float periodTicks = static_cast<float>(periodCaptureHz) / frequencyHz;
  const float activeTicks = static_cast<float>(pulseCaptureHz) * dutyPct /
    (100.0f * frequencyHz);
  if (periodTicks < 4.0f || activeTicks < 2.0f) return FailReason::RESOLUTION;
  const float timerPeriodError = 100.0f / periodTicks;
  const float timerPulseError = 100.0f / activeTicks;
  // Measurement uses the duty actually programmed into LEDC. A coarse PWM
  // step is not itself an error when the requested value (e.g. 50%) is exactly
  // representable; only the selected value's actual quantization matters.
  const float effectiveAccuracy = effectiveTolerancePct(accuracyPct);
  return (timerPeriodError > effectiveAccuracy || timerPulseError > effectiveAccuracy)
    ? FailReason::RESOLUTION : FailReason::NONE;
}

FailReason evaluatePeriod(const PulsePeriod &p, uint32_t tickHz, float expectedHz,
                          float expectedDuty, float tolerance, uint8_t repeat,
                          StageStats &s) {
  const uint32_t pulseTickHz = p.activeTickHz ? p.activeTickHz : tickHz;
  if (!p.periodTicks || !p.activeTicks || !tickHz || !pulseTickHz || expectedHz <= 0.0f)
    return FailReason::EXTRA_EDGE;
  const float hz = static_cast<float>(tickHz) / p.periodTicks;
  const float duty = static_cast<float>(100.0 * p.activeTicks * tickHz /
    (static_cast<double>(pulseTickHz) * p.periodTicks));
  ++s.periods;
  s.periodSum += p.periodTicks; s.activeSum += p.activeTicks;
  if (p.periodTicks < s.minPeriod) s.minPeriod = p.periodTicks;
  if (p.periodTicks > s.maxPeriod) s.maxPeriod = p.periodTicks;
  if (p.activeTicks < s.minActive) s.minActive = p.activeTicks;
  if (p.activeTicks > s.maxActive) s.maxActive = p.activeTicks;
  // Validate every complete period independently. A single capture tick is the
  // unavoidable endpoint uncertainty, so only that one tick may be corrected
  // toward the expected value. It cannot hide a larger isolated distortion.
  bool frequencyOk = periodWithin(hz, expectedHz, tolerance);
  if (!frequencyOk) {
    uint32_t correctedPeriod = p.periodTicks;
    if (hz > expectedHz) ++correctedPeriod;
    else if (correctedPeriod > 1U) --correctedPeriod;
    frequencyOk = periodWithin(static_cast<float>(tickHz) / correctedPeriod,
      expectedHz, tolerance);
  }

  const double expectedPulseTicks = static_cast<double>(pulseTickHz) * expectedDuty /
    (100.0 * expectedHz);
  bool pulseOk = expectedPulseTicks > 0.0 &&
    fabs(static_cast<double>(p.activeTicks) - expectedPulseTicks) * 100.0 /
      expectedPulseTicks <= tolerance + 0.0001;
  if (!pulseOk) {
    uint32_t correctedActive = p.activeTicks;
    if (correctedActive > expectedPulseTicks) {
      if (correctedActive) --correctedActive;
    } else {
      ++correctedActive;
    }
    pulseOk = fabs(static_cast<double>(correctedActive) - expectedPulseTicks) *
      100.0 / expectedPulseTicks <= tolerance + 0.0001;
  }

  const FailReason reason = !frequencyOk ? FailReason::PERIOD_OUT :
    (!pulseOk ? FailReason::DUTY_OUT : FailReason::NONE);
  if (reason != FailReason::NONE && s.reason == FailReason::NONE) {
    s.reason = reason; s.firstBadPeriod = s.periods; s.firstBadRepeat = repeat;
    s.badFrequency = hz; s.badDuty = duty;
  }
  return reason;
}

bool makePeriodLimits(uint32_t expectedHz, float expectedDuty, float tolerance,
                      uint32_t tickHz, PeriodLimits &limits) {
  if (!expectedHz || !tickHz || tolerance < 0.0f || tolerance >= 100.0f ||
      expectedDuty <= 0.0f || expectedDuty >= 100.0f) return false;
  const uint32_t toleranceX100 = static_cast<uint32_t>(lroundf(tolerance * 100.0f));
  const uint32_t dutyX100 = static_cast<uint32_t>(lroundf(expectedDuty * 100.0f));
  const uint64_t numerator = static_cast<uint64_t>(tickHz) * 10000ULL;
  const uint64_t highDenominator = static_cast<uint64_t>(expectedHz) * (10000U + toleranceX100);
  const uint64_t lowDenominator = static_cast<uint64_t>(expectedHz) * (10000U - toleranceX100);
  limits.minPeriodTicks = static_cast<uint32_t>((numerator + highDenominator - 1U) / highDenominator);
  limits.maxPeriodTicks = static_cast<uint32_t>(numerator / lowDenominator);
  limits.minDutyX100 = dutyX100 > toleranceX100 ? dutyX100 - toleranceX100 : 0;
  limits.maxDutyX100 = dutyX100 + toleranceX100;
  return limits.minPeriodTicks && limits.maxPeriodTicks >= limits.minPeriodTicks;
}

FailReason evaluatePeriodFast(const PulsePeriod &p, uint32_t tickHz,
                              const PeriodLimits &limits, uint8_t repeat,
                              StageStats &s) {
  if (!p.periodTicks || p.activeTicks >= p.periodTicks) return FailReason::EXTRA_EDGE;
  ++s.periods;
  s.periodSum += p.periodTicks; s.activeSum += p.activeTicks;
  if (p.periodTicks < s.minPeriod) s.minPeriod = p.periodTicks;
  if (p.periodTicks > s.maxPeriod) s.maxPeriod = p.periodTicks;
  if (p.activeTicks < s.minActive) s.minActive = p.activeTicks;
  if (p.activeTicks > s.maxActive) s.maxActive = p.activeTicks;

  FailReason reason = FailReason::NONE;
  if (p.periodTicks < limits.minPeriodTicks || p.periodTicks > limits.maxPeriodTicks) {
    reason = FailReason::PERIOD_OUT;
  } else {
    // The configured range (>= 1 kHz at 80 MHz capture) fits these products
    // into 32 bits. Keep a 64-bit fallback for unusually slow external input.
    if (p.periodTicks <= UINT32_MAX / 10000U && limits.maxDutyX100 <= 10000U) {
      const uint32_t scaledActive = p.activeTicks * 10000U;
      const uint32_t minActive = p.periodTicks * limits.minDutyX100;
      const uint32_t maxActive = p.periodTicks * limits.maxDutyX100;
      if (scaledActive < minActive || scaledActive > maxActive) reason = FailReason::DUTY_OUT;
    } else {
      const uint64_t scaledActive = static_cast<uint64_t>(p.activeTicks) * 10000ULL;
      const uint64_t minActive = static_cast<uint64_t>(p.periodTicks) * limits.minDutyX100;
      const uint64_t maxActive = static_cast<uint64_t>(p.periodTicks) * limits.maxDutyX100;
      if (scaledActive < minActive || scaledActive > maxActive) reason = FailReason::DUTY_OUT;
    }
  }
  if (reason != FailReason::NONE && s.reason == FailReason::NONE) {
    s.reason = reason; s.firstBadPeriod = s.periods; s.firstBadRepeat = repeat;
    s.badFrequency = static_cast<float>(tickHz) / p.periodTicks;
    s.badDuty = 100.0f * p.activeTicks / p.periodTicks;
  }
  return reason;
}

FailReason evaluatePeriodWindow(uint64_t periodSum, uint64_t activeSum,
                                uint32_t periodCount, uint32_t tickHz,
                                float expectedHz, float expectedDuty,
                                float tolerance,
                                uint32_t minPeriod, uint32_t maxPeriod,
                                uint8_t repeat,
                                StageStats &s) {
  if (!periodSum || !periodCount || activeSum >= periodSum || !tickHz)
    return FailReason::EXTRA_EDGE;
  const float hz = static_cast<float>(
    static_cast<double>(tickHz) * periodCount / periodSum);
  const float duty = static_cast<float>(
    100.0 * static_cast<double>(activeSum) / periodSum);
  bool frequencyOk = periodWithin(hz, expectedHz, tolerance);
  if (!frequencyOk && maxPeriod == minPeriod + 1U) {
    // At a tolerance boundary, alternating adjacent capture counts prove that the
    // result is quantization-limited. Accept only when a one-tick correction
    // toward the expected value returns the averaged frequency into tolerance.
    // Consecutive periods telescope into one first-to-last edge interval, so
    // the whole window has a one-tick endpoint uncertainty, not one tick per
    // period.
    uint64_t correctedPeriodSum = periodSum;
    if (hz > expectedHz) ++correctedPeriodSum;
    else if (periodSum > 1U) --correctedPeriodSum;
    const float correctedHz = static_cast<float>(
      static_cast<double>(tickHz) * periodCount / correctedPeriodSum);
    frequencyOk = periodWithin(correctedHz, expectedHz, tolerance);
  }

  const double expectedPulseTicks = static_cast<double>(tickHz) * expectedDuty /
    (100.0 * expectedHz);
  const double measuredPulseTicks = static_cast<double>(activeSum) / periodCount;
  bool pulseOk = expectedPulseTicks > 0.0 &&
    fabs(measuredPulseTicks - expectedPulseTicks) * 100.0 / expectedPulseTicks <= tolerance + 0.0001;
  if (!pulseOk) {
    // Unlike full periods, active intervals do not telescope: every pulse is
    // bounded by a different rising/falling edge pair. With slowly drifting
    // asynchronous clocks an entire short window can therefore quantize to
    // the same adjacent count (e.g. 41/80 for a true 50% duty). Apply one tick
    // per active interval even when minActive == maxActive.
    const bool dutyHigh = measuredPulseTicks > expectedPulseTicks;
    const uint64_t correctedActive = dutyHigh
      ? (activeSum > periodCount ? activeSum - periodCount : 0U)
      : activeSum + periodCount;
    const double correctedPulseTicks = static_cast<double>(correctedActive) / periodCount;
    pulseOk = fabs(correctedPulseTicks - expectedPulseTicks) * 100.0 /
      expectedPulseTicks <= tolerance + 0.0001;
  }

  FailReason reason = !frequencyOk ? FailReason::PERIOD_OUT :
    (!pulseOk ? FailReason::DUTY_OUT : FailReason::NONE);
  if (reason != FailReason::NONE && s.reason == FailReason::NONE) {
    s.reason = reason;
    s.firstBadPeriod = s.periods >= periodCount ? s.periods - periodCount + 1U : 1U;
    s.firstBadRepeat = repeat;
    s.badFrequency = hz;
    s.badDuty = duty;
  }
  return reason;
}