#include "esp_wifi.h"
#include <WiFi.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
#include <PubSubClient.h>
#include "Adafruit_DS248x.h"
//Je kunt de fout in de bibliotheek zelf simpel corrigeren. 
//De compiler geeft je eigenlijk al de oplossing in de foutmelding:
//Open het bestand: C:\Users\tthde\Documents\Arduino\libraries\ESPAsyncWebServer\src\WebAuthentication.cpp
//Zoek naar de regels 74, 75 en 76.
//Verwijder de toevoeging _ret van de drie functienamen.
//Verander mbedtls_md5_starts_ret naar mbedtls_md5_startsVerander mbedtls_md5_update_ret naar mbedtls_md5_updateVerander mbedtls_md5_finish_ret naar mbedtls_md5_finish


#define DS18B20_FAMILY_CODE 0x28
#define DS18B20_CMD_CONVERT_T 0x44
#define DS18B20_CMD_MATCH_ROM 0x55
#define DS18B20_CMD_READ_SCRATCHPAD 0xBE

#define temp_addr_read 0x4F //DS1721
#define temp_addr_write 0x4F //DS1721

// --- NETWERK CONFIGURATIE ---
const char* ssid = "CAM_LINK";
const char* password = "DENWEP128BITSDEN46WIRELESSVIGOR";

const char* mqtt_server = "192.168.200.26";
const int mqttPort = 1883;
const char* mqttUser = "vlc_viewer";
const char* mqttPassword = "vlc_viewer12";
const char* mqttClientId = "MeterKastController";

const char* mqttTopicBelVoorTest = "BelVoorTest";
const char* mqttTopicRestarCamera = "RestartCamera";
const char* mqttTopicDisableVAlarm = "DisableVAlarm";      
const char* mqttTopicEnableVAlarm = "EnableVAlarm";
const char* mqttTopicActivateFan = "ActivateFanMeterkast";      
const char* mqttTopicDeactivateFan = "DeactivateFanMeterkast";
const char* mqttTopicActivateBuzzer = "ActivateAlarmBuzzer";      
const char* mqttTopicDeactivateBuzzer = "DeactivateAlarmBuzzer"; 
const char* mqttTopicAlarmLampAan = "AlarmLampAan";      
const char* mqttTopicAlarmLampUit = "AlarmLAmpUit";

const char* mqttTopicDalVerbruikt = "DalVerbruikt_kwh";
const char* mqttTopicPiekVerbruikt = "PiekVerbruikt_kwh";
const char* mqttTopicDalGeleverd = "DalGeleverd_kwh";
const char* mqttTopicPiekGeleverd = "PiekGeleverd_kwh";
const char* mqttTopicHuidigVerbruik = "HuidigVerbruik_kw";
const char* mqttTopicHuidigGeleverd = "HuidigGeleverd_kw";
const char* mqttTopicGasTotaal = "gas_totaal_m3";

const char* mqttTopicTempMeterKast = "TempMeterKast";
const char* mqttTopicTempThermostaat = "TempThermostaat";
const char* mqttTopicTempBuiten = "TempBuiten";
const char* mqttTopicTempKamer = "TempKamer";
const char* mqttTopicTempOverloop = "TempOverloop";
const char* mqttTopicTempMeterKastController = "TempMeterKastController";

const char* mqttTopicHcProtected = "HcProtected";
const char* mqttTopicHcAlarm = "HcAlarm";
const char* mqttTopicHcVAlarm = "HcVAlarm";
const char* mqttTopicHcAlarmBuzzer = "HcAlarmBuzzer";
const char* mqttTopicHcAlarmLamp = "HCLampAlarm";
const char* mqttTopicFanMeterkast = "FanMeterKast";
const char* mqttTopicAlarmStatus = "AlarmStatus";
const char* mqttTopicP1Line = "P1Line";
const char* mqttTopicP1InvalidLine = "P1InvalidLine";
const char* mqttTopicSerialOutput = "SerialOutput";

const char* mqtt_temperature_topic = "BrievenbusTemperatuur";
const char* mqtt_bel_topic = "BelVoor";
const char* mqtt_bel_state_topic = "BelVoorStatus";
const char* mqtt_test_topic = "BrievenbusTest";
const char* mqtt_debug_topic = "BrievenbusDebug";
const char* mqtt_box_state_topic = "BrievenbusStatus";
const char* mqtt_box_opened_topic = "BrievenbusOpen";
const char* mqtt_box_closed_topic = "BrievenbusDicht";
const char* mqtt_info_topic = "BrievenbusInfo";
char result[50];
unsigned long lastTimeHost = 0;
int result_length;


// --- PIN DEFINITIES ---
const int PIN_SDA = 21;
const int PIN_SCL = 22;
const int PIN_BUZZER = 32;
const int PIN_VALARM = 27;
const int PIN_FAN = 33;
const int PIN_ALARMLAMP = 18;
const int PIN_RELAIS_BEL  = 19;
const int PIN_INPUT_DOOR  = 15;     // T1_DOOR: DoorsOpen  DR
const int PIN_INPUT_ACT   = 13;      // T2_ACT: AlarmActivated (Extern signaal) SL
const int PIN_RELAIS_CAM  = 23;
const int PIN_P1_REQ      = 17;
const int PIN_WATER_SENSOR = 25;
const int PIN_BEL      = 12;
const int PIN_BOX = 14;

volatile byte interruptCounterBel = 0;
volatile byte interruptCounterBox = 0;


void IRAM_ATTR handleInterruptbel() {
  interruptCounterBel++;
}

void IRAM_ATTR handleInterruptbox() {
  interruptCounterBox++;
}
int numberOfInterruptsBel = 0;
int numberOfInterruptsBox = 0;
// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTimeBel = 0;  // the last time the bel knob was handled
unsigned long lastDebounceTimeBox = 0;  // the last time the bel knob was handled
#define debounceDelayBel 1000    // the debounce time;
#define debounceDelayBox 1000    // the debounce time;
float temperature;
unsigned long temp_readoutTime = 0;
int boxState = 0;
unsigned long box_readoutTime = 0;
unsigned long bel_readoutTime = 0;
int belState = 0;
int prev_boxState = -1;
int prev_belState = -1;

unsigned long info_Time = 0;



bool wifiRestarted = false;
bool wifiNeedsRestart = false;


