#include "Core.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 *failName(FailReason r) {
  static const char *names[] = {"NONE", "NO SIGNAL", "PERIOD OUT", "DUTY OUT",
    "EXTRA EDGE", "GLITCH", "LOST EDGE", "TOO FEW PERIODS", "LINK LOST",
    "UNSUPPORTED", "RESOLUTION", "ABORTED"};
  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;
}

uint32_t frequencyPointCount(uint32_t startHz, uint32_t endHz, uint32_t stepHz) {
  if (!startHz || !stepHz || endHz <= startHz) return 0;
  const uint64_t span = static_cast<uint64_t>(endHz) - startHz;
  return static_cast<uint32_t>(span / stepHz + 1U + ((span % stepHz) ? 1U : 0U));
}

uint32_t frequencyAt(uint32_t startHz, uint32_t endHz, uint32_t stepHz, uint32_t index) {
  const uint32_t count = frequencyPointCount(startHz, endHz, stepHz);
  if (!count || index >= count) return 0;
  if (index == count - 1) return endHz;
  const uint64_t v = static_cast<uint64_t>(startHz) + static_cast<uint64_t>(stepHz) * index;
  return v < endHz ? static_cast<uint32_t>(v) : endHz;
}

uint64_t nominalTotalUs(const TestParams &p, uint32_t settleCycles) {
  uint64_t total = 0;
  const uint32_t count = frequencyPointCount(p.startHz, p.endHz, p.stepHz);
  for (uint32_t i = 0; i < count; ++i) {
    const uint32_t f = frequencyAt(p.startHz, p.endHz, p.stepHz, i);
    total += (1000000ULL * settleCycles + f - 1) / f;
    total += static_cast<uint64_t>(p.testTimeMs) * 1000ULL * p.repeats;
  }
  return total;
}

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;
}

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 chooseIntegerPwmConfig(uint32_t requestedHz, uint32_t sourceClockHz,
                            uint8_t maxBits, uint8_t dutyPct,
                            IntegerPwmConfig &config) {
  if (!requestedHz || !sourceClockHz || !maxBits || dutyPct > 100U) return false;

  bool found = false;
  uint64_t bestError = 0;
  uint32_t bestDenominator = 1;
  uint32_t bestDutyError = 0;
  uint32_t bestLevels = 1;

  for (uint8_t bits = 1; bits <= maxBits && bits < 31; ++bits) {
    const uint32_t levels = 1UL << bits;
    const uint64_t requestedProduct = static_cast<uint64_t>(requestedHz) * levels;
    uint32_t lowerDivider = static_cast<uint32_t>(sourceClockHz / requestedProduct);
    if (lowerDivider < 1U) lowerDivider = 1U;
    if (lowerDivider > 1023U) lowerDivider = 1023U;

    const uint32_t candidates[] = {lowerDivider,
      lowerDivider < 1023U ? lowerDivider + 1U : lowerDivider};
    for (uint8_t candidate = 0; candidate < 2; ++candidate) {
      const uint32_t divider = candidates[candidate];
      if (candidate && divider == candidates[0]) continue;
      const uint32_t denominator = levels * divider;
      const uint64_t targetClock = static_cast<uint64_t>(requestedHz) * denominator;
      const uint64_t error = targetClock > sourceClockHz
        ? targetClock - sourceClockHz : sourceClockHz - targetClock;
      const uint32_t dutyCount = (static_cast<uint64_t>(levels) * dutyPct + 50U) / 100U;
      const uint32_t representedDuty = dutyCount * 100U;
      const uint32_t requestedDuty = levels * dutyPct;
      const uint32_t dutyError = representedDuty > requestedDuty
        ? representedDuty - requestedDuty : requestedDuty - representedDuty;

      const bool frequencyBetter = !found ||
        error * bestDenominator < bestError * denominator;
      const bool frequencyEqual = found &&
        error * bestDenominator == bestError * denominator;
      const bool dutyBetter = frequencyEqual &&
        static_cast<uint64_t>(dutyError) * bestLevels <
          static_cast<uint64_t>(bestDutyError) * levels;
      const bool dutyEqual = frequencyEqual &&
        static_cast<uint64_t>(dutyError) * bestLevels ==
          static_cast<uint64_t>(bestDutyError) * levels;
      if (!frequencyBetter && !dutyBetter && !(dutyEqual && bits > config.bits)) continue;

      config.actualHz = static_cast<uint32_t>(
        (static_cast<uint64_t>(sourceClockHz) + denominator / 2U) / denominator);
      config.divider = static_cast<uint16_t>(divider);
      config.bits = bits;
      bestError = error;
      bestDenominator = denominator;
      bestDutyError = dutyError;
      bestLevels = levels;
      found = true;
    }
  }
  return found;
}

FailReason validateResolution(uint32_t frequencyHz, float dutyPct, float accuracyPct,
                              uint32_t captureHz, uint8_t pwmBits) {
  if (!frequencyHz || !captureHz || !pwmBits) return FailReason::RESOLUTION;
  const float periodTicks = static_cast<float>(captureHz) / frequencyHz;
  const float activeTicks = periodTicks * dutyPct / 100.0f;
  const float inactiveTicks = periodTicks - activeTicks;
  if (periodTicks < 4.0f || activeTicks < 2.0f || inactiveTicks < 2.0f) return FailReason::RESOLUTION;
  const float timerPeriodError = 100.0f / periodTicks;
  const float timerDutyError = 100.0f / periodTicks;
  // 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.
  return (timerPeriodError > accuracyPct || timerDutyError > accuracyPct)
    ? FailReason::RESOLUTION : FailReason::NONE;
}

FailReason evaluatePeriod(const PulsePeriod &p, uint32_t tickHz, float expectedHz,
                          float expectedDuty, float tolerance, uint8_t repeat,
                          StageStats &s) {
  if (!p.periodTicks || p.activeTicks >= p.periodTicks) return FailReason::EXTRA_EDGE;
  const float hz = static_cast<float>(tickHz) / p.periodTicks;
  const float duty = 100.0f * p.activeTicks / 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;
  FailReason reason = FailReason::NONE;
  if (!periodWithin(hz, expectedHz, tolerance)) reason = FailReason::PERIOD_OUT;
  else if (!dutyWithin(duty, expectedDuty, tolerance)) reason = FailReason::DUTY_OUT;
  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;
}