#pragma once

#include <Arduino.h>

#include "config.h"

class PidAutotuner {
public:
  enum class Phase : uint8_t { Idle, Preheat, Relay, Done, Failed };

  PidAutotuner()
      : phase_(Phase::Idle),
        setpointC_(AUTOTUNE_DEFAULT_TEMP_C),
        relayHigh_(0.0f),
        relayLow_(0.0f),
        peakSinceCross_(0.0f),
        valleySinceCross_(0.0f),
        lastCrossMs_(0),
        periodSumMs_(0),
        periodCount_(0),
        amplitudeSum_(0.0f),
        amplitudeCount_(0),
        spreadSum_(0.0f),
        spreadSamples_(0),
        cycleCount_(0),
        aboveSetpoint_(false),
        useMaxSensorPv_(false),
        sessionStartMs_(0),
        phaseStartMs_(0),
        resultKp_(HEAT_PI_KP),
        resultKi_(HEAT_PI_KI) {}

  Phase phase() const { return phase_; }

  bool isActive() const { return phase_ == Phase::Preheat || phase_ == Phase::Relay; }

  uint32_t elapsedMs(uint32_t nowMs) const {
    if (sessionStartMs_ == 0) {
      return 0;
    }
    return nowMs - sessionStartMs_;
  }

  uint8_t cycleCount() const { return cycleCount_; }
  uint8_t periodCount() const { return periodCount_; }

  float preheatTargetC() const { return setpointC_ - AUTOTUNE_PREHEAT_BAND_C; }

  bool usesMaxSensor() const { return useMaxSensorPv_; }

  const char *phaseName() const {
    switch (phase_) {
    case Phase::Preheat:
      return "preheat";
    case Phase::Relay:
      return useMaxSensorPv_ ? "relay-max" : "relay-avg";
    default:
      return "";
    }
  }

  bool start(float setpointC) {
    if (setpointC < 25.0f || setpointC > TARGET_MAX_C) {
      return false;
    }

    setpointC_ = setpointC;
    relayHigh_ = setpointC + AUTOTUNE_HYSTERESIS_C;
    relayLow_ = setpointC - AUTOTUNE_HYSTERESIS_C;
    resetMeasurements();
    phase_ = Phase::Preheat;
    sessionStartMs_ = millis();
    phaseStartMs_ = sessionStartMs_;
    Serial.print(F("autotune: preheat to "));
    Serial.print(preheatTargetC(), 1);
    Serial.print(F("-"));
    Serial.print(setpointC_, 1);
    Serial.println(F("C avg (heat PI only)"));
    return true;
  }

  void abort() {
    if (phase_ == Phase::Preheat || phase_ == Phase::Relay) {
      Serial.println(F("autotune: cancelled"));
    }
    phase_ = Phase::Idle;
    sessionStartMs_ = 0;
  }

  void reset() {
    phase_ = Phase::Idle;
    sessionStartMs_ = 0;
  }

  float setpoint() const { return setpointC_; }

  float resultKp() const { return resultKp_; }
  float resultKi() const { return resultKi_; }

  Phase update(float avgTempC, float maxTempC, float spreadC, uint32_t nowMs, float &heaterDutyOut,
               uint8_t &fanPwmOut) {
    heaterDutyOut = 0.0f;
    fanPwmOut = AUTOTUNE_PREHEAT_FAN_PWM;

    if (phase_ == Phase::Idle || phase_ == Phase::Done || phase_ == Phase::Failed) {
      return phase_;
    }

    if (maxTempC >= EMERGENCY_MAX_TEMP_C) {
      fail(F("autotune: abort — max sensor at emergency limit"));
      return phase_;
    }

    fanPwmOut = AUTOTUNE_PREHEAT_FAN_PWM;

    if (phase_ == Phase::Preheat) {
      if (nowMs - phaseStartMs_ > AUTOTUNE_PREHEAT_TIMEOUT_MS) {
        Serial.print(F("autotune: preheat failed — avg "));
        Serial.print(avgTempC, 1);
        Serial.print(F("C after "));
        Serial.print((nowMs - sessionStartMs_) / 60000UL);
        Serial.println(F(" min"));
        fail(F("autotune: abort — preheat timeout"));
        return phase_;
      }
      if (avgTempC >= preheatTargetC() || maxTempC >= setpointC_ - 2.0f) {
        enterRelay(avgTempC, maxTempC, spreadC, nowMs);
      } else {
        heaterDutyOut = AUTOTUNE_PREHEAT_DUTY;
      }
      return phase_;
    }

    if (nowMs - sessionStartMs_ > AUTOTUNE_SESSION_TIMEOUT_MS) {
      fail(F("autotune: abort — session timeout"));
      return phase_;
    }

    if (cycleCount_ == 0 && nowMs - phaseStartMs_ > AUTOTUNE_RELAY_STALL_MS) {
      Serial.print(F("autotune: relay stalled — avg "));
      Serial.print(avgTempC, 1);
      Serial.print(F("C max "));
      Serial.print(maxTempC, 1);
      Serial.println(F("C (spread too large for avg to cross setpoint?)"));
      fail(F("autotune: abort — no oscillation"));
      return phase_;
    }

    const float pv = useMaxSensorPv_ ? maxTempC : avgTempC;

    spreadSum_ += spreadC;
    ++spreadSamples_;

    if (pv > peakSinceCross_) {
      peakSinceCross_ = pv;
    }
    if (pv < valleySinceCross_) {
      valleySinceCross_ = pv;
    }

    bool heatOn = false;
    if (pv <= relayLow_) {
      heatOn = true;
    } else if (pv >= relayHigh_) {
      heatOn = false;
    } else {
      heatOn = !aboveSetpoint_;
    }
    heaterDutyOut = heatOn ? 100.0f : 0.0f;

    const bool nowAbove = pv >= setpointC_;
    if (nowAbove != aboveSetpoint_) {
      onSetpointCrossing(nowMs);
      aboveSetpoint_ = nowAbove;
    }

    return phase_;
  }

private:
  void enterRelay(float avgTempC, float maxTempC, float spreadC, uint32_t nowMs) {
    phase_ = Phase::Relay;
    phaseStartMs_ = nowMs;
    useMaxSensorPv_ = spreadC > GOOD_SPREAD_C;
    const float pv = useMaxSensorPv_ ? maxTempC : avgTempC;
    aboveSetpoint_ = pv >= setpointC_;
    peakSinceCross_ = pv;
    valleySinceCross_ = pv;
    lastCrossMs_ = 0;
    Serial.print(F("autotune: relay "));
    Serial.print(useMaxSensorPv_ ? F("max-sensor") : F("avg"));
    Serial.print(F(" ("));
    Serial.print((nowMs - sessionStartMs_) / 1000UL);
    Serial.println(F("s preheat)"));
  }