// --- ALARM STATES ---
enum AlarmState { S0_Deactivated, S1_Activated, S2_TriggeredActive, S3_TriggeredInactive };
AlarmState currentState = S0_Deactivated;
unsigned long s2StartTime = 0;
const unsigned long s2Timeout = 30 * 60 * 1000; // 30 minuten in ms
bool alarmInitialized = false;

// --- 1-WIRE ADRESSEN (Vertaald uit Python dict) ---
uint8_t addr_Kast[]       = { 0x28, 0xAA, 0x3D, 0x64, 0x40, 0x14, 0x01, 0xc3 }; // TempMeterKast
uint8_t addr_Thermostaat[] = { 0x28, 0xAA, 0x22, 0x64, 0x40, 0x14, 0x01, 0xd0 }; // TempThermostaat
uint8_t addr_Buiten[]      = { 0x28, 0xAA, 0xB6, 0x4F, 0x40, 0x14, 0x01, 0x9c }; // TempBuiten
uint8_t addr_Kamer[]       = { 0x28, 0xF0, 0x33, 0x79, 0x97, 0x11, 0x03, 0x13 }; // TempKamer
uint8_t addr_Overloop[]    = { 0x28, 0x49, 0xA0, 0x79, 0x97, 0x14, 0x03, 0x92 }; // TempOverloop
uint8_t addr_Pi[]          = { 0x28, 0x85, 0x4B, 0xB8, 0x0B, 0x00, 0x00, 0xCA }; // TempRaspberryPi
// Opmerking: De laatste byte (CRC) mag vaak 0x00 of 0xFF zijn als je ds.select() gebruikt, 
// de DS2482 handelt de adressering af.

// --- STATUS & TIMERS ---
unsigned long now;
unsigned long lastP1Read = 0;
unsigned long lastTempRead = 0;

unsigned long relBelTimer = 0;
bool relBelActive = false;
unsigned long relCamTimer = 0;
bool relCamActive = false;

int lastT1Door = HIGH, stableT1Door = HIGH;
int lastT2Act = HIGH, stableT2Act = HIGH;
unsigned long lastDbT1Door = 0, lastDbT2Act = 0;
const unsigned long dbDelay = 500; //500ms

// --- P1 DATA VARIABELEN ---
float elDalVerbruikt, elPiekVerbruikt, elDalGeleverd, elPiekGeleverd;
float elHuidigVerbruik, elHuidigGeleverd, elGas;


WiFiClient espClient;
bool WifiConnected = false;

PubSubClient mqttClient(espClient);
//AsyncWebServer server(80);
//WebSerial webSerial;
long lastReconnectAttempt = 0;
// Initialisatie DS2482 op standaard adres 0x18
Adafruit_DS248x ds;
// Timing constants
unsigned long vorigeMillis = 0;
const long meetInterval = 60000; // Elke minuut nieuwe meting
bool metingBezig = false;


bool belRequest = false; 
int fanRequest = -1;           //-1=no_request, 0=disable, 1=enable
int enableAlarmRequest = -1;   //-1=no_request, 0=disable, 1=enable
int buzzerRequest = -1;        //-1=no_request, 0=disable, 1=enable
int lampRequest = -1;        //-1=no_request, 0=disable, 1=enable
bool restartCameraRequest = false;
bool temp_initialised = false;
unsigned long lastWiFiCheck = 0;

// We maken een nieuwe klasse die 'Print' overneemt
class DualOutput : public Print {
  private:
    String mqttBuffer = ""; // Buffer om een volledige regel te bouwen
    String bootBuffer = "";      // Hier slaan we de logs op
    bool bufferFull = false;     // Om te voorkomen dat het geheugen volloopt
    const size_t maxBufferSize = 2048; // Maximaal 2KB aan opstartlogs

  public:
    // Dit is de magische functie waar alles doorheen gaat
    size_t write(uint8_t character) override {
      // 1. Schrijf naar de fysieke Seriële poort (USB)
      size_t s = Serial.write(character);

      // 2. Bouw de regel op voor MQTT
      if (character == '\n') {
        // Alleen versturen als er een verbinding is en de buffer niet leeg is
        if (mqttClient.connected() && mqttBuffer.length() > 0) {
          mqttClient.publish(mqttTopicSerialOutput, mqttBuffer.c_str());
        }
        mqttBuffer = ""; // Maak buffer leeg voor de volgende regel
      } else if (character != '\r') 
      { // Negeer 'carriage return'
        mqttBuffer += (char)character;
      }

      // Beperk de bufferlengte om geheugenproblemen te voorkomen
      if (mqttBuffer.length() > 150) mqttBuffer = ""; 

      // // 3. Als er wel iemand kijkt, stuur het direct
      // if (webSerial.getConnectionCount() > 0) {
      //   webSerial.write(character);
      // } else if (!bufferFull) {
      //   bootBuffer += (char)character;
      //   if (bootBuffer.length() >= maxBufferSize) bufferFull = true;
      // }
      return s;
    }

    // // Functie om de opgeslagen logs naar een nieuwe kijker te sturen
    // void flushBuffer() {
    //   if (bootBuffer.length() > 0) {
    //     webSerial.println("--- Opgeslagen opstartlogs ---");
    //     webSerial.print(bootBuffer);
    //     webSerial.println("--- Einde opstartlogs, live weergave gestart ---");
    //     bootBuffer = ""; // Maak leeg na versturen om RAM vrij te maken
    //   }
    // }
};

// Maak een object aan van je nieuwe klasse
DualOutput debuglog;

void I2CScanner()
{
  debuglog.println("I2C scanner. Scanning ...");
  byte count = 0;
  char buffer[50]; // Zorg dat de buffer groot genoeg is voor de hele tekst
 
  for (byte i = 1; i < 120; i++)
  {
    Wire.beginTransmission (i);
    if (Wire.endTransmission () == 0)
      {     
      snprintf(buffer, sizeof(buffer), "Found address: %d (0x%02X)", i, i);
      debuglog.println(buffer);
      count++;
      delay (1);  // maybe unneeded?
      } // end of good response
  } // end of for loop
  snprintf(buffer, sizeof(buffer), "Scan done. Found %d device(s).", count);
  debuglog.println(buffer);

}

