#include <TM1637Display.h>

// --- TM1637 DISPLAY CONFIGURATIE ---
const int PIN_CLK = 14;
const int PIN_DIO = 15;
TM1637Display display(PIN_CLK, PIN_DIO);

// --- PIN DEFINITIES ---

// Wissel Relais (Kato - Relais 1 t/m 10 en 25 t/m 28)
const int PIN_W4R = 2;   const int PIN_W4A = 3;
const int PIN_W5R = 4;   const int PIN_W5A = 5;
const int PIN_W6R = 6;   const int PIN_W6A = 7;
const int PIN_W9R = 8;   const int PIN_W9A = 9;
const int PIN_W10R = 10;  const int PIN_W10A = 11;
const int PIN_W7R = 44;  const int PIN_W7A = 45; 
const int PIN_W8R = 46;  const int PIN_W8A = 47; 

// Rijstroom Stoprelais (Relais 11 t/m 24) - Deze blijven Active Low (HIGH = Rijstroom AAN)
const int PIN_STOP_SP5 = 22; const int PIN_REM_SP5 = 23;
const int PIN_STOP_SP6 = 24; const int PIN_REM_SP6 = 25;
const int PIN_STOP_SP7 = 26; const int PIN_REM_SP7 = 27;
const int PIN_STOP_SP8 = 28; const int PIN_REM_SP8 = 29;
const int PIN_STOP_SP9 = 30; const int PIN_REM_SP9 = 31; 
const int PIN_STOP_SP10 = 32; const int PIN_REM_SP10 = 33; 
const int PIN_STOP_SP11 = 34; const int PIN_REM_SP11 = 35; 

// Bezetmelders
const int PIN_BM5 = 36;   const int PIN_BM6 = 37;
const int PIN_BM7 = 38;   const int PIN_BM8 = 39;
const int PIN_BM9 = 48;   const int PIN_BM10 = 49; const int PIN_BM11 = 50; 

// Reedcontacten
const int PIN_REED_C = 40; const int PIN_REED_D = 41;
const int PIN_REED_M = 42; const int PIN_REED_N = 43; const int PIN_REED_E = 51;

// Viessmann Lichtseinen 
const int PIN_S5_R = A0;  const int PIN_S5_G = A1;
const int PIN_S6_R = A2;  const int PIN_S6_G = A3;  
const int PIN_S7_R = A4;  const int PIN_S7_G = A5;
const int PIN_S8_R = A6;  const int PIN_S8_G = A7;  
const int PIN_S9_R = A8;  const int PIN_S9_G = A9;  
const int PIN_S10_R = A10; const int PIN_S10_G = A11; 
const int PIN_S11_R = A12; const int PIN_S11_G = A13; 

// Timers en status
unsigned long wisselPulsStart = 0;
int actieveWisselPin = -1;
unsigned long overstapTimerStart = 0;
bool overstapTimerActief = false;
int resterendeTijd = 0;
int actiefWachtSpoor = -1; // <--- DEZE REGEL MOET ERBIJ!

