#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),
        phaseStartMs_(0),
        resultKp_(PID_KP),
        resultKi_(PID_KI),
        resultKd_(PID_KD),
        resultFanMixMax_(FAN_MIX_MAX_PWM) {}

  Phase phase() const { return phase_; }

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

  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;
    phaseStartMs_ = millis();
    Serial.print(F("autotune: preheat to "));
    Serial.print(setpointC_, 1);
    Serial.println(F("C"));
    return true;
  }

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

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

  float setpoint() const { return setpointC_; }

  float resultKp() const { return resultKp_; }
  float resultKi() const { return resultKi_; }
  float resultKd() const { return resultKd_; }
  uint8_t resultFanMixMax() const { return resultFanMixMax_; }

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

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

    if (maxTempC >= setpointC_ + AUTOTUNE_ABORT_ABOVE_C) {
      fail(F("autotune: abort — temperature too high"));
      return phase_;
    }

    if (nowMs - phaseStartMs_ > AUTOTUNE_TIMEOUT_MS) {
      fail(F("autotune: abort — timeout"));
      return phase_;
    }

    if (phase_ == Phase::Preheat) {
      if (avgTempC >= setpointC_ - AUTOTUNE_PREHEAT_BAND_C) {
        enterRelay(avgTempC, nowMs);
      } else {
        heaterDutyOut = AUTOTUNE_PREHEAT_DUTY;
      }
      return phase_;
    }

    spreadSum_ += spreadC;
    ++spreadSamples_;

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

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

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

    return phase_;
  }

private:
  void enterRelay(float avgTempC, uint32_t nowMs) {
    phase_ = Phase::Relay;
    phaseStartMs_ = nowMs;
    aboveSetpoint_ = avgTempC >= setpointC_;
    peakSinceCross_ = avgTempC;
    valleySinceCross_ = avgTempC;
    lastCrossMs_ = 0;
    Serial.println(F("autotune: relay test started"));
  }

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

  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(" amp="));
      Serial.println(amplitude, 2);
    }

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

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

  void finish() {
    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);
    resultKd_ = resultKp_ * avgPeriodSec / 6.3f;

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

    resultFanMixMax_ = FAN_MIX_MAX_PWM;
    const float spreadAvg =
        spreadSamples_ > 0 ? spreadSum_ / static_cast<float>(spreadSamples_) : GOOD_SPREAD_C;
    if (spreadAvg > GOOD_SPREAD_C) {
      const float boost = 1.0f + ((spreadAvg - GOOD_SPREAD_C) / 10.0f);
      int boosted = static_cast<int>(static_cast<float>(FAN_MIX_MAX_PWM) * boost);
      if (boosted > static_cast<int>(FAN_MAX_PWM) - 20) {
        boosted = FAN_MAX_PWM - 20;
      }
      resultFanMixMax_ = static_cast<uint8_t>(boosted);
    }

    phase_ = Phase::Done;
    Serial.println(F("autotune: done"));
    Serial.print(F("  Kp="));
    Serial.print(resultKp_, 3);
    Serial.print(F(" Ki="));
    Serial.print(resultKi_, 4);
    Serial.print(F(" Kd="));
    Serial.print(resultKd_, 3);
    Serial.print(F(" fanMixMax="));
    Serial.println(resultFanMixMax_);
  }

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

  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_;
  uint32_t phaseStartMs_;
  float resultKp_;
  float resultKi_;
  float resultKd_;
  uint8_t resultFanMixMax_;
};