  void resetMeasurements() {
    peakSinceCross_ = 0.0f;
    valleySinceCross_ = 0.0f;
    lastCrossMs_ = 0;
    periodSumMs_ = 0;
    periodCount_ = 0;
    amplitudeSum_ = 0.0f;
    amplitudeCount_ = 0;
    spreadSum_ = 0.0f;
    spreadSamples_ = 0;
    cycleCount_ = 0;
    aboveSetpoint_ = false;
    useMaxSensorPv_ = false;
  }

  void onSetpointCrossing(uint32_t nowMs) {
    const float amplitude = peakSinceCross_ - valleySinceCross_;
    if (amplitude >= 0.3f) {
      amplitudeSum_ += amplitude;
      ++amplitudeCount_;
      ++cycleCount_;

      Serial.print(F("autotune: cycle "));
      Serial.print(cycleCount_);
      Serial.print(F("/"));
      Serial.print(AUTOTUNE_CYCLES_REQUIRED);
      Serial.print(F(" amp="));
      Serial.print(amplitude, 2);
      Serial.print(F("C elapsed="));
      Serial.print((nowMs - sessionStartMs_) / 1000UL);
      Serial.println(F("s"));
    }

    if (lastCrossMs_ > 0) {
      const uint32_t period = nowMs - lastCrossMs_;
      if (period > 8000 && period < AUTOTUNE_RELAY_PERIOD_MAX_MS) {
        periodSumMs_ += period;
        ++periodCount_;
      }
    }
    lastCrossMs_ = nowMs;
    peakSinceCross_ = valleySinceCross_;

    if (cycleCount_ >= AUTOTUNE_CYCLES_REQUIRED && periodCount_ >= 3 && amplitudeCount_ >= 3) {
      finish(nowMs);
    }
  }

  void finish(uint32_t nowMs) {
    const float avgPeriodSec =
        static_cast<float>(periodSumMs_ / periodCount_) / 1000.0f;
    const float avgAmplitude = amplitudeSum_ / static_cast<float>(amplitudeCount_);

    if (avgAmplitude < 0.3f || avgPeriodSec < 8.0f) {
      fail(F("autotune: failed — oscillation too small"));
      return;
    }

    const float ku = (4.0f * 100.0f) / (PI * avgAmplitude);
    resultKp_ = 0.45f * ku;
    resultKi_ = resultKp_ / (2.2f * avgPeriodSec);

    if (resultKp_ < 0.5f) {
      resultKp_ = 0.5f;
    }
    if (resultKi_ > resultKp_ / 3.0f) {
      resultKi_ = resultKp_ / 3.0f;
    }

    phase_ = Phase::Done;
    Serial.print(F("autotune: done in "));
    Serial.print((nowMs - sessionStartMs_) / 1000UL);
    Serial.println(F("s"));
    Serial.print(F("  heat Kp="));
    Serial.print(resultKp_, 3);
    Serial.print(F(" Ki="));
    Serial.println(resultKi_, 4);
    Serial.println(F("  tune heat PI: pid save after autotune"));
  }

  void fail(const __FlashStringHelper *reason) {
    Serial.println(reason);
    phase_ = Phase::Failed;
    sessionStartMs_ = 0;
  }

  Phase phase_;
  float setpointC_;
  float relayHigh_;
  float relayLow_;
  float peakSinceCross_;
  float valleySinceCross_;
  uint32_t lastCrossMs_;
  uint32_t periodSumMs_;
  uint8_t periodCount_;
  float amplitudeSum_;
  uint8_t amplitudeCount_;
  float spreadSum_;
  uint16_t spreadSamples_;
  uint8_t cycleCount_;
  bool aboveSetpoint_;
  bool useMaxSensorPv_;
  uint32_t sessionStartMs_;
  uint32_t phaseStartMs_;
  float resultKp_;
  float resultKi_;
};