/*
  PID + feedforward baseline voor PTC's
  - Volle bak tot 45°C
  - Lineair verminderen tussen 45..60°C (baseline)
  - PID zorgt voor fijnregeling rond 60°C (REVERSE)
  - Twee DS18B20 sensoren, twee PTC outputs, 1 ventilator
*/

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

// ---- pinnen ----
#define ONE_WIRE_BUS 2
#define FAN_PWM_PIN 3
#define PTC1_PWM_PIN 5
#define PTC2_PWM_PIN 6

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

// ---- setpoints / limieten ----
double setpoint = 60.0;          // doel (°C) voor beide zones
const double SAFE_MAX_TEMP = 90.0;
const double MIN_VALID = -20.0, MAX_VALID = 125.0;

// ---- filtering (EMA) ----
const double EMA_ALPHA = 0.25;   // 0..1 (lager = sterker filter)
double t1_raw = NAN, t2_raw = NAN;
double t1_f = NAN, t2_f = NAN;

// ---- PID variabelen ----
double in1=0, out1=0, in2=0, out2=0;
// startwaarden tuning — pas aan bij testen
double Kp = 6.0, Ki = 0.25, Kd = 8.0;

PID pid1(&in1, &out1, &setpoint, Kp, Ki, Kd, REVERSE);
PID pid2(&in2, &out2, &setpoint, Kp, Ki, Kd, REVERSE);

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

// ---- hulpfuncties ----
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);
}

// baseline: 255 voor t <= 45, 0 voor t >= 60, lineair ertussen
int baselinePowerForTemp(double t) {
  if (!validTemp(t)) return 0;
  if (t <= 45.0) return 255;
  if (t >= 60.0) return 0;
  double frac = (60.0 - t) / (60.0 - 45.0); // 1 at t=45, 0 at t=60
  return (int)(frac * 255.0 + 0.5);
}

// ventilatorcurve op warmste zone
uint8_t fanCurve(double hottest) {
  if (isnan(hottest)) return 0;
  if (hottest <= 45.0) return 0;
  if (hottest >= 70.0) return 255;
  double frac = (hottest - 45.0) / (70.0 - 45.0);
  return (uint8_t)(frac * 255.0 + 0.5);
}

void printHeader() {
  Serial.println("time, t1, t2, t1_f, t2_f, base1, base2, pid1, pid2, out1, out2, fan");
}

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");

  // PID setup
  pid1.SetOutputLimits(0, 255);
  pid2.SetOutputLimits(0, 255);
  pid1.SetSampleTime(2000); // 2 s
  pid2.SetSampleTime(2000);
  pid1.SetMode(AUTOMATIC);
  pid2.SetMode(AUTOMATIC);

  // kleine vertraging en header
  delay(300);
  printHeader();
}

void loop() {
  unsigned long now = millis();
  if (now - lastRead < READ_PERIOD) return;
  lastRead = now;

  // 1) Lees sensoren (blocking call)
  sensors.requestTemperatures();
  t1_raw = sensors.getTempC(addr1);
  t2_raw = sensors.getTempC(addr2);

  if (!validTemp(t1_raw)) t1_raw = NAN;
  if (!validTemp(t2_raw)) t2_raw = NAN;

  // 2) EMA filtering
  if (isnan(t1_f)) t1_f = t1_raw; else if (!isnan(t1_raw)) t1_f = EMA_ALPHA * t1_raw + (1.0 - EMA_ALPHA) * t1_f;
  if (isnan(t2_f)) t2_f = t2_raw; else if (!isnan(t2_raw)) t2_f = EMA_ALPHA * t2_raw + (1.0 - EMA_ALPHA) * t2_f;

  double hottest = max(t1_f, t2_f);

  // 3) Safety
  if ((validTemp(hottest) && hottest >= SAFE_MAX_TEMP) || (!validTemp(t1_f) && !validTemp(t2_f))) {
    Serial.println("🚨 VEILIGHEIDSSTOP!");
    safeOffAll();
    delay(1000);
    return;
  }

  // 4) PID inputs: gebruik gefilterde temps. Als sensor ongeldig: set input = setpoint
  in1 = validTemp(t1_f) ? t1_f : setpoint;
  in2 = validTemp(t2_f) ? t2_f : setpoint;

  // 5) Compute PID (PID library doet intern anti-windup via SetOutputLimits maar let op tuning)
  if (validTemp(t1_f)) pid1.Compute(); else out1 = 0;
  if (validTemp(t2_f)) pid2.Compute(); else out2 = 0;

  // 6) Baseline (feedforward) en final output: NEVER go below baseline
  int base1 = baselinePowerForTemp(t1_f);
  int base2 = baselinePowerForTemp(t2_f);

  // final output is max(pid_output, baseline)
  int finalOut1 = max((int)round(out1), base1);
  int finalOut2 = max((int)round(out2), base2);

  // schrijf PWM
  analogWrite(PTC1_PWM_PIN, constrain(finalOut1, 0, 255));
  analogWrite(PTC2_PWM_PIN, constrain(finalOut2, 0, 255));
  analogWrite(FAN_PWM_PIN, fanCurve(hottest));

  // 7) Debug (CSV-achtig)
  Serial.print(millis()); Serial.print(", ");
  Serial.print(t1_raw); Serial.print(", ");
  Serial.print(t2_raw); Serial.print(", ");
  Serial.print(t1_f); Serial.print(", ");
  Serial.print(t2_f); Serial.print(", ");
  Serial.print(base1); Serial.print(", ");
  Serial.print(base2); Serial.print(", ");
  Serial.print(out1); Serial.print(", ");
  Serial.print(out2); Serial.print(", ");
  Serial.print(finalOut1); Serial.print(", ");
  Serial.print(finalOut2); Serial.print(", ");
  Serial.println(fanCurve(hottest));
}