void scanOneWireBus() {
    uint8_t adresBuffer[8]; // Buffer voor het 64-bits (8 bytes) ROM adres
    int aantalApparaten = 0;

    Serial.println("Scannen naar 1-Wire apparaten...");

    // Reset de interne zoek-pointer van de DS248x
    ds.OneWireSearchReset();

    // Blijf zoeken zolang er nieuwe apparaten worden gevonden
    while (ds.OneWireSearch(adresBuffer)) {
        aantalApparaten++;
        Serial.print("Gevonden apparaat ");
        Serial.print(aantalApparaten);
        Serial.print(": ROM = ");

        // Print het unieke adres in HEX-formaat
        for (int i = 0; i < 8; i++) {
            if (adresBuffer[i] < 16) Serial.print("0");
            Serial.print(adresBuffer[i], HEX);
            Serial.print(" ");
        }
        
        // Identificeer het type apparaat aan de hand van de Family Code (1e byte)
        Serial.print(" -> Type: ");
        if (adresBuffer[0] == 0x28) {
            Serial.println("DS18B20 Temperatuursensor");
        } else if (adresBuffer[0] == 0x10) {
            Serial.println("DS18S20 / DS1820 Sensor");
        } else {
            Serial.print("Onbekend (Family Code: 0x");
            Serial.print(adresBuffer[0], HEX);
            Serial.println(")");
        }
    }

    if (aantalApparaten == 0) {
        Serial.println("Geen 1-Wire apparaten gevonden op de bus.");
    } else {
        Serial.println("Scan voltooid.");
    }
}

void setupOta()
{
  debuglog.println("Setting up OTA");
  // Port defaults to 8266
  //ArduinoOTA.setPort(8266);

  // Hostname defaults to esp8266-[ChipID]
  ArduinoOTA.setHostname("MeterKastController");

  // No authentication by default
  ArduinoOTA.setPassword((const char *)"tdsadmin");

  ArduinoOTA.onStart([]() {
    debuglog.println("OTA Start");
  });
  ArduinoOTA.onEnd([]() {
    debuglog.println("\nOTA End");
  });
  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    debuglog.printf("OTA Progress: %u%%\r", (progress / (total / 100)));
  });
  ArduinoOTA.onError([](ota_error_t error) {
    debuglog.printf("OTA Error[%u]: ", error);
    if (error == OTA_AUTH_ERROR) debuglog.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR) debuglog.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR) debuglog.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR) debuglog.println("Receive Failed");
    else if (error == OTA_END_ERROR) debuglog.println("End Failed");
  });
  ArduinoOTA.begin();
  debuglog.println("Ready");
  debuglog.print("IP address: ");
  debuglog.println(WiFi.localIP());

}

void sendMqttStatus(const char* topic, const char* val) {
    mqttClient.publish(topic, val);
}

void sendAlarmStatus(const char* status)
{
  debuglog.println(status);
  mqttClient.publish(mqttTopicAlarmStatus, status);
}

void sendP1Line(const char* line)
{
  debuglog.println(line);
  mqttClient.publish(mqttTopicP1Line, line);
}

void sendP1InvalidLine(const char* line)
{
  debuglog.println(line);
  mqttClient.publish(mqttTopicP1InvalidLine, line);
}

// states:
// S0=AlarmDeactivated
// S1=AlarmActivated
// S2=AlarmTriggeredMosfetActive
// S3=AlarmTriggeredMosfetInactive
// The transitions happen based on the two 12V inputs: DoorsOpen (T1Door), and AlarmActivated(T2Act)
// S0 -> T1Door -> S0 (mosfet stays inactive)
// S0 -> T2Act and T1Door active -> S2 {mosfet becomes active, send topic HcProtected with value1, send topic HcAlarm with value 1)
// S0 -> T2Act and T1Door inactive -> S1 (mosfet  stays inactive, send topic HcProtected with value 1)
// S1 -> T1Door active -> S2 (mosfet becomes active,  send topic HcAlarm with value 1)
// S1 -> T2Act inactive -> S0 (mosfet stays inactive, send topic HcProtected with value 0)
// S2 -> T2Act inactive -> S0 (moset becomes inactive, send topic HcProtected with value 0, send topic HcAlarm with value 0)
// S2 -> Ater 30min -> S3 (mosfet becomes inactive)
// S3 -> T1Door inactive -> S1 (mosfet stays inactive, send topic HcAlarm with value 0)
// S3 -> T2Act inactive -> S0 (moset stays inactive, send topic HcProtected with value 0, send topic HcAlarm with value 0)

void SwitchVAlarm(bool aan)
{
  if (aan)
  {
    digitalWrite(PIN_VALARM, LOW);
    sendMqttStatus(mqttTopicHcVAlarm, "1");
  } else
  {
    digitalWrite(PIN_VALARM, HIGH);
    sendMqttStatus(mqttTopicHcVAlarm, "0");
  }
}

void SwitchBuzzer(bool aan)
{
  if (aan)
  {
    digitalWrite(PIN_BUZZER, LOW);
    sendMqttStatus(mqttTopicHcAlarmBuzzer, "1");
  } else
  {
    digitalWrite(PIN_BUZZER, HIGH);
    sendMqttStatus(mqttTopicHcAlarmBuzzer, "0");
  }
}

void SwitchAlarmLamp(bool aan)
{
  if (aan)
  {
    digitalWrite(PIN_ALARMLAMP, LOW);
    sendMqttStatus(mqttTopicHcAlarmLamp, "1");
  } else
  {
    digitalWrite(PIN_ALARMLAMP, HIGH);
    sendMqttStatus(mqttTopicHcAlarmLamp, "0");
  }
}

void SwitchFan(bool aan)
{
  if (aan)
  {
    digitalWrite(PIN_FAN, LOW);
    sendMqttStatus(mqttTopicFanMeterkast, "1");
  } else
  {
    digitalWrite(PIN_FAN, HIGH);
    sendMqttStatus(mqttTopicFanMeterkast, "0");
  }
}

