arduino-filament-dryer/include/thermal_controller.h

#pragma once

#include <Arduino.h>

#include "config.h"
#include "pid_autotuner.h"
#include "pid_controller.h"
#include "tuning_store.h"

class ThermalController {
public:
  ThermalController()
      : pid_(PID_KP, PID_KI, PID_KD, 0.0f, 100.0f),
        autotuner_(),
        targetTempC_(TARGET_TEMP_C),
        heaterDutyPercent_(0.0f),
        heaterAllowancePercent_(100.0f),
        cornerSpreadC_(0.0f),
        lastMaxTempC_(0.0f),
        fanPwm_(FAN_MAX_PWM),
        fanMixMax_(FAN_MIX_MAX_PWM),
        adaptiveEnabled_(false),
        fanIdleOverride_(false),
        sensorWarmValid_(false),
        cutoffActive_(false),
        failSafeActive_(true),
        heaterCycleStartMs_(0),
        lastHeaterUpdateMs_(0),
        heaterOn_(false) {}

  void begin() {
    pinMode(FAN_PIN, OUTPUT);
    pinMode(HEATER_PIN, OUTPUT);
    digitalWrite(HEATER_PIN, LOW);

    pid_.setSetpoint(targetTempC_);
    pid_.reset();
    heaterCycleStartMs_ = millis();
    lastHeaterUpdateMs_ = 0;
    failSafeActive_ = true;
    fanPwm_ = FAN_MAX_PWM;
    cornerSpreadC_ = 0.0f;
    lastMaxTempC_ = 0.0f;
    sensorWarmValid_ = false;
    heaterAllowancePercent_ = 100.0f;
    if (isIdle()) {
      forceHeaterOff();
    }
    applyFan();

    TuningData stored;
    if (tuningLoad(stored)) {
      applyTuning(stored);
      Serial.println(F("Loaded learned PID from EEPROM"));
      printTuning();
    }
  }

  void applyTuning(const TuningData &data) {
    pid_.setTunings(data.kp, data.ki, data.kd);
    fanMixMax_ = data.fanMixMax;
    adaptiveEnabled_ = true;
  }

  void clearTuning() {
    adaptiveEnabled_ = false;
    fanMixMax_ = FAN_MIX_MAX_PWM;
    pid_.setTunings(PID_KP, PID_KI, PID_KD);
    tuningClear();
    pid_.reset();
    Serial.println(F("PID reset to defaults"));
  }

  void printTuning() const {
    Serial.print(F("PID Kp="));
    Serial.print(pidKp(), 3);
    Serial.print(F(" Ki="));
    Serial.print(pidKi(), 4);
    Serial.print(F(" Kd="));
    Serial.print(pidKd(), 3);
    Serial.print(F(" fanMixMax="));
    Serial.print(fanMixMax_);
    Serial.print(F(" adaptive="));
    Serial.println(adaptiveEnabled_ ? F("yes") : F("no"));
  }

  float pidKp() const { return pid_.kp(); }
  float pidKi() const { return pid_.ki(); }
  float pidKd() const { return pid_.kd(); }

  bool isAdaptive() const { return adaptiveEnabled_; }

  bool startAutotune(float setpointC) {
    if (autotuner_.isActive()) {
      return false;
    }
    adaptiveEnabled_ = false;
    cutoffActive_ = false;
    pid_.reset();
    return autotuner_.start(setpointC);
  }

  void stopAutotune() { autotuner_.abort(); }

  bool isAutotuning() const { return autotuner_.isActive(); }

  uint32_t autotuneElapsedMs(uint32_t nowMs) const { return autotuner_.elapsedMs(nowMs); }

  const char *autotunePhaseName() const { return autotuner_.phaseName(); }

  uint8_t autotuneCycleCount() const { return autotuner_.cycleCount(); }

  uint8_t autotunePeriodCount() const { return autotuner_.periodCount(); }

  float autotunePreheatTargetC() const { return autotuner_.preheatTargetC(); }

  bool commitAutotuneIfDone() {
    if (autotuner_.phase() != PidAutotuner::Phase::Done) {
      return false;
    }

    TuningData data;
    data.magic = TUNING_MAGIC;
    data.kp = autotuner_.resultKp();
    data.ki = autotuner_.resultKi();
    data.kd = autotuner_.resultKd();
    data.fanMixMax = autotuner_.resultFanMixMax();
    tuningSave(data);
    applyTuning(data);
    autotuner_.reset();
    Serial.println(F("Saved learned PID to EEPROM"));
    return true;
  }

