// =========================================================================
// DEFINITIEVE SLAVE CODE - MET AUTOMATISCHE WISSELSTURING VIA SERIAL1
// =========================================================================

// Seinpinnen exact volgens jouw JSON-draadschema
const int seinRood[]  = {0, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 13};
const int seinGroen[] = {0, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32};

// REEDCONTACTEN UITRIJDEN (Drukknoppen A t/m E)
const int pin_knopA = A0; // Reset spoor 1 & 2
const int pin_knopB = A1; // Reset spoor 3 & 4
const int pin_knopC = A2; // Reset spoor 5 & 6
const int pin_knopD = A3; // Reset spoor 7 & 8
const int pin_knopE = A4; // Reset spoor 9, 10 & 11

// REEDCONTACTEN BINNENKOMST / TIMERS (Pinnen A5 t/m A8)
const int pin_rdT1 = A5; 
const int pin_rdT2 = A6;
const int pin_rdT5 = A7;
const int pin_rdT6 = A8;

// De 11 bezetmelders (knoppen onderin je scherm)
const int pinDetector[] = {0, A9, A10, A11, A12, A13, A14, A15, 2, 3, 4, 5};
bool vorigeStatusDetector[] = {0, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH};

// Automaat-statussen (Standaard direct actief)
bool timer1Actief = true;
bool timer2Actief = true;
bool timer5Actief = true;
bool timer6Actief = true;

unsigned long timer1Start = 0; bool timer1Loopt = false;
unsigned long timer2Start = 0; bool timer2Loopt = false;
unsigned long timer5Start = 0; bool timer5Loopt = false;
unsigned long timer6Start = 0; bool timer6Loopt = false;
const unsigned long wachtTijd = 15000; // 15 seconden

bool systeemStop = false;

void verwerkCommando(String cmd) {
  cmd.trim();
  if (cmd.length() == 0) return;

  Serial.print("[RX]: "); Serial.println(cmd);

  if (cmd == "STOP") {
    systeemStop = true;
    Serial.println("⚠️ NOODSTOP! Alle seinen op ROOD!");
    for (int i = 1; i <= 11; i++) {
      digitalWrite(seinGroen[i], LOW);
      digitalWrite(seinRood[i], HIGH);
    }
    timer1Loopt = timer2Loopt = timer5Loopt = timer6Loopt = false;
    return;
  }
  else if (cmd == "START") {
    systeemStop = false;
    Serial.println("▶️ Systeem actief.");
    return;
  }

  if (systeemStop) return;

  if (cmd == "T1_AAN")  { timer1Actief = true; }
  if (cmd == "T1_UIT")  { timer1Actief = false; timer1Loopt = false; }
  if (cmd == "T2_AAN")  { timer2Actief = true; }
  if (cmd == "T2_UIT")  { timer2Actief = false; timer2Loopt = false; }
  if (cmd == "T5_AAN")  { timer5Actief = true; }
  if (cmd == "T5_UIT")  { timer5Actief = false; timer5Loopt = false; }
  if (cmd == "T6_AAN")  { timer6Actief = true; }
  if (cmd == "T6_UIT")  { timer6Actief = false; timer6Loopt = false; }

  // Handmatige seinbediening vanaf paneel via de HUB
  if (cmd.startsWith("M_R")) { 
    int nummer = cmd.substring(3).toInt();
    if (nummer >= 1 && nummer <= 11) {
      digitalWrite(seinGroen[nummer], LOW);   
      digitalWrite(seinRood[nummer], HIGH);  
      if (nummer == 1 || nummer == 2) { timer1Loopt = false; timer2Loopt = false; }
      if (nummer == 5 || nummer == 6) { timer5Loopt = false; timer6Loopt = false; }
    }
  }
  else if (cmd.startsWith("M_G")) { 
    int nummer = cmd.substring(3).toInt();
    if (nummer >= 1 && nummer <= 11) {
      // Stuur direct ook de wissels mee als er handmatig groen wordt gegeven!
      if (nummer == 1) { Serial1.println("WISSEL_SPOOR1"); Serial.println("[TX HUB] -> Zet wissels voor Spoor 1"); }
      if (nummer == 2) { Serial1.println("WISSEL_SPOOR2"); Serial.println("[TX HUB] -> Zet wissels voor Spoor 2"); }
      if (nummer == 5) { Serial1.println("WISSEL_SPOOR5"); Serial.println("[TX HUB] -> Zet wissels voor Spoor 5"); }
      if (nummer == 6) { Serial1.println("WISSEL_SPOOR6"); Serial.println("[TX HUB] -> Zet wissels voor Spoor 6"); }
      
      delay(100); // Korte pauze zodat de wissels eerst kunnen omgaan
      digitalWrite(seinRood[nummer], LOW);    
      digitalWrite(seinGroen[nummer], HIGH);  
    }
  }
}