void updateAlarmState() {
    bool T1Door = (stableT1Door == LOW); // LOW = 12V aanwezig (Input Active)
    bool T2Act = (stableT2Act == LOW);

    switch (currentState) {
        case S0_Deactivated:
            if (T2Act && !T1Door) {
                currentState = S1_Activated;
                sendAlarmStatus("<S0_Deactivated> =>Deuren dicht, SL_aan=> <S1_Activated>: Alarm aangezet, alarm lampjes aan");
                sendMqttStatus(mqttTopicHcProtected, "1");
                SwitchAlarmLamp(true);
            } else if (T2Act && T1Door) {
                currentState = S2_TriggeredActive;
                sendAlarmStatus("<S0_Deactivated =>Deuren open, SL_aan=> <S2_TriggeredActive>: Alarm getriggered");
                s2StartTime = millis();
                SwitchBuzzer(true);                
                sendMqttStatus(mqttTopicHcProtected, "1");
                sendMqttStatus(mqttTopicHcAlarm, "1");
            }
            break;
        case S1_Activated:
            if (T1Door) { //doors are opened
                currentState = S2_TriggeredActive;
                sendAlarmStatus("<S1_Activated> =>Deuren open=> <S2_TriggeredActive>: Alarm getriggered, Alarm lampjes uit");
                s2StartTime = millis();
                SwitchBuzzer(true);                 
                SwitchAlarmLamp(false);
                sendMqttStatus(mqttTopicHcAlarm, "1");
            } else if (!T2Act) {  //doors are closed
                currentState = S0_Deactivated;              
                sendAlarmStatus("<S1_Activated> =>Deuren dicht, SL_uit=> <S0_Deactivated>: Alarm uitgezet, Alarm lampjes uit");                                
                SwitchAlarmLamp(false);
                sendMqttStatus(mqttTopicHcProtected, "0");
                sendMqttStatus(mqttTopicHcAlarm, "0");
            }
            break;

        case S2_TriggeredActive:
            if (!T2Act) {
                currentState = S0_Deactivated;              
                sendAlarmStatus("<S2_TriggeredActive> =>SL_uit=> <S0_Deactivated>: Alarm uitgezet, Alarm lampjes uit");
                SwitchBuzzer(false);                 
                SwitchAlarmLamp(false);
                sendMqttStatus(mqttTopicHcProtected, "0");
                sendMqttStatus(mqttTopicHcAlarm, "0");
            } else if (millis() - s2StartTime >= s2Timeout) {                   
                if (T1Door)
                {               
                  currentState = S3_TriggeredInactive;      
                  sendAlarmStatus("<S2_TriggeredActive> =>Deuren open, SL_aan=> <S3_TriggeredInactive>: Buzzer na timeout uitgezet");
                } else
                {
                  currentState = S1_Activated;      
                  sendAlarmStatus("<S2_TriggeredActive> =>Deuren dicht, SL_aan=> <S1_Activated>: Buzzer na timeout uitgezet, Alarm weer aangezet,  Alarm lampjes aan, Alarm untriggered");
                  SwitchAlarmLamp(true);
                  sendMqttStatus(mqttTopicHcAlarm, "0");
                }
                SwitchBuzzer(false);
            }
            break;

        case S3_TriggeredInactive:
            if (!T1Door) {
                currentState = S1_Activated;                
                sendAlarmStatus("<S3_TriggeredInactive> =>Deuren dicht, SL_aan=> <S1_Activated>: Deuren dicht, Alarm weer aangezet,  Alarm lampjes aan, Alarm untriggered");
                SwitchAlarmLamp(true);
                sendMqttStatus(mqttTopicHcAlarm, "0");
            } else if (!T2Act) {
                currentState = S0_Deactivated;
                sendAlarmStatus("<S3_TriggeredInactive> =>Deuren open, SL_uit=> <S0_Deactivated>: Alarm uitgezet, Alarm lampjes aan");
                SwitchAlarmLamp(false);
                sendMqttStatus(mqttTopicHcProtected, "0");
                sendMqttStatus(mqttTopicHcAlarm, "0");
            }
            break;
    }
}

void callback(char* topic, byte* payload, unsigned int length) {
  debuglog.print("Topic received: ");
  debuglog.println(topic);
  if (strcmp(topic, mqttTopicBelVoorTest) == 0) {
    belRequest = true;    
  } else if (strcmp(topic, mqttTopicRestarCamera) == 0) {
    restartCameraRequest = true;
  } else if (strcmp(topic, mqttTopicDisableVAlarm) == 0) {
    enableAlarmRequest = 0;
  } else if (strcmp(topic, mqttTopicEnableVAlarm) == 0) {
    enableAlarmRequest = 1;
  } else if (strcmp(topic, mqttTopicActivateFan) == 0) {
    fanRequest = 1;
  } else if (strcmp(topic, mqttTopicDeactivateFan) == 0) {
    fanRequest = 0;
  } else if (strcmp(topic, mqttTopicActivateBuzzer) == 0) {
    buzzerRequest = 1;
  } else if (strcmp(topic, mqttTopicDeactivateBuzzer) == 0) {
    buzzerRequest = 0;
  } else if (strcmp(topic, mqttTopicAlarmLampAan) == 0) {
    lampRequest = 1;
  } else if (strcmp(topic, mqttTopicAlarmLampUit) == 0) {
    lampRequest = 0;
  }
}

void SetupTemperatureBel()
{
  // defaults are ok
  // continuous mode, 12bit resolution,  active high output for thermostat (not used)
    Wire.beginTransmission(temp_addr_write); 
    Wire.write(0x51);  //start convert
    Wire.endTransmission( true );
    delay(800); //time required for first conversion, otherwise initial temp will be wrong
}



void ReadTemperatureBel()
{
  int tempreg;
  int msb_data;
  int lsb_data;
  // Receives data from the Temperature Register 
  Wire.beginTransmission(temp_addr_write); 
  Wire.write(0xAA);  //switch to read register
  Wire.endTransmission( false );
  
  Wire.requestFrom(temp_addr_read, 2, true); 

  msb_data = Wire.read(); 
  lsb_data = Wire.read(); 

//  Debugprint("msb= ");
//  Debugprint(String(msb_data, HEX));
//  Debugprint(" lsb= ");
//  Debugprint(String(lsb_data, HEX));
  
  tempreg = msb_data;       
  tempreg= tempreg << 8;      
  tempreg |= lsb_data; 

//  Debugprint(" tempreg= ");
//  Debugprint(String(tempreg, HEX));

  int bit_set = tempreg & 0x8000;
  
  // two's complement
  if (bit_set > 0) 
  { //make a negative number
    temperature = -0.0625 * (float)((0xFFF - ((tempreg & 0xFFF0) >> 4)) + 1);
  } else
  {
    temperature = 0.0625 * (float)((tempreg & 0x7FF0) >> 4);
  }
//  Debugprint(" shifted= ");
//  Debugprint(String(((tempreg & 0x7FF0) >> 4), HEX));
//  Debugprint(" temperature= ");
//  Debugprintln(temperature);
}