void setup() {
  Serial.begin(9600);
  display.setBrightness(0x0f);
  display.showNumberDec(0, false);

  // 1. WISSEL RELAIS IN RUST OP LOW ZETTEN (Active High Logica)
  // Dit zorgt dat er DIRECT 0 Volt op de pinnen staat bij het opstarten!
  digitalWrite(PIN_W4R, LOW);  digitalWrite(PIN_W4A, LOW);
  digitalWrite(PIN_W5R, LOW);  digitalWrite(PIN_W5A, LOW);
  digitalWrite(PIN_W6R, LOW);  digitalWrite(PIN_W6A, LOW);
  digitalWrite(PIN_W9R, LOW);  digitalWrite(PIN_W9A, LOW);
  digitalWrite(PIN_W10R, LOW); digitalWrite(PIN_W10A, LOW);
  digitalWrite(PIN_W7R, LOW);  digitalWrite(PIN_W7A, LOW);
  digitalWrite(PIN_W8R, LOW);  digitalWrite(PIN_W8A, LOW);

  // Nu pas instellen als OUTPUT
  pinMode(PIN_W4R, OUTPUT);  pinMode(PIN_W4A, OUTPUT);
  pinMode(PIN_W5R, OUTPUT);  pinMode(PIN_W5A, OUTPUT);
  pinMode(PIN_W6R, OUTPUT);  pinMode(PIN_W6A, OUTPUT);
  pinMode(PIN_W9R, OUTPUT);  pinMode(PIN_W9A, OUTPUT);
  pinMode(PIN_W10R, OUTPUT); pinMode(PIN_W10A, OUTPUT);
  pinMode(PIN_W7R, OUTPUT);  pinMode(PIN_W7A, OUTPUT);
  pinMode(PIN_W8R, OUTPUT);  pinMode(PIN_W8A, OUTPUT);

  // 2. Initialiseer alle rijstroom/stop-relais (Standaard HIGH = stroom op de baan)
  int stopPins[] = {
    PIN_STOP_SP5, PIN_REM_SP5, PIN_STOP_SP6, PIN_REM_SP6, 
    PIN_STOP_SP7, PIN_REM_SP7, PIN_STOP_SP8, PIN_REM_SP8, 
    PIN_STOP_SP9, PIN_REM_SP9, PIN_STOP_SP10, PIN_REM_SP10, 
    PIN_STOP_SP11, PIN_REM_SP11
  };
  for (int pin : stopPins) {
    digitalWrite(pin, HIGH);
    pinMode(pin, OUTPUT);
  }

  // 3. Initialiseer alle Seinen (Standaard HIGH = UIT)
  int seinPins[] = {
    PIN_S5_R, PIN_S5_G, PIN_S6_R, PIN_S6_G, PIN_S7_R, PIN_S7_G, 
    PIN_S8_R, PIN_S8_G, PIN_S9_R, PIN_S9_G, PIN_S10_R, PIN_S10_G, 
    PIN_S11_R, PIN_S11_G
  };
  for (int pin : seinPins) {
    digitalWrite(pin, HIGH);
    pinMode(pin, OUTPUT);
  }

  // Inputs met Pull-up
  pinMode(PIN_BM5, INPUT_PULLUP); pinMode(PIN_BM6, INPUT_PULLUP);
  pinMode(PIN_BM7, INPUT_PULLUP); pinMode(PIN_BM8, INPUT_PULLUP);
  pinMode(PIN_BM9, INPUT_PULLUP); pinMode(PIN_BM10, INPUT_PULLUP); pinMode(PIN_BM11, INPUT_PULLUP);
  
  pinMode(PIN_REED_C, INPUT_PULLUP); pinMode(PIN_REED_D, INPUT_PULLUP);
  pinMode(PIN_REED_M, INPUT_PULLUP); pinMode(PIN_REED_N, INPUT_PULLUP);
  pinMode(PIN_REED_E, INPUT_PULLUP);

  // Zet bij start alle seinen netjes op Rood
  zetSein(5, true); zetSein(6, true); zetSein(7, true); zetSein(8, true);
  zetSein(9, true); zetSein(10, true); zetSein(11, true);

  Serial.println("SLAVE BINNENBAAN - CRITICAL WISSEL LOGIC FIX GESTART.");
}

void loop() {
  leesLokaleSensoren();
  handleSerialInvoer();
  updateWisselPuls();
  updateOverstapTimer();
}

