#include <OneWire.h>
#include <DallasTemperature.h>
#include <PID_v1.h>

// ---- Pinnen ----
#define ONE_WIRE_BUS 2
#define FAN_PWM_PIN 3    // PWM-capable
#define PTC1_PWM_PIN 5   // PWM-capable
#define PTC2_PWM_PIN 6   // PWM-capable

// ---- DS18B20 ----
OneWire oneWire(ONE_WIRE_BUS);
DallasTemperature sensors(&oneWire);
DeviceAddress addr1, addr2;

// ---- Doelen / limieten ----
double setpoint1 = 60.0;   // Doeltemp zone 1 (°C)
double setpoint2 = 60.0;   // Doeltemp zone 2 (°C)
const double MAX_SAFE_TEMP = 90.0; // Noodstop (°C)
const double MIN_VALID = -20.0, MAX_VALID = 125.0; // DS18B20 bereik

// ---- Metingen & filtering ----
double t1 = NAN, t2 = NAN;
double t1_f = NAN, t2_f = NAN;
const double ALPHA = 0.25; // smoothing (lager = meer filtering)

// ---- PID voor PTC’s ----
double in1, out1, in2, out2;
// Tuning startwaarden (conservatief)
double Kp1=18.0, Ki1=0.4, Kd1=18.0;
double Kp2=18.0, Ki2=0.4, Kd2=18.0;

PID pid1(&in1, &out1, &setpoint1, Kp1, Ki1, Kd1, DIRECT);
PID pid2(&in2, &out2, &setpoint2, Kp2, Ki2, Kd2, DIRECT);

// ---- Ventilatorcurve ----
const double FAN_ON_AT = 45.0;     // start ventilator ondersteund bij hogere target
const double FAN_FULL_AT = 70.0;
const uint8_t FAN_MIN_PWM = 80;

// ---- Soft-start / ramp ----
const uint8_t RAMP_STEP = 6; // per cycle (0..255), pas aan voor zachtere of snellere ramp

// ---- Limits ----
const uint8_t MAX_PWM_GLOBAL = 255;    // fysieke limiet
const uint8_t MAX_PWM_NEAR_SETPOINT = 180; // begrenzing dicht bij setpoint (prevent jitter)
const double NEAR_SP_THRESHOLD = 0.6;  // °C binnen setpoint om begrenzing toe te passen

// ---- Hulpfuncties ----
uint8_t clamp255(double x) {
  if (x < 0) return 0;
  if (x > 255) return 255;
  return (uint8_t)(x + 0.5);
}

uint8_t fanCurve(double hottest) {
  if (isnan(hottest)) return 0;
  if (hottest <= FAN_ON_AT) return 0;
  if (hottest >= FAN_FULL_AT) return 255;
  double frac = (hottest - FAN_ON_AT) / (FAN_FULL_AT - FAN_ON_AT);
  uint8_t pwm = (uint8_t)(frac * 255.0);
  if (pwm > 0 && pwm < FAN_MIN_PWM) pwm = FAN_MIN_PWM;
  return pwm;
}

bool validTemp(double t) {
  return (!isnan(t) && t > MIN_VALID && t < MAX_VALID);
}

void safeOffAll() {
  analogWrite(FAN_PWM_PIN, 0);
  analogWrite(PTC1_PWM_PIN, 0);
  analogWrite(PTC2_PWM_PIN, 0);
}

// ---- Setup ----
void setup() {
  Serial.begin(115200);
  pinMode(FAN_PWM_PIN, OUTPUT);
  pinMode(PTC1_PWM_PIN, OUTPUT);
  pinMode(PTC2_PWM_PIN, OUTPUT);
  safeOffAll();

  sensors.begin();
  sensors.setResolution(12);

  if (!sensors.getAddress(addr1, 0)) Serial.println("⚠️ DS18B20 #1 niet gevonden");
  if (!sensors.getAddress(addr2, 1)) Serial.println("⚠️ DS18B20 #2 niet gevonden");

  pid1.SetOutputLimits(0, MAX_PWM_GLOBAL);
  pid2.SetOutputLimits(0, MAX_PWM_GLOBAL);
  pid1.SetSampleTime(2000); // ms - langere sampletijd voor traag systeem
  pid2.SetSampleTime(2000);
  pid1.SetMode(AUTOMATIC);
  pid2.SetMode(AUTOMATIC);
}