void execute_bel_request()
{
    digitalWrite(PIN_RELAIS_BEL, LOW);
    sendAlarmStatus("<BelRequest>: PIN_RELAIS_BEL=>LOW");
    relBelTimer = millis();
    relBelActive = true;
    belRequest = false;
}

void handle_requests() {
  if (belRequest)
  {
    execute_bel_request();
  }
  if (restartCameraRequest)
  { 
    digitalWrite(PIN_RELAIS_CAM, LOW);
    sendAlarmStatus("<CameraRestartRequest>: PIN_RELAIS_CAM=>LOW");
    relCamTimer = millis();
    relCamActive = true;
    restartCameraRequest = false;
  }
  if (fanRequest >= 0)
  {
    if (fanRequest > 0)
    {
      digitalWrite(PIN_FAN, LOW);
      sendAlarmStatus("<FanRequestOn>: PIN_FAN=>LOW");
      sendMqttStatus(mqttTopicFanMeterkast, "1");
    } else 
    {
      digitalWrite(PIN_FAN, LOW);
      sendAlarmStatus("<FanRequestOff>: PIN_FAN=>HIGH");
      sendMqttStatus(mqttTopicFanMeterkast, "0");
    }
    fanRequest = -1;
  }
  if (buzzerRequest >= 0)
  {
    if (buzzerRequest > 0)
    {
      digitalWrite(PIN_BUZZER, LOW);  
      sendAlarmStatus("<BuzzerRequestOn>: PIN_BUZZER=>LOW");
      sendMqttStatus(mqttTopicHcAlarmBuzzer, "1");
    } else 
    {
      digitalWrite(PIN_BUZZER, HIGH); 
      sendAlarmStatus("<BuzzerRequestOff>: PIN_BUZZER=>HIGH");
      sendMqttStatus(mqttTopicHcAlarmBuzzer, "0");
    }
    buzzerRequest = -1;
  }  
  if (enableAlarmRequest >= 0)
  {
    if (enableAlarmRequest > 0)
    
    {
      digitalWrite(PIN_VALARM, LOW);   
      sendAlarmStatus("<VAlarmRequestOn>: PIN_VALARM=>LOW");
      sendMqttStatus(mqttTopicHcVAlarm, "1");
    } else 
    {
      digitalWrite(PIN_VALARM, HIGH);
      sendAlarmStatus("<VAlarmRequestOff>: PIN_VALARM=>HIGH");
      sendMqttStatus(mqttTopicHcVAlarm, "0");
    }
    enableAlarmRequest = -1;
  }  
  if (lampRequest >= 0)
  {
    if (lampRequest > 0)
    
    {
      digitalWrite(PIN_ALARMLAMP, LOW);  
      sendAlarmStatus("<LampRequestOn>: PIN_ALARMLAMP=>LOW");
      sendMqttStatus(mqttTopicHcAlarmLamp, "1");
    } else 
    {
      digitalWrite(PIN_ALARMLAMP, HIGH);
      sendAlarmStatus("<LampRequestOff>: PIN_ALARMLAMP=>HIGH");
      sendMqttStatus(mqttTopicHcAlarmLamp, "0");
    }
    lampRequest = -1;
  }
}

boolean reconnect() {
  // Loop until we're reconnected
  //debuglog.print("Attempting MQTT connection...");
  while (!mqttClient.connected()) {
    if (WifiConnected)
    {
      debuglog.print("Attempting MQTT connection...");
      // Attempt to connect
      if (mqttClient.connect(mqttClientId, mqttUser, mqttPassword)) {
        debuglog.println("mqtt connected");
        // Once connected, publish an announcement...
        //mqttClient.publish("outTopic", "hello world");
        // ... and resubscribe
        delay(1000);
        mqttClient.subscribe(mqttTopicBelVoorTest);
        mqttClient.subscribe(mqttTopicRestarCamera);
        mqttClient.subscribe(mqttTopicDisableVAlarm);
        mqttClient.subscribe(mqttTopicEnableVAlarm);
        mqttClient.subscribe(mqttTopicActivateFan);
        mqttClient.subscribe(mqttTopicDeactivateFan);
        mqttClient.subscribe(mqttTopicActivateBuzzer);
        mqttClient.subscribe(mqttTopicDeactivateBuzzer);
        mqttClient.subscribe(mqttTopicAlarmLampAan);
        mqttClient.subscribe(mqttTopicAlarmLampUit);
        return true;
      } else {
        debuglog.print("mqtt failed, rc=");
        debuglog.print(mqttClient.state());
        debuglog.println(" try again soon...");
        return false;
      }
    }
  }
}

void handle_temperatures()
{
  unsigned long huidigeMillis = millis();

  // STAP 1: Start de conversie voor ALLE sensoren tegelijk (Skip ROM)
  if (!metingBezig && (huidigeMillis - vorigeMillis >= meetInterval)) {
    debuglog.print("Starting all temp measurements...");
    startAlleMetingen();
    metingBezig = true;
    vorigeMillis = huidigeMillis;
    debuglog.println("Temp meting gestart...");
  }

  // STAP 2: Na 750ms zijn de sensoren klaar en lezen we ze één voor één uit
  if (metingBezig && (huidigeMillis - vorigeMillis >= 800)) { // 800ms marge
    debuglog.println("--- Nieuwe Metingen ---");
    float t = leesTemperatuur(addr_Kast);
    if (t >-50)
    {
      debuglog.print("Meterkast:   "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempMeterKast, String(t, 1).c_str());
    }

    t = leesTemperatuur(addr_Thermostaat);
    if (t >-50)
    {
      debuglog.print("Thermostaat: "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempThermostaat, String(t, 1).c_str());
    }
    
    t = leesTemperatuur(addr_Buiten);
    if (t >-50)
    {
      debuglog.print("Buiten:      "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempBuiten, String(t, 1).c_str());
    }
    
    t = leesTemperatuur(addr_Kamer);
    if (t >-50)
    {
      debuglog.print("Woonkamer:   "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempKamer, String(t, 1).c_str());
    }
    
    t = leesTemperatuur(addr_Overloop);
    if (t >-50)
    {
      debuglog.print("Overloop:    "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempOverloop, String(t, 1).c_str());
    }
    
    t = leesTemperatuur(addr_Pi);
    if (t >-50)
    {
      debuglog.print("Meterkast Controller: "); debuglog.println(t);
      mqttClient.publish(mqttTopicTempMeterKastController, String(t, 1).c_str());   
    } 
      
    metingBezig = false; // Reset voor de volgende cyclus
    debuglog.println("-----------------------");
    lastTempRead = now;
  }
}