void leesLokaleSensoren() {
  // Reedcontact C: Zet sein 5 en 6 op rood (relais vallen af), Sein 7 op groen
  if (digitalRead(PIN_REED_C) == LOW) { 
    Serial.println("REED5_C"); 
    zetSein(5, true);  // true = Rood
    zetSein(6, true);  // true = Rood
    zetSein(7, false); // false = Groen
    delay(250); 
  }
  
  // Reedcontact D: Zet 7 en 8 op rood (relais vallen af)
  if (digitalRead(PIN_REED_D) == LOW) { 
    Serial.println("REED6_D"); 
    zetSein(7, true);  // true = Rood
    zetSein(8, true);  // true = Rood
    delay(250); 
  }
  
  // Reedcontact E: Zet 9, 10, en 11 op rood (relais vallen af)
  if (digitalRead(PIN_REED_E) == LOW) { 
    Serial.println("REED_E"); 
    zetSein(9, true);  // true = Rood
    zetSein(10, true); // true = Rood
    zetSein(11, true); // true = Rood
    delay(250); 
  }

  // REED_M (Spoor 5): Trein start hier de countdown/overstaptimer
  if (digitalRead(PIN_REED_M) == LOW) { 
    Serial.println("REED5_M"); 
    actiefWachtSpoor = 5;
    startCountdown(5); 
    delay(250); 
  }
  
  // REED_N (Spoor 6): Trein start hier de countdown/overstaptimer
  if (digitalRead(PIN_REED_N) == LOW) { 
    Serial.println("REED6_N"); 
    actiefWachtSpoor = 6;
    startCountdown(5); 
    delay(250); 
  }

  // Bezetmelders met debounce (ontstoring)
  const int AANTAL_BMS = 7;
  const int bmPins[AANTAL_BMS] = {PIN_BM5, PIN_BM6, PIN_BM7, PIN_BM8, PIN_BM9, PIN_BM10, PIN_BM11};
  const String bmNamen[AANTAL_BMS] = {"BM5", "BM6", "BM7", "BM8", "BM9", "BM10", "BM11"};
  
  static bool stabieleStatus[AANTAL_BMS] = {HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH};
  static bool laatsteMeting[AANTAL_BMS] = {HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH};
  static unsigned long veranderTijd[AANTAL_BMS] = {0, 0, 0, 0, 0, 0, 0};
  
  unsigned long nu = millis();

  for (int i = 0; i < AANTAL_BMS; i++) {
    bool actueleMeting = digitalRead(bmPins[i]);
    if (actueleMeting != laatsteMeting[i]) {
      veranderTijd[i] = nu;
      laatsteMeting[i] = actueleMeting;
    }
    if ((nu - veranderTijd[i]) > 50) {
      if (actueleMeting != stabieleStatus[i]) {
        stabieleStatus[i] = actueleMeting;
        if (stabieleStatus[i] == LOW) {
          Serial.println(bmNamen[i]);
        } else {
          Serial.println(bmNamen[i] + "_VRIJ");
        }
      }
    }
  }
}void handleSerialInvoer() {
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    if (cmd == "S5_R") zetSein(5, true);
    else if (cmd == "S5_G") zetSein(5, false);
    else if (cmd == "S6_R") zetSein(6, true);
    else if (cmd == "S6_G") zetSein(6, false);
    else if (cmd == "S7_R") zetSein(7, true);
    else if (cmd == "S7_G") zetSein(7, false);
    else if (cmd == "S8_R") zetSein(8, true);
    else if (cmd == "S8_G") zetSein(8, false);
    else if (cmd == "S9_R") zetSein(9, true);   
    else if (cmd == "S9_G") zetSein(9, false);  
    else if (cmd == "S10_R") zetSein(10, true); 
    else if (cmd == "S10_G") zetSein(10, false);
    else if (cmd == "S11_R") zetSein(11, true); 
    else if (cmd == "S11_G") zetSein(11, false);

    else if (cmd == "W4_R") activeerWisselPuls(PIN_W4R);
    else if (cmd == "W4_A") activeerWisselPuls(PIN_W4A);
    else if (cmd == "W5_R") activeerWisselPuls(PIN_W5R);
    else if (cmd == "W5_A") activeerWisselPuls(PIN_W5A);
    else if (cmd == "W6_R") activeerWisselPuls(PIN_W6R);
    else if (cmd == "W6_A") activeerWisselPuls(PIN_W6A);
    else if (cmd == "W7_R") activeerWisselPuls(PIN_W7R); 
    else if (cmd == "W7_A") activeerWisselPuls(PIN_W7A); 
    else if (cmd == "W8_R") activeerWisselPuls(PIN_W8R); 
    else if (cmd == "W8_A") activeerWisselPuls(PIN_W8A); 
    else if (cmd == "W9_R") activeerWisselPuls(PIN_W9R);
    else if (cmd == "W9_A") activeerWisselPuls(PIN_W9A);
    else if (cmd == "W10_R") activeerWisselPuls(PIN_W10R);
    else if (cmd == "W10_A") activeerWisselPuls(PIN_W10A);
  }
}