// ---- Loop ----
unsigned long lastRead = 0;
const unsigned long READ_PERIOD = 2000; // ms

// Huidige PWM waarde (voor ramp)
uint8_t currentPTC1 = 0;
uint8_t currentPTC2 = 0;

void loop() {
  unsigned long now = millis();

  if (now - lastRead >= READ_PERIOD) {
    lastRead = now;
    sensors.requestTemperatures(); // blokkeert ~750ms op 12-bit

    t1 = sensors.getTempC(addr1);
    t2 = sensors.getTempC(addr2);

    if (!validTemp(t1)) t1 = NAN;
    if (!validTemp(t2)) t2 = NAN;

    if (isnan(t1_f)) t1_f = t1; else if (!isnan(t1)) t1_f = ALPHA*t1 + (1-ALPHA)*t1_f;
    if (isnan(t2_f)) t2_f = t2; else if (!isnan(t2)) t2_f = ALPHA*t2 + (1-ALPHA)*t2_f;

    double hottest = max(t1_f, t2_f);

    // Veiligheid: noodstop bij extreem hoge temp of beide sensoren ongeldig
    if ((validTemp(hottest) && hottest >= MAX_SAFE_TEMP) || (!validTemp(t1_f) && !validTemp(t2_f))) {
      Serial.println("🚨 VEILIGHEIDSSTOP: te heet of sensoren ongeldig");
      safeOffAll();
      delay(1000);
      return;
    }

    // PID invoer
    in1 = validTemp(t1_f) ? t1_f : setpoint1;
    in2 = validTemp(t2_f) ? t2_f : setpoint2;

    if (!validTemp(t1_f)) out1 = 0; else pid1.Compute();
    if (!validTemp(t2_f)) out2 = 0; else pid2.Compute();

    // Begrenzing dicht bij setpoint om jitter te voorkomen
    if (validTemp(t1_f) && fabs(setpoint1 - t1_f) < NEAR_SP_THRESHOLD) out1 = min(out1, (double)MAX_PWM_NEAR_SETPOINT);
    if (validTemp(t2_f) && fabs(setpoint2 - t2_f) < NEAR_SP_THRESHOLD) out2 = min(out2, (double)MAX_PWM_NEAR_SETPOINT);

    // Soft-start / ramp: stap richting gewenst output
    uint8_t target1 = clamp255(out1);
    uint8_t target2 = clamp255(out2);

    if (currentPTC1 < target1) currentPTC1 = min((int)currentPTC1 + RAMP_STEP, target1);
    else if (currentPTC1 > target1) currentPTC1 = max((int)currentPTC1 - RAMP_STEP, target1);

    if (currentPTC2 < target2) currentPTC2 = min((int)currentPTC2 + RAMP_STEP, target2);
    else if (currentPTC2 > target2) currentPTC2 = max((int)currentPTC2 - RAMP_STEP, target2);

    analogWrite(PTC1_PWM_PIN, currentPTC1);
    analogWrite(PTC2_PWM_PIN, currentPTC2);

    uint8_t fanPWM = fanCurve(hottest);
    analogWrite(FAN_PWM_PIN, fanPWM);

    // Debug
    Serial.print("T1="); Serial.print(t1_f);
    Serial.print("  T2="); Serial.print(t2_f);
    Serial.print("  targetPTC1="); Serial.print(target1);
    Serial.print("  curPTC1="); Serial.print(currentPTC1);
    Serial.print("  targetPTC2="); Serial.print(target2);
    Serial.print("  curPTC2="); Serial.print(currentPTC2);
    Serial.print("  FAN="); Serial.println(fanPWM);
  }
}