void startAlleMetingen() {
  ds.OneWireReset();
  ds.OneWireWriteByte(0xCC); // Skip ROM
  ds.OneWireWriteByte(0x44); // Start Conversie
}

float leesTemperatuur(uint8_t* adres) {
  if (!ds.OneWireReset()) return -127.0;
 // Adafruit gebruikt onewireWriteByte voor adressering
  ds.OneWireWriteByte(0x55); 
  
  // De 8 bytes van het adres één voor één versturen
  for (uint8_t i = 0; i < 8; i++) {
    ds.OneWireWriteByte(adres[i]);
  }

  ds.OneWireWriteByte(0xBE); // Read Scratchpad commando


  uint8_t data[9];
  for (int i = 0; i < 9; i++) {
    ds.OneWireReadByte(&data[i]);
  }

  // Controleer of de sensor aanwezig is (data[0] en [1] zijn niet 0xFF bij geldige data)
  if (data[0] == 0xFF && data[1] == 0xFF) return -127.0;

  // Converteer bytes naar temperatuur
  int16_t raw = (data[1] << 8) | data[0];
  return (float)raw / 16.0;
}


void handleInputs() {
    int curT1 = digitalRead(PIN_INPUT_DOOR);
    if (curT1 != lastT1Door) lastDbT1Door = millis();
    if ((millis() - lastDbT1Door) > dbDelay) {
        if (curT1 != stableT1Door) { stableT1Door = curT1; updateAlarmState(); }
    }
    lastT1Door = curT1;

    int curT2 = digitalRead(PIN_INPUT_ACT);
    if (curT2 != lastT2Act) lastDbT2Act = millis();
    if ((millis() - lastDbT2Act) > dbDelay) {
        if (curT2 != stableT2Act) { stableT2Act = curT2; updateAlarmState(); }
    }
    lastT2Act = curT2;
}

void handle_mqtt()
{
    if (!mqttClient.connected())
  {
    long now = millis();
    if (now - lastReconnectAttempt > 5000)
    {
      lastReconnectAttempt = now;
      // Attempt to reconnect
      if (reconnect())
      {
        lastReconnectAttempt = 0;
      }
    }
  } else
  {
    mqttClient.loop();
    delay(10);
  }
}


void readP1() {
  digitalWrite(PIN_P1_REQ, HIGH);
  unsigned long start = millis();
  bool line_is_valid = true;
  while (millis() - start < 1500) {
    if (Serial2.available()) {
      String line = Serial2.readStringUntil('\n');
      //debuglog.print("Lengte van string: ");
      //debuglog.println(line.length());
      //debuglog.print("P1 line: ");
      //debuglog.println(line);
      //sendP1Line(line);
      line_is_valid = true;
      if (line.startsWith("1-0:1.8.1")) sscanf(line.c_str(), "1-0:1.8.1(%f*kWh)", &elDalVerbruikt);
      else if (line.startsWith("1-0:1.8.2")) sscanf(line.c_str(), "1-0:1.8.2(%f*kWh)", &elPiekVerbruikt);
      else if (line.startsWith("1-0:2.8.1")) sscanf(line.c_str(), "1-0:2.8.1(%f*kWh)", &elDalGeleverd);
      else if (line.startsWith("1-0:2.8.2")) sscanf(line.c_str(), "1-0:2.8.2(%f*kWh)", &elPiekGeleverd);
      else if (line.startsWith("1-0:1.7.0")) sscanf(line.c_str(), "1-0:1.7.0(%f*kW)", &elHuidigVerbruik);
      else if (line.startsWith("1-0:2.7.0")) sscanf(line.c_str(), "1-0:2.7.0(%f*kW)", &elHuidigGeleverd);
      else if (line.startsWith("0-1:24.2.1")) {
        // Gas is lastiger: (tijd)(waarde) -> we zoeken de laatste '('
        int lastParen = line.lastIndexOf('(');
        if (lastParen != -1) {
          sscanf(line.substring(lastParen).c_str(), "(%f*m3)", &elGas);
        }
      } else
      {
        line_is_valid = false;
      }
      if (line_is_valid)
      {
        sendP1Line(line.c_str());
      } else
      {
        sendP1InvalidLine(line.c_str());
      }
      if (line.startsWith("!")) break;
    } else {
      //sendP1InvalidLine("Serial is not available, can not read");
      //debuglog.println("Serial2 is not available, can not read P1");
    }
  }
  digitalWrite(PIN_P1_REQ, LOW);
  mqttClient.publish(mqttTopicDalVerbruikt, String(elDalVerbruikt).c_str());
  mqttClient.publish(mqttTopicPiekVerbruikt, String(elPiekVerbruikt).c_str());
  mqttClient.publish(mqttTopicDalGeleverd, String(elDalGeleverd).c_str());
  mqttClient.publish(mqttTopicPiekGeleverd, String(elPiekGeleverd).c_str());
  mqttClient.publish(mqttTopicHuidigVerbruik, String(elHuidigVerbruik).c_str());
  mqttClient.publish(mqttTopicHuidigGeleverd, String(elHuidigGeleverd).c_str());
  mqttClient.publish(mqttTopicGasTotaal, String(elGas).c_str());
}

void WiFiEvent(WiFiEvent_t event, WiFiEventInfo_t info) { 
  switch (event) 
  {
        case ARDUINO_EVENT_WIFI_STA_CONNECTED:
            Serial.println("Verbonden met de router, still no ip!");
            break;
            
        case ARDUINO_EVENT_WIFI_STA_GOT_IP:
            Serial.print("IP-adres ontvangen: ");
            Serial.println(WiFi.localIP());
            WifiConnected = true;
            wifiRestarted = true;            
            break;

        case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
            WifiConnected = false;
            Serial.print("Disconnected! Reason: ");
            Serial.println(info.wifi_sta_disconnected.reason);
            //wifiNeedsRestart = true;
            // Dwing een reconnect, zelfs als de eerste poging faalde
            //Serial.println("Opnieuw proberen...");
            //WiFi.begin(); 
            break;
            
        default:
            break;
    }
}

