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

// Pin definities
#define ONE_WIRE_BUS 2        // DS18B20 data pin
#define PTC1_PIN 3            // PWM pin voor PTC element 1
#define PTC2_PIN 5            // PWM pin voor PTC element 2
#define FAN1_PIN 6            // PWM pin voor ventilator 1 (achter PTC)
#define FAN2_PIN 9            // PWM pin voor ventilator 2 (circulatie)

// OneWire setup voor DS18B20 sensoren
OneWire oneWire(ONE_WIRE_BUS);
DallasTemperature sensors(&oneWire);

// Sensor adressen (deze moeten worden ingevuld na het scannen)
DeviceAddress sensor1, sensor2, sensor3, sensor4;

// PID variabelen voor beide PTC elementen
double setpoint = 60.0;           // Gewenste temperatuur
double input1, output1;           // PTC1 PID variabelen
double input2, output2;           // PTC2 PID variabelen

// PID parameters (deze kunnen worden afgesteld)
double Kp = 2.0, Ki = 5.0, Kd = 1.0;

// PID controllers
PID pid1(&input1, &output1, &setpoint, Kp, Ki, Kd, DIRECT);
PID pid2(&input2, &output2, &setpoint, Kp, Ki, Kd, DIRECT);

// Temperatuur variabelen
float temp1, temp2, temp3, temp4;
float avgTemp = 0;

// Fan control variabelen
int fan1Speed = 0;    // Ventilator achter PTC
int fan2Speed = 100;  // Circulatie ventilator (constant laag)

// Timing variabelen
unsigned long lastTempRead = 0;
unsigned long lastPIDCompute = 0;
unsigned long lastSerialOutput = 0;
const unsigned long tempInterval = 1000;      // Temperatuur lezen elke seconde
const unsigned long pidInterval = 100;        // PID berekening elke 100ms
const unsigned long serialInterval = 2000;    // Serial output elke 2 seconden

void setup() {
  Serial.begin(115200);
  Serial.println("PTC Verwarming Controlesysteem gestart");
  
  // Pin modes instellen
  pinMode(PTC1_PIN, OUTPUT);
  pinMode(PTC2_PIN, OUTPUT);
  pinMode(FAN1_PIN, OUTPUT);
  pinMode(FAN2_PIN, OUTPUT);
  
  // DS18B20 sensoren initialiseren
  sensors.begin();
  
  // Aantal sensoren detecteren
  int deviceCount = sensors.getDeviceCount();
  Serial.print("Aantal DS18B20 sensoren gevonden: ");
  Serial.println(deviceCount);
  
  // Sensor adressen ophalen
  if (sensors.getAddress(sensor1, 0)) Serial.println("Sensor 1 gevonden");
  if (sensors.getAddress(sensor2, 1)) Serial.println("Sensor 2 gevonden");
  if (sensors.getAddress(sensor3, 2)) Serial.println("Sensor 3 gevonden");
  if (sensors.getAddress(sensor4, 3)) Serial.println("Sensor 4 gevonden");
  
  // Sensor resolutie instellen (12-bit = 0.0625°C precisie)
  sensors.setResolution(12);
  
  // PID controllers instellen
  pid1.SetMode(AUTOMATIC);
  pid1.SetOutputLimits(0, 255);
  pid1.SetSampleTime(pidInterval);
  
  pid2.SetMode(AUTOMATIC);
  pid2.SetOutputLimits(0, 255);
  pid2.SetSampleTime(pidInterval);
  
  // Circulatie ventilator starten op lage snelheid
  analogWrite(FAN2_PIN, fan2Speed);
  
  Serial.println("Systeem geinitialiseerd. Opwarmen naar 60°C...");
}

void loop() {
  unsigned long currentTime = millis();
  
  // Temperaturen lezen
  if (currentTime - lastTempRead >= tempInterval) {
    readTemperatures();
    lastTempRead = currentTime;
  }
  
  // PID berekeningen en output updates
  if (currentTime - lastPIDCompute >= pidInterval) {
    computePID();
    updateOutputs();
    lastPIDCompute = currentTime;
  }
  
  // Serial monitoring
  if (currentTime - lastSerialOutput >= serialInterval) {
    printStatus();
    lastSerialOutput = currentTime;
  }
  
  // Safety check - maximale temperatuur
  safetyCheck();
}

void readTemperatures() {
  sensors.requestTemperatures();
  
  temp1 = sensors.getTempC(sensor1);
  temp2 = sensors.getTempC(sensor2);
  temp3 = sensors.getTempC(sensor3);
  temp4 = sensors.getTempC(sensor4);
  
  // Validatie van temperatuurmetingen
  if (temp1 == DEVICE_DISCONNECTED_C) temp1 = 0;
  if (temp2 == DEVICE_DISCONNECTED_C) temp2 = 0;
  if (temp3 == DEVICE_DISCONNECTED_C) temp3 = 0;
  if (temp4 == DEVICE_DISCONNECTED_C) temp4 = 0;
  
  // Gemiddelde temperatuur berekenen (alleen geldige sensoren)
  int validSensors = 0;
  avgTemp = 0;
  
  if (temp1 > 0) { avgTemp += temp1; validSensors++; }
  if (temp2 > 0) { avgTemp += temp2; validSensors++; }
  if (temp3 > 0) { avgTemp += temp3; validSensors++; }
  if (temp4 > 0) { avgTemp += temp4; validSensors++; }
  
  if (validSensors > 0) {
    avgTemp /= validSensors;
  }
}

void computePID() {
  // Input voor PID controllers instellen
  input1 = avgTemp;  // Beide PTC's gebruiken gemiddelde temperatuur
  input2 = avgTemp;
  
  // PID berekeningen uitvoeren
  pid1.Compute();
  pid2.Compute();
}

void updateOutputs() {
  // PTC elementen aansturen
  analogWrite(PTC1_PIN, (int)output1);
  analogWrite(PTC2_PIN, (int)output2);
  
  // Ventilator 1 snelheid aanpassen op basis van PTC output
  // Meer verwarming = hogere ventilator snelheid voor betere warmteverdeling
  fan1Speed = map((output1 + output2) / 2, 0, 255, 80, 255);
  fan1Speed = constrain(fan1Speed, 0, 255);
  analogWrite(FAN1_PIN, fan1Speed);
  
  // Ventilator 2 snelheid aanpassen op basis van temperatuurverschil
  // Meer circulatie als er grote temperatuurverschillen zijn
  float tempRange = getTemperatureRange();
  if (tempRange > 5.0) {
    fan2Speed = 150;  // Verhoog circulatie bij grote verschillen
  } else if (tempRange > 2.0) {
    fan2Speed = 120;
  } else {
    fan2Speed = 100;  // Minimale circulatie
  }
  analogWrite(FAN2_PIN, fan2Speed);
}

float getTemperatureRange() {
  float minTemp = 100;
  float maxTemp = -100;
  
  if (temp1 > 0) { minTemp = min(minTemp, temp1); maxTemp = max(maxTemp, temp1); }
  if (temp2 > 0) { minTemp = min(minTemp, temp2); maxTemp = max(maxTemp, temp2); }
  if (temp3 > 0) { minTemp = min(minTemp, temp3); maxTemp = max(maxTemp, temp3); }
  if (temp4 > 0) { minTemp = min(minTemp, temp4); maxTemp = max(maxTemp, temp4); }
  
  return maxTemp - minTemp;
}

void safetyCheck() {
  // Noodstop bij oververhitting
  if (temp1 > 70 || temp2 > 70 || temp3 > 70 || temp4 > 70) {
    analogWrite(PTC1_PIN, 0);
    analogWrite(PTC2_PIN, 0);
    analogWrite(FAN1_PIN, 255);  // Ventilatoren op maximum
    analogWrite(FAN2_PIN, 255);
    
    Serial.println("WAARSCHUWING: Oververhitting gedetecteerd! PTC elementen uitgeschakeld.");
    
    // Wacht tot temperatuur daalt
    while (temp1 > 65 && temp2 > 65 && temp3 > 65 && temp4 > 65) {
      sensors.requestTemperatures();
      temp1 = sensors.getTempC(sensor1);
      temp2 = sensors.getTempC(sensor2);
      temp3 = sensors.getTempC(sensor3);
      temp4 = sensors.getTempC(sensor4);
      delay(1000);
    }
    
    Serial.println("Temperatuur gedaald. Systeem hervat.");
    analogWrite(FAN1_PIN, fan1Speed);
    analogWrite(FAN2_PIN, fan2Speed);
  }
}

void printStatus() {
  Serial.println("=== Status Rapport ===");
  Serial.print("Setpoint: "); Serial.print(setpoint); Serial.println("°C");
  Serial.print("Gem. Temp: "); Serial.print(avgTemp, 1); Serial.println("°C");
  Serial.print("Temp 1: "); Serial.print(temp1, 1); Serial.println("°C");
  Serial.print("Temp 2: "); Serial.print(temp2, 1); Serial.println("°C");
  Serial.print("Temp 3: "); Serial.print(temp3, 1); Serial.println("°C");
  Serial.print("Temp 4: "); Serial.print(temp4, 1); Serial.println("°C");
  Serial.print("PTC1 Output: "); Serial.print((int)output1); Serial.println("/255");
  Serial.print("PTC2 Output: "); Serial.print((int)output2); Serial.println("/255");
  Serial.print("Fan1 Speed: "); Serial.print(fan1Speed); Serial.println("/255");
  Serial.print("Fan2 Speed: "); Serial.print(fan2Speed); Serial.println("/255");
  Serial.print("Temp Range: "); Serial.print(getTemperatureRange(), 1); Serial.println("°C");
  Serial.println("=====================");
}

// Functie om setpoint aan te passen via Serial
void serialEvent() {
  if (Serial.available()) {
    String input = Serial.readString();
    input.trim();
    
    if (input.startsWith("SET:")) {
      float newSetpoint = input.substring(4).toFloat();
      if (newSetpoint >= 20 && newSetpoint <= 65) {
        setpoint = newSetpoint;
        Serial.print("Nieuwe setpoint: ");
        Serial.print(setpoint);
        Serial.println("°C");
      } else {
        Serial.println("Setpoint moet tussen 20 en 65°C zijn");
      }
    }
    
    if (input.startsWith("PID:")) {
      // Format: PID:Kp,Ki,Kd (bijv. PID:2.0,5.0,1.0)
      int comma1 = input.indexOf(',');
      int comma2 = input.indexOf(',', comma1 + 1);
      
      if (comma1 > 0 && comma2 > 0) {
        Kp = input.substring(4, comma1).toFloat();
        Ki = input.substring(comma1 + 1, comma2).toFloat();
        Kd = input.substring(comma2 + 1).toFloat();
        
        pid1.SetTunings(Kp, Ki, Kd);
        pid2.SetTunings(Kp, Ki, Kd);
        
        Serial.println("PID parameters bijgewerkt:");
        Serial.print("Kp: "); Serial.println(Kp);
        Serial.print("Ki: "); Serial.println(Ki);
        Serial.print("Kd: "); Serial.println(Kd);
      }
    }
  }
}