void setup() {
  Serial.begin(9600);   
  Serial1.begin(9600); // Verbinding met de Nano HUB 

  pinMode(pin_knopA, INPUT_PULLUP);
  pinMode(pin_knopB, INPUT_PULLUP);
  pinMode(pin_knopC, INPUT_PULLUP);
  pinMode(pin_knopD, INPUT_PULLUP);
  pinMode(pin_knopE, INPUT_PULLUP);

  pinMode(pin_rdT1, INPUT_PULLUP);
  pinMode(pin_rdT2, INPUT_PULLUP);
  pinMode(pin_rdT5, INPUT_PULLUP);
  pinMode(pin_rdT6, INPUT_PULLUP);

  for (int i = 1; i <= 11; i++) {
    pinMode(pinDetector[i], INPUT_PULLUP);
    vorigeStatusDetector[i] = digitalRead(pinDetector[i]);
  }

  for (int i = 1; i <= 11; i++) {
    pinMode(seinRood[i], OUTPUT);
    pinMode(seinGroen[i], OUTPUT);
    digitalWrite(seinRood[i], HIGH);  
    digitalWrite(seinGroen[i], LOW);
  }

  Serial.println("=== SLAVE ONLINE ===");
}

void loop() {
  if (Serial.available())   { verwerkCommando(Serial.readStringUntil('\n')); }
  if (Serial1.available())  { verwerkCommando(Serial1.readStringUntil('\n')); }

  if (systeemStop) return;

  // --- 1. REEDCONTACTEN BIJ HET UITRIJDEN (SEIN DIRECT OP ROOD) ---
  if (digitalRead(pin_knopA) == LOW) {
    digitalWrite(seinGroen[1], LOW);   digitalWrite(seinRood[1], HIGH);
    digitalWrite(seinGroen[2], LOW);   digitalWrite(seinRood[2], HIGH);
    timer1Loopt = false; 
    timer2Loopt = false; 
    Serial.println("[REED A] Trein passeert. Sein 1 & 2 direct op ROOD gezet.");
    delay(200); 
  }
  if (digitalRead(pin_knopB) == LOW) {
    digitalWrite(seinGroen[3], LOW);   digitalWrite(seinRood[3], HIGH);
    digitalWrite(seinGroen[4], LOW);   digitalWrite(seinRood[4], HIGH);
    Serial.println("[REED B] Trein passeert. Sein 3 & 4 op ROOD gezet.");
    delay(200);
  }
  if (digitalRead(pin_knopC) == LOW) {
    digitalWrite(seinGroen[5], LOW);   digitalWrite(seinRood[5], HIGH);
    digitalWrite(seinGroen[6], LOW);   digitalWrite(seinRood[6], HIGH);
    timer5Loopt = false; 
    timer6Loopt = false; 
    Serial.println("[REED C] Trein passeert. Sein 5 & 6 op ROOD gezet.");
    delay(200);
  }
  if (digitalRead(pin_knopD) == LOW) {
    digitalWrite(seinGroen[7], LOW);   digitalWrite(seinRood[7], HIGH);
    digitalWrite(seinGroen[8], LOW);   digitalWrite(seinRood[8], HIGH);
    Serial.println("[REED D] Trein passeert. Sein 7 & 8 op ROOD gezet.");
    delay(200);
  }
  if (digitalRead(pin_knopE) == LOW) {
    digitalWrite(seinGroen[9], LOW);   digitalWrite(seinRood[9], HIGH);
    digitalWrite(seinGroen[10], LOW);  digitalWrite(seinRood[10], HIGH);
    digitalWrite(seinGroen[11], LOW);  digitalWrite(seinRood[11], HIGH);
    Serial.println("[REED E] Trein passeert. Sein 9, 10 & 11 op ROOD gezet.");
    delay(200);
  }

  // --- 2. BEZETMELDERS TRACKING ---
  for (int i = 1; i <= 11; i++) {
    bool actueleStatus = digitalRead(pinDetector[i]); 
    if (actueleStatus != vorigeStatusDetector[i]) {
      delay(20); 
      if (digitalRead(pinDetector[i]) == actueleStatus) {
        if (actueleStatus == LOW) {
          Serial1.print("B"); Serial1.print(i); Serial1.println("_AAN"); 
          Serial.print("[SPOOR] Spoor "); Serial.print(i); Serial.println(" BEZET");
        } else {
          Serial1.print("B"); Serial1.print(i); Serial1.println("_UIT"); 
          Serial.print("[SPOOR] Spoor "); Serial.print(i); Serial.println(" VRIJ");
        }
        vorigeStatusDetector[i] = actueleStatus;
      }
    }
  }

  // --- 3. AUTOMATISCHE TIMERS (TREIN START DE ANDERE TREIN) ---
  
  // TREIN OP SPOOR 1 activeert T1 -> na 15s gaat SPOOR 2 (Sein 2) rijden
  if (digitalRead(pin_rdT1) == LOW && timer1Actief && !timer1Loopt) {
    delay(50); 
    if (digitalRead(pin_rdT1) == LOW) {
      Serial.println("[AUTOMAAT 1] Reed T1 geactiveerd. Trein 1 is binnen. Timer loopt voor Spoor 2...");
      timer1Start = millis();
      timer1Loopt = true;
    }
  }
  if (timer1Loopt && (millis() - timer1Start >= wachtTijd)) {
    timer1Loopt = false;
    Serial.println("[TIMER 1] Afgelopen! Wissels omzetten en Uitrijsein 2 (Spoor 2) naar GROEN.");
    Serial1.println("WISSEL_SPOOR2"); // Geef via de HUB door aan de wissel-mega om te schakelen!
    delay(200); 
    digitalWrite(seinRood[2], LOW); digitalWrite(seinGroen[2], HIGH);
  }

  // TREIN OP SPOOR 2 activeert T2 -> na 15s gaat SPOOR 1 (Sein 1) rijden
  if (digitalRead(pin_rdT2) == LOW && timer2Actief && !timer2Loopt) {
    delay(50); 
    if (digitalRead(pin_rdT2) == LOW) {
      Serial.println("[AUTOMAAT 2] Reed T2 geactiveerd. Trein 2 is binnen. Timer loopt voor Spoor 1...");
      timer2Start = millis(); 
      timer2Loopt = true;
    }
  }
  if (timer2Loopt && (millis() - timer2Start >= wachtTijd)) {
    timer2Loopt = false;
    Serial.println("[TIMER 2] Afgelopen! Wissels omzetten en Uitrijsein 1 (Spoor 1) naar GROEN.");
    Serial1.println("WISSEL_SPOOR1"); // Geef via de HUB door aan de wissel-mega om te schakelen!
    delay(200);
    digitalWrite(seinRood[1], LOW); digitalWrite(seinGroen[1], HIGH);
  }

  // TREIN OP SPOOR 5 activeert T5 -> na 15s gaat SPOOR 6 (Sein 6) rijden
  if (digitalRead(pin_rdT5) == LOW && timer5Actief && !timer5Loopt) {
    delay(50); 
    if (digitalRead(pin_rdT5) == LOW) {
      Serial.println("[AUTOMAAT 5] Reed T5 geactiveerd. Trein 5 is binnen. Timer loopt voor Spoor 6...");
      timer5Start = millis(); 
      timer5Loopt = true;
    }
  }
  if (timer5Loopt && (millis() - timer5Start >= wachtTijd)) {
    timer5Loopt = false;
    Serial.println("[TIMER 5] Afgelopen! Wissels omzetten en Uitrijsein 6 (Spoor 6) naar GROEN.");
    Serial1.println("WISSEL_SPOOR6");
    delay(200);
    digitalWrite(seinRood[6], LOW); digitalWrite(seinGroen[6], HIGH);
  }

  // TREIN OP SPOOR 6 activeert T6 -> na 15s gaat SPOOR 5 (Sein 5) rijden
  if (digitalRead(pin_rdT6) == LOW && timer6Actief && !timer6Loopt) {
    delay(50); 
    if (digitalRead(pin_rdT6) == LOW) {
      Serial.println("[AUTOMAAT 6] Reed T6 geactiveerd. Trein 6 is binnen. Timer loopt voor Spoor 5...");
      timer6Start = millis(); 
      timer6Loopt = true;
    }
  }
  if (timer6Loopt && (millis() - timer6Start >= wachtTijd)) {
    timer6Loopt = false;
    Serial.println("[TIMER 6] Afgelopen! Wissels omzetten en Uitrijsein 5 (Spoor 5) naar GROEN.");
    Serial1.println("WISSEL_SPOOR5");
    delay(200);
    digitalWrite(seinRood[5], LOW); digitalWrite(seinGroen[5], HIGH);
  }
}