  void setTarget(float targetC) {
    targetTempC_ = targetC;
    pid_.setSetpoint(targetC);
    pid_.reset();
    cutoffActive_ = false;
    if (targetC > 0.0f) {
      fanIdleOverride_ = false;
    } else {
      forceHeaterOff();
      fanIdleOverride_ = false;
    }
    applyFan();
  }

  void noteSensorMax(float maxTempC) {
    lastMaxTempC_ = maxTempC;
    sensorWarmValid_ = true;
  }

  bool setFanOff() {
    if (!isIdle()) {
      return false;
    }
    fanIdleOverride_ = false;
    applyFan();
    return true;
  }

  void setFanIdle() {
    if (!isIdle()) {
      return;
    }
    if (!sensorWarmValid_ || lastMaxTempC_ >= IDLE_AUTO_FAN_OFF_TEMP_C) {
      return;
    }
    fanIdleOverride_ = true;
    applyFan();
  }

  bool isFanOff() const {
    return isIdle() && sensorWarmValid_ && lastMaxTempC_ < IDLE_AUTO_FAN_OFF_TEMP_C &&
           !fanIdleOverride_;
  }

  bool isIdleCooling() const {
    return isIdle() &&
           (!sensorWarmValid_ || lastMaxTempC_ >= IDLE_AUTO_FAN_OFF_TEMP_C);
  }

  float target() const { return targetTempC_; }

  bool isIdle() const { return targetTempC_ <= 0.0f; }

  float cutoffThreshold() const {
    if (isIdle()) {
      return INFINITY;
    }
    return targetTempC_ * (1.0f + OVERTEMP_FRACTION);
  }

  bool isCutoffActive() const { return cutoffActive_; }

  bool isFailSafeActive() const { return failSafeActive_; }

  float heaterDutyPercent() const { return heaterDutyPercent_; }

  float heaterAllowance() const { return heaterAllowancePercent_; }

  float cornerSpread() const { return cornerSpreadC_; }

  uint8_t fanPwm() const { return fanPwm_; }

  void update(float avgTempC, float maxTempC, float cornerSpreadC, uint32_t nowMs) {
    failSafeActive_ = false;
    noteSensorMax(maxTempC);

    cornerSpreadC_ =
        SPREAD_EMA_ALPHA * cornerSpreadC +
        (1.0f - SPREAD_EMA_ALPHA) * cornerSpreadC_;

    if (autotuner_.isActive()) {
      updateAutotune(avgTempC, maxTempC, nowMs);
      return;
    }

    if (isIdle()) {
      forceHeaterOff();
      cutoffActive_ = false;
      pid_.reset();
      lastHeaterUpdateMs_ = nowMs;
      applyFan();
      return;
    }

    if (adaptiveEnabled_) {
      updateAdaptive(avgTempC, maxTempC, nowMs);
    } else {
      updateLegacy(avgTempC, maxTempC, nowMs);
    }

    lastHeaterUpdateMs_ = nowMs;
    applyHeaterBurst(nowMs);
    applyFan();
  }

  void enterFailSafe() {
    failSafeActive_ = true;
    cutoffActive_ = false;
    forceHeaterOff();
    applyFan();
    pid_.reset();
    autotuner_.abort();
  }

  void forceHeaterOff() {
    heaterDutyPercent_ = 0.0f;
    heaterAllowancePercent_ = 0.0f;
    heaterOn_ = false;
    digitalWrite(HEATER_PIN, LOW);
  }

  void writeFan(uint8_t pwm) {
    fanPwm_ = pwm;
    if (pwm == 0) {
      // Re-assert output and stop Timer0 PWM on D5 — analogWrite(0) can leave the pin driving
      pinMode(FAN_PIN, OUTPUT);
      digitalWrite(FAN_PIN, LOW);
    } else {
      analogWrite(FAN_PIN, pwm);
    }
  }

private:
  void updateAutotune(float avgTempC, float maxTempC, uint32_t nowMs) {
    float duty = 0.0f;
    uint8_t fan = FAN_HEAT_MIN_PWM;
    autotuner_.update(avgTempC, maxTempC, cornerSpreadC_, nowMs, duty, fan);

    heaterDutyPercent_ = duty;
    heaterAllowancePercent_ = duty;
    heaterOn_ = duty >= 50.0f;
    digitalWrite(HEATER_PIN, heaterOn_ ? HIGH : LOW);
    writeFan(fan);
    lastHeaterUpdateMs_ = nowMs;
    commitAutotuneIfDone();
  }