void zetSein(int id, bool rood) {
  if (id == 5) {
    digitalWrite(PIN_S5_R, rood ? LOW : HIGH);   digitalWrite(PIN_S5_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP5, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP5, rood ? LOW : HIGH); 
  } 
  else if (id == 6) {
    digitalWrite(PIN_S6_R, rood ? LOW : HIGH);   digitalWrite(PIN_S6_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP6, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP6, rood ? LOW : HIGH);
  } 
  else if (id == 7) {
    digitalWrite(PIN_S7_R, rood ? LOW : HIGH);   digitalWrite(PIN_S7_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP7, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP7, rood ? LOW : HIGH);
  } 
  else if (id == 8) {
    digitalWrite(PIN_S8_R, rood ? LOW : HIGH);   digitalWrite(PIN_S8_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP8, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP8, rood ? LOW : HIGH);
  }
  else if (id == 9) { 
    digitalWrite(PIN_S9_R, rood ? LOW : HIGH);   digitalWrite(PIN_S9_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP9, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP9, rood ? LOW : HIGH);
  }
  else if (id == 10) { 
    digitalWrite(PIN_S10_R, rood ? LOW : HIGH);  digitalWrite(PIN_S10_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP10, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP10, rood ? LOW : HIGH);
  }
  else if (id == 11) { 
    digitalWrite(PIN_S11_R, rood ? LOW : HIGH);  digitalWrite(PIN_S11_G, rood ? HIGH : LOW);
    digitalWrite(PIN_STOP_SP11, rood ? LOW : HIGH); digitalWrite(PIN_REM_SP11, rood ? LOW : HIGH);
  }
}

void activeerWisselPuls(int pin) {
  if (actieveWisselPin != -1) {
    digitalWrite(actieveWisselPin, LOW); // Vorige puls direct afbreken (veiligheid)
  }
  actieveWisselPin = pin;
  wisselPulsStart = millis();
  digitalWrite(actieveWisselPin, HIGH); // Puls AAN (Naar 5V sturen voor Active High relais)
}

void updateWisselPuls() {
  if (actieveWisselPin != -1 && (millis() - wisselPulsStart >= 200)) {
    digitalWrite(actieveWisselPin, LOW); // Puls UIT (Weer terug naar 0V na 200ms)
    actieveWisselPin = -1;
  }
}

void startCountdown(int seconden) {
  overstapTimerStart = millis();
  resterendeTijd = seconden;
  overstapTimerActief = true;
  display.showNumberDec(resterendeTijd, false);
}

void updateOverstapTimer() {
  if (overstapTimerActief) {
    unsigned long verlopen = millis() - overstapTimerStart;
    int actueleResterendeTijd = 5 - (verlopen / 1000);
    
    if (actueleResterendeTijd != resterendeTijd) {
      resterendeTijd = actueleResterendeTijd;
      if (resterendeTijd >= 0) display.showNumberDec(resterendeTijd, false);
    }
    
    // --- TIJD IS OM (0 BEREIKT) ---
    if (verlopen >= 5000) {
      overstapTimerActief = false;
      display.showNumberDec(0, false);
      
      // Als Spoor 5 (M) klaar is met wachten
      if (actiefWachtSpoor == 5) {
        zetSein(5, false);       // Slave zet lokaal sein 5 op groen
        Serial.println("S5_G");  // Slave meldt "S5_G" aan de Master (Master schakelt de wissels!)
      } 
      // Als Spoor 6 (N) klaar is met wachten
      else if (actiefWachtSpoor == 6) {
        zetSein(6, false);       // Slave zet lokaal sein 6 op groen
        Serial.println("S6_G");  // Slave meldt "S6_G" aan de Master (Master schakelt de wissels!)
      }
      
      actiefWachtSpoor = -1; // Reset voor de volgende trein
    }
  }
}