void handleWifiRestart()
{
  if (wifiNeedsRestart)
  {
    wifiNeedsRestart = false;
    debuglog.println("Retrying to connect to WiFi");
    WiFi.begin(ssid, password);
  } else if (wifiRestarted)
  {
    wifiRestarted = false;
    setupOta();
    reconnect();
    // IP ophalen
    String ipAdres = WiFi.localIP().toString();
    String url = "MeterKastController: " + ipAdres;
    sendAlarmStatus(url.c_str());
  }
}

void PublishBelMQtt(int counter)
{
  //char result[20] = "";
  sprintf(result, "%i\0", counter);
    if (!mqttClient.publish(mqtt_bel_topic, result)) 
    {
      debuglog.println("Bel message failed to send");  
    }
}

void PublishTemperatureMQtt(float temperature)
{
  //char result[20] = "";
  sprintf(result, "Temp C: %.2f\0", temperature);

    if (!mqttClient.publish(mqtt_temperature_topic, result)) 
   {      
      debuglog.println("Temperature message failed to send.");
    }
}

void PublishBoxStateMQtt(int boxState)
{
  //char result[20] = "";
  sprintf(result, "%i\0", boxState);
    if (!mqttClient.publish(mqtt_box_state_topic, result)) 
    {
      debuglog.println("Box state message failed to send."); 
    }
}

void PublishBelStateMQtt(int belState)
{
  //char result[20] = "";
  sprintf(result, "%i\0", belState);
    if (!mqttClient.publish(mqtt_bel_state_topic, result)) 
    {
      debuglog.println("Bel state message failed to send."); 
    }
}

void PublishInfoMQtt(int belState, int boxState, float temperature)
{
  //char result[30] = "";
  sprintf(result, "{'Temp': %.2f, 'Bel':%i, 'Box':%i, 'Wifi':%i}\0", temperature, belState, boxState, WiFi.RSSI());

   if (!mqttClient.publish(mqtt_info_topic, result)) 
   {
      
      debuglog.println("Info message failed to send.");
    }
  
}

void SendBoxOpenedToClients(int numberOfInterruptsBox)
{
  //char result[20] = "";
  sprintf(result, "%i\0", numberOfInterruptsBox);
    if (!mqttClient.publish(mqtt_box_opened_topic, result)) 
    {
      debuglog.println("Box opened message failed to send."); 
    }
}

void SendBoxClosedToClients(int numberOfInterruptsBox)
{
  //char result[20] = "";
  sprintf(result, "%i\0", numberOfInterruptsBox);
    if (!mqttClient.publish(mqtt_box_closed_topic, result)) 
    {
      debuglog.println("Box closed message failed to send.");
    }
}

void SendDeBelGaatToClients(int counter)
{
  PublishBelMQtt(counter);
}

void update_box_state()
{
    char buffer[50];
    prev_boxState = boxState;
    boxState = digitalRead(PIN_BOX);
    box_readoutTime = millis();
    if (prev_boxState != boxState)
    {
      PublishBoxStateMQtt(boxState);
      // Display the box state in the Serial Monitor 
      if (boxState > 0)
      { 
        snprintf(buffer, sizeof(buffer), "Box State = %d Brievenbus opened", boxState);
        debuglog.println(buffer); 
        SendBoxOpenedToClients(numberOfInterruptsBox);
      } else
      {
        snprintf(buffer, sizeof(buffer), "Box State = %d Brievenbus closed", boxState);
        debuglog.println(buffer);
        SendBoxClosedToClients(numberOfInterruptsBox);
      }
      PublishInfoMQtt(belState, boxState, temperature);
    }
}

void update_bel_state()
{
  char buffer[50];
    prev_belState = belState;
    belState = digitalRead(PIN_BEL);
    bel_readoutTime = millis();
    if (prev_belState != belState)
    {
      execute_bel_request();

      PublishBelStateMQtt(belState);
      // Display the bel state in the Serial Monitor 
      if (belState > 0)
      { 
        snprintf(buffer, sizeof(buffer), "Bel State = %d De bel gaat uit", belState);
        debuglog.println(buffer);
        //SendBoxClosedToClients(numberOfInterruptsBox);        
      } else
      {
        snprintf(buffer, sizeof(buffer), "Bel State = %d De bel gaat", belState);
        debuglog.println(buffer);     
        SendDeBelGaatToClients(numberOfInterruptsBel);
      }
      PublishInfoMQtt(belState, boxState, temperature);
    }
}

void HandelBelInterrupt()
{
  char buffer[50];
  if(interruptCounterBel > 0)
  {
    interruptCounterBel = 0;
 //   if ((millis() - lastDebounceTimeBel) > debounceDelayBel)
 //   {
      // reset the debouncing timer
 //     lastDebounceTimeBel = millis(); 
      
      numberOfInterruptsBel++;
   
      snprintf(buffer, sizeof(buffer), "An Bel interrupt has occurred. Total: %d", numberOfInterruptsBel);
      debuglog.println(buffer); 
      update_bel_state();
      //SendDeBelGaatToClients(numberOfInterruptsBel);
 //   }
  }
}

void HandelBoxInterrupt()
{
  char buffer[50];
  if(interruptCounterBox > 0)
  {
    interruptCounterBox = 0;
   // if ((millis() - lastDebounceTimeBox) > debounceDelayBox)
   // {
      // reset the debouncing timer
   //   lastDebounceTimeBox = millis(); 
      
      numberOfInterruptsBox++;
   
      snprintf(buffer, sizeof(buffer), "An Box interrupt has occurred. Total: %d", numberOfInterruptsBox);
      debuglog.println(buffer); 
      update_box_state();
 //   }
  }
}

void HandleTemperatureBel()
{
  char buffer[50];
  if ((millis() - temp_readoutTime) > 30000)
  {
    float prev_temperature = temperature;
    ReadTemperatureBel();
    temp_readoutTime = millis();
    if (prev_temperature != temperature)
    {
      PublishTemperatureMQtt(temperature);
      // Display the temperature in the Serial Monitor 
      snprintf(buffer, sizeof(buffer), "Temp =  %0.2f C", temperature);
      debuglog.println(buffer); 
      PublishInfoMQtt(belState, boxState, temperature);
    }
  }
}