  void updateAdaptive(float avgTempC, float maxTempC, uint32_t nowMs) {
    const float cutoff = cutoffThreshold();

    if (maxTempC >= cutoff) {
      cutoffActive_ = true;
      heaterDutyPercent_ = 0.0f;
      heaterAllowancePercent_ = 0.0f;
      heaterOn_ = false;
      pid_.reset();
      return;
    }

    if (cutoffActive_ && maxTempC <= targetTempC_) {
      cutoffActive_ = false;
      pid_.reset();
    }

    if (cutoffActive_) {
      return;
    }

    const float maxHeatStopC = maxHeatStopTemp(avgTempC);
    if (maxTempC >= maxHeatStopC) {
      heaterDutyPercent_ = 0.0f;
      heaterAllowancePercent_ = 0.0f;
      pid_.reset();
      return;
    }

    float duty = pid_.compute(avgTempC, nowMs);
    const float maxDuty = heaterMaxDuty(avgTempC);
    if (duty > maxDuty) {
      duty = maxDuty;
    }
    heaterAllowancePercent_ = maxDuty;
    heaterDutyPercent_ = duty;
  }

  void updateLegacy(float avgTempC, float maxTempC, uint32_t nowMs) {
    const float cutoff = cutoffThreshold();

    if (maxTempC >= cutoff) {
      cutoffActive_ = true;
      heaterDutyPercent_ = 0.0f;
      heaterAllowancePercent_ = 0.0f;
      heaterOn_ = false;
      pid_.reset();
      return;
    }

    if (cutoffActive_ && maxTempC <= targetTempC_) {
      cutoffActive_ = false;
      pid_.reset();
    }

    if (cutoffActive_) {
      return;
    }

    const float maxHeatStopC = maxHeatStopTemp(avgTempC);
    if (maxTempC >= maxHeatStopC) {
      heaterDutyPercent_ = 0.0f;
      heaterAllowancePercent_ = 0.0f;
      pid_.reset();
      return;
    }

    const float pidOut = pid_.compute(avgTempC, nowMs);
    heaterAllowancePercent_ = heaterAllowancePercent(avgTempC, maxTempC);
    float duty = pidOut;
    if (duty > heaterAllowancePercent_) {
      duty = heaterAllowancePercent_;
    }
    heaterDutyPercent_ = applyHeaterRamp(duty, avgTempC, nowMs);
  }

  static float clampPercent(float value) {
    if (value < 0.0f) {
      return 0.0f;
    }
    if (value > 100.0f) {
      return 100.0f;
    }
    return value;
  }

  bool isBalancedChamber() const { return cornerSpreadC_ <= GOOD_SPREAD_C; }

  float maxHeatStopTemp(float avgTempC) const {
    if (avgTempC >= targetTempC_) {
      return targetTempC_;
    }

    float stopAt = targetTempC_;
    if (isBalancedChamber()) {
      stopAt = targetTempC_ + BALANCED_MAX_ABOVE_TARGET_C;
    } else {
      stopAt = targetTempC_ + cornerSpreadC_ * SPREAD_HEADROOM_FACTOR + 1.0f;
    }

    const float belowCutoff = cutoffThreshold() - 0.1f;
    if (stopAt > belowCutoff) {
      stopAt = belowCutoff;
    }
    return stopAt;
  }

  float allowanceFromMaxCorner(float maxTempC, float avgTempC) const {
    if (isBalancedChamber() && avgTempC < targetTempC_) {
      return 100.0f;
    }

    const float stopAt = maxHeatStopTemp(avgTempC);
    if (maxTempC >= stopAt) {
      return 0.0f;
    }

    const float headroom = stopAt - maxTempC;
    if (headroom >= MAX_TEMP_HEADROOM_C) {
      return 100.0f;
    }

    return clampPercent((headroom / MAX_TEMP_HEADROOM_C) * 100.0f);
  }

  float allowanceFromAverage(float avgTempC) const {
    if (avgTempC >= targetTempC_) {
      return 0.0f;
    }

    const float below = targetTempC_ - avgTempC;
    if (below >= APPROACH_BAND_C) {
      return 100.0f;
    }

    return clampPercent((below / APPROACH_BAND_C) * 100.0f);
  }

  float heaterMaxDuty(float avgTempC) const {
    if (avgTempC >= targetTempC_) {
      return HEATER_MAX_DUTY_NEAR;
    }

    const float below = targetTempC_ - avgTempC;
    if (below > HEATER_COLD_BELOW_C) {
      return HEATER_MAX_DUTY_COLD;
    }
    if (below > HEATER_WARM_BELOW_C) {
      return HEATER_MAX_DUTY_MID;
    }
    return HEATER_MAX_DUTY_NEAR;
  }

  float heaterAllowancePercent(float avgTempC, float maxTempC) const {
    const float fromMax = allowanceFromMaxCorner(maxTempC, avgTempC);
    const float fromAvg = allowanceFromAverage(avgTempC);
    float allowance = fromMax < fromAvg ? fromMax : fromAvg;
    const float maxDuty = heaterMaxDuty(avgTempC);
    if (allowance > maxDuty) {
      allowance = maxDuty;
    }
    return allowance;
  }

  float applyHeaterRamp(float requestedDuty, float avgTempC, uint32_t nowMs) {
    const float maxDuty = heaterMaxDuty(avgTempC);
    if (requestedDuty > maxDuty) {
      requestedDuty = maxDuty;
    }

    if (lastHeaterUpdateMs_ > 0 && requestedDuty > heaterDutyPercent_) {
      const float dt = static_cast<float>(nowMs - lastHeaterUpdateMs_) / 1000.0f;
      const float maxUp = heaterDutyPercent_ + HEATER_SLEW_UP_PER_S * dt;
      if (requestedDuty > maxUp) {
        requestedDuty = maxUp;
      }
    }

    return requestedDuty;
  }

  uint8_t fanPwmForHeaterDemand() const {
    if (heaterDutyPercent_ <= 0.0f) {
      return 0;
    }

    const uint8_t span = FAN_HEAT_MAX_PWM - FAN_HEAT_MIN_PWM;
    return FAN_HEAT_MIN_PWM +
           static_cast<uint8_t>((heaterDutyPercent_ / 100.0f) * static_cast<float>(span));
  }

  uint8_t fanPwmForCornerSpread() const {
    if (cornerSpreadC_ <= SPREAD_DEADBAND_C) {
      return 0;
    }

    float spread = cornerSpreadC_;
    if (spread > SPREAD_FULL_MIX_C) {
      spread = SPREAD_FULL_MIX_C;
    }

    const float t =
        (spread - SPREAD_DEADBAND_C) / (SPREAD_FULL_MIX_C - SPREAD_DEADBAND_C);
    const uint8_t mixMax = fanMixMax_;
    const uint8_t mixMin = FAN_MIX_MIN_PWM;
    const uint8_t span = mixMax > mixMin ? mixMax - mixMin : 0;
    return mixMin + static_cast<uint8_t>(t * static_cast<float>(span));
  }

  void applyHeaterBurst(uint32_t nowMs) {
    if (heaterDutyPercent_ <= 0.0f) {
      forceHeaterOff();
      return;
    }

    if (nowMs - heaterCycleStartMs_ >= HEATER_CYCLE_MS) {
      heaterCycleStartMs_ = nowMs;
    }

    const float onFraction = heaterDutyPercent_ / 100.0f;
    const uint32_t onTimeMs = static_cast<uint32_t>(HEATER_CYCLE_MS * onFraction);
    const bool shouldHeat = (nowMs - heaterCycleStartMs_) < onTimeMs;

    if (shouldHeat != heaterOn_) {
      heaterOn_ = shouldHeat;
      digitalWrite(HEATER_PIN, heaterOn_ ? HIGH : LOW);
    }
  }

  void applyFan() {
    if (isIdle()) {
      if (!sensorWarmValid_ || lastMaxTempC_ >= IDLE_AUTO_FAN_OFF_TEMP_C) {
        writeFan(FAN_MAX_PWM);
      } else if (fanIdleOverride_) {
        writeFan(FAN_IDLE_PWM);
      } else {
        writeFan(0);
      }
      return;
    }

    if (failSafeActive_ || cutoffActive_) {
      writeFan(FAN_MAX_PWM);
      return;
    }

    uint8_t duty = fanPwmForHeaterDemand();
    const uint8_t mixFan = fanPwmForCornerSpread();
    if (mixFan > duty) {
      duty = mixFan;
    }

    if (lastMaxTempC_ > targetTempC_ && duty < FAN_MAX_PWM) {
      duty = FAN_MAX_PWM;
    }

    writeFan(duty);
  }

  PidController pid_;
  PidAutotuner autotuner_;
  float targetTempC_;
  float heaterDutyPercent_;
  float heaterAllowancePercent_;
  float cornerSpreadC_;
  float lastMaxTempC_;
  uint8_t fanPwm_;
  uint8_t fanMixMax_;
  bool adaptiveEnabled_;
  bool fanIdleOverride_;
  bool sensorWarmValid_;
  bool cutoffActive_;
  bool failSafeActive_;
  uint32_t heaterCycleStartMs_;
  uint32_t lastHeaterUpdateMs_;
  bool heaterOn_;
};