void HandleBelState()
{
  if ((millis() - bel_readoutTime) > 500)
  {
    update_bel_state();
  }
}

void HandleBoxState()
{
  if ((millis() - box_readoutTime) > 500)
  {
    update_box_state();
  }
}

void HandleBelInfo()
{
  if ((millis() - info_Time) > 30000)
  {
    info_Time = millis();
    PublishInfoMQtt(belState, boxState, temperature);
  }
}

void HandleBelBox()
{
  HandelBelInterrupt();
  HandelBoxInterrupt();
  HandleTemperatureBel();
  HandleBelState();
  HandleBoxState();
  HandleBelInfo();
}

// Taak uitvoeren op tweede Core, zodat deze doro blijft lopen als wifi disconnected is (sensoren uitlezen, etc.)
void SensorAndAlarmTaskTask(void * pvParameters) {
    for(;;) {
          HandleBelBox();
          handle_requests(); 
          
          if (relBelActive && (millis() - relBelTimer >= 200)) { 
            digitalWrite(PIN_RELAIS_BEL, HIGH); 
            sendAlarmStatus("<BelRequest_OnTimeout>: PIN_RELAIS_BEL=>HIGH");
            relBelActive = false; 
            }
          if (relCamActive && (millis() - relCamTimer >= 5000)) { 
            digitalWrite(PIN_RELAIS_CAM, HIGH); 
            sendAlarmStatus("<CameraRestartRequest_OnTimeout>: PIN_RELAIS_CAM=>HIGH");
            relCamActive = false; 
            }

          if (!alarmInitialized)
          {
            debuglog.println("Determine initial alarm state");
            updateAlarmState(); 
            alarmInitialized = true;
          } else{
              if (currentState == S2_TriggeredActive) updateAlarmState(); // Check timeout voor S2 -> S3
          }

          handleInputs();

          now = millis();

          if (temp_initialised)
          {
            handle_temperatures();
          }
          

          if (now - lastP1Read > 10000) { readP1(); lastP1Read = now; }
    }
}

void setup() {
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, 16, -1, true);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_FAN, OUTPUT);
  pinMode(PIN_VALARM, OUTPUT);
  pinMode(PIN_ALARMLAMP, OUTPUT);
  pinMode(PIN_RELAIS_BEL, OUTPUT);
  pinMode(PIN_RELAIS_CAM, OUTPUT);
  pinMode(PIN_P1_REQ, OUTPUT);
  pinMode(PIN_INPUT_DOOR, INPUT_PULLUP); 
  pinMode(PIN_INPUT_ACT, INPUT_PULLUP); 

  
  digitalWrite(PIN_BUZZER, HIGH);
  digitalWrite(PIN_FAN, HIGH);
  digitalWrite(PIN_VALARM, LOW);
  digitalWrite(PIN_ALARMLAMP, HIGH);                                          
 
  digitalWrite(PIN_RELAIS_BEL, HIGH);
  digitalWrite(PIN_RELAIS_CAM, HIGH);
  
  pinMode(PIN_BEL, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_BEL), handleInterruptbel, CHANGE);
  pinMode(PIN_BOX, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_BOX), handleInterruptbox, CHANGE);
  lastDebounceTimeBel = millis();
  lastDebounceTimeBox = millis();

  Wire.begin(PIN_SDA, PIN_SCL); //by default use sda=21 and scl=22  
  Wire.setClock(400000); // Zet I2C snelheid op 400kHz voor strakkere timing
  I2CScanner();

  //Wire.begin(21, 22); //by default use sda=21 and scl=22    
  if (!ds.begin(&Wire, DS248X_ADDRESS)) {
    temp_initialised = false;
    sendAlarmStatus("DS2482-100 initialization failed.");
  } else
  {
    temp_initialised = true;
    sendAlarmStatus("DS2482-100 initialization OK!");

    scanOneWireBus();

    // Speed up I2C, as searching for ROMs, specifically, is slow!
    //Wire.setClock(400000);
    //debuglog.println("DS2482 geinitialiseerd wirespeed increased.");
  }

  SetupTemperatureBel();

  // 1. Maak een aparte, onafhankelijke taak aan voor jouw code op Core 1
  xTaskCreatePinnedToCore(
        SensorAndAlarmTaskTask,   // De functie hierboven
        "AalarmTask",            // Naam van de taak
        4096,                 // Geheugengrootte (stack size)
        NULL,                 // Parameters
        1,                    // Prioriteit
        NULL,                 // Task handle
        1                     // Forceer uitvoering op Core 1 (WiFi zit op Core 0)
    );

  mqttClient.setServer(mqtt_server, mqttPort);
  mqttClient.setCallback(callback);

  debuglog.println("Setting up WiFi");
   
  WiFi.onEvent(WiFiEvent); // Register the event handler
  WiFi.disconnect(true, true); 
  delay(1000);
  WiFi.mode(WIFI_STA);
  //WiFi.setSleep(false);  //Dit houdt de radio stabieler op één vermogensniveau, wat soms helpt bij "Reason 2" fouten
  WiFi.setAutoReconnect(true); 
  esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G); // Schakel 'N' uit, Forceer stabiele protocollen (B/G)
  debuglog.println("Trying to connect to WiFi");
  WiFi.begin(ssid, password);  
 

  // while (WiFi.waitForConnectResult() != WL_CONNECTED) {
  //   debuglog.println("Connection Failed! Rebooting...");
  //   delay(5000);
  //   ESP.restart();
  // }
  // 2. Pas nu webSerial en Server initialiseren
  //webSerial.begin(&server);
  //server.begin();

  //setupOta();
  lastReconnectAttempt = 0;  

  
}

void loop() {    
  //handleWifi();
  //if (!bufferVerstuurd && webSerial.getConnectionCount() > 0) {
  //  debuglog.flushBuffer();
  //  bufferVerstuurd = true; 
  //}
  handleWifiRestart();
  if (WiFi.status() == WL_CONNECTED) 
  {
    ArduinoOTA.handle();
    handle_mqtt();
  } else 
  {
    // Geef de processor rust als er geen WiFi is
    delay(100); 
  }
}