// 1. PINNEN EN GLOBALE DEFINITIES
const int DS_PIN = 53;
const int STCP_PIN = 52;
const int SHCP_PIN = 51;
byte regData[5] = {0, 0, 0, 0, 0}; 

const int knopRecht[] = {2, 4, 6, 8, 10, 22, 24};
const int knopAfb[]   = {3, 5, 7, 9, 11, 23, 25};
const int btnSeinR[]  = {27, 29, 31, 33, 35, 37, 39};
const int btnSeinG[]  = {26, 28, 30, 32, 34, 36, 38};
const int ledPins[]   = {40, 41, 42, 43, 44, 45, 46, 47, 48, 49, A0, A1, A2, A3};

bool bezetStatus[7] = {false, false, false, false, false, false, false}; // BM5 t/m BM11

// Segmenten voor 0 t/m 9 (A t/m G)
const byte cijfers[] = {
  0b11000000, 0b11111001, 0b10100100, 0b10110000, 0b10011001, 
  0b10010010, 0b10000010, 0b11111000, 0b10000000, 0b10010000
};
unsigned long startTijd[2] = {0, 0}; // 0 = spoor 5, 1 = spoor 6
bool timerActief[2] = {false, false};
const unsigned long duur = 10000;   // 10 seconden

// 2. FUNCTIES (Eerst declareren, dan gebruiken)
void updateRegisters() {
  digitalWrite(STCP_PIN, LOW); // LatchPin
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[4]); // SR5 (Spoor 6 display)
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[3]); // SR4 (Spoor 5 display)
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[2]); // SR3
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[1]); // SR2
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[0]); // SR1
  digitalWrite(STCP_PIN, HIGH); // LatchPin
}

void setWissel(int wisselIndex, bool recht) {
  int totaalBit = wisselIndex * 2; 
  int regIndex = totaalBit / 8;
  int bitPos = totaalBit % 8;
  if (recht) { bitSet(regData[regIndex], bitPos); bitClear(regData[regIndex], bitPos + 1); } 
  else { bitClear(regData[regIndex], bitPos); bitSet(regData[regIndex], bitPos + 1); }
  updateRegisters();
}

bool isWisselRecht(int wisselIndex) {
  int totaalBit = wisselIndex * 2;
  return bitRead(regData[totaalBit / 8], totaalBit % 8) == 1;
}

void stelWisselsVoorSein(int seinIndex) {
  switch(seinIndex) {
    case 0: setWissel(0, false); setWissel(2, false); setWissel(1, true); break; // S5
    case 1: setWissel(0, true); setWissel(2, true); setWissel(1, true); break;   // S6
    case 2: setWissel(5, false); setWissel(6, false); break;                    // S7
    case 3: setWissel(5, true); setWissel(6, true); break;                      // S8
    case 4: setWissel(1, false); setWissel(3, true); setWissel(4, false); break; // S9
    case 5: setWissel(1, false); setWissel(3, true); setWissel(4, true); break;  // S10
    case 6: setWissel(1, false); setWissel(3, false); break;                    // S11
  }
}

bool magSeinGroen(int seinIndex) {
  // Controleer wisselstand
  bool wisselsGoed = false;
  switch(seinIndex) {
    case 0: wisselsGoed = (!isWisselRecht(0) && !isWisselRecht(2) && isWisselRecht(1)); break;
    case 1: wisselsGoed = (isWisselRecht(0) && isWisselRecht(2) && isWisselRecht(1)); break;
    case 2: wisselsGoed = (!isWisselRecht(5) && !isWisselRecht(6)); break;
    case 3: wisselsGoed = (isWisselRecht(5) && isWisselRecht(6)); break;
    case 4: wisselsGoed = (!isWisselRecht(1) && isWisselRecht(3) && !isWisselRecht(4)); break;
    case 5: wisselsGoed = (!isWisselRecht(1) && isWisselRecht(3) && isWisselRecht(4)); break;
    case 6: wisselsGoed = (!isWisselRecht(1) && !isWisselRecht(3)); break;
  }
  if (!wisselsGoed) return false;
  
  // Controleer andere seinen
  switch(seinIndex) {
    case 0: return (digitalRead(ledPins[3]) == LOW); 
    case 1: return (digitalRead(ledPins[1]) == LOW); 
    case 2: return (digitalRead(ledPins[7]) == LOW); 
    case 3: return (digitalRead(ledPins[5]) == LOW); 
    case 4: return (digitalRead(ledPins[1]) == LOW && digitalRead(ledPins[3]) == LOW && digitalRead(ledPins[11]) == LOW && digitalRead(ledPins[13]) == LOW); 
    case 5: return (digitalRead(ledPins[1]) == LOW && digitalRead(ledPins[3]) == LOW && digitalRead(ledPins[9]) == LOW && digitalRead(ledPins[13]) == LOW);  
    case 6: return (digitalRead(ledPins[1]) == LOW && digitalRead(ledPins[3]) == LOW && digitalRead(ledPins[9]) == LOW && digitalRead(ledPins[11]) == LOW); 
  }
  return false;
}

// 3. SETUP
void setup() {
  // 1. ZET DE REGISTERS OP DE JUISTE "UIT"-STAND
  regData[0] = 0b00000000; // SR1: Wissels (Active High)
  regData[1] = 0b00000000; // SR2: Wissels (Active High)
  regData[2] = 0b00000000; // SR3: Bezetmelders (Active High)
  regData[3] = 0b11111111; // SR4: Display 5 (Active Low)
  regData[4] = 0b11111111; // SR5: Display 6 (Active Low)
  updateRegisters();
  
  // 2. DE REST VAN JE SETUP (Alles wat je al had)
  Serial.begin(9600);
  Serial.println("Systeem Gestart: Seinen op ROOD, Wissels op RECHT.");
  pinMode(DS_PIN, OUTPUT); pinMode(STCP_PIN, OUTPUT); pinMode(SHCP_PIN, OUTPUT);
  pinMode(A15, INPUT_PULLUP); // Schakelaar die de timers blokkeert
  for(int i = 0; i < 7; i++) {
    pinMode(knopRecht[i], INPUT_PULLUP); pinMode(knopAfb[i], INPUT_PULLUP);
    pinMode(btnSeinR[i], INPUT_PULLUP); pinMode(btnSeinG[i], INPUT_PULLUP);
  }
  for(int i = 0; i < 14; i++) { pinMode(ledPins[i], OUTPUT); digitalWrite(ledPins[i], LOW); }
  for(int i = 0; i < 7; i++) { setWissel(i, true); }
  for(int i = 0; i < 7; i++) { digitalWrite(ledPins[i*2], HIGH); digitalWrite(ledPins[i*2+1], LOW); }
}

void updateDisplay(int spoor, int waarde) {
  if (waarde >= 0 && waarde <= 9) {
    if (spoor == 0) regData[3] = cijfers[waarde]; // Spoor 5
    else            regData[4] = cijfers[waarde]; // Spoor 6
    updateRegisters();
  }
}


// 4. LOOP
void loop() {
  // --- ZONE 1: Handmatige bediening (Knoppen) ---
  for(int i = 0; i < 7; i++) {
    if (digitalRead(knopRecht[i]) == LOW) { setWissel(i, true); delay(50); }
    if (digitalRead(knopAfb[i]) == LOW)   { setWissel(i, false); delay(50); }
  }
  
  for(int i = 0; i < 7; i++) {
    if (digitalRead(btnSeinR[i]) == LOW) { 
      digitalWrite(ledPins[i*2], HIGH); 
      digitalWrite(ledPins[i*2+1], LOW); 
    }
    if (digitalRead(btnSeinG[i]) == LOW) {
      stelWisselsVoorSein(i);
      delay(200);
      if (magSeinGroen(i)) { 
        digitalWrite(ledPins[i*2], LOW); 
        digitalWrite(ledPins[i*2+1], HIGH); 
      }
    }
  }

  // --- ZONE 2: Commando's ontvangen (Slave & Timer start) ---
  if (Serial.available() > 0) {
    String commando = Serial.readStringUntil('\n');
    commando.trim();
    
    // De VEILIGHEIDS-commando's (REED5_C, D, E)
    if (commando == "REED5_C") {
      digitalWrite(ledPins[0], HIGH); digitalWrite(ledPins[1], LOW);
      digitalWrite(ledPins[2], HIGH); digitalWrite(ledPins[3], LOW);
      stelWisselsVoorSein(3);
      digitalWrite(ledPins[6], LOW); digitalWrite(ledPins[7], HIGH);
    }
    else if (commando == "REED6_D") {
      digitalWrite(ledPins[4], HIGH); digitalWrite(ledPins[5], LOW);
      digitalWrite(ledPins[6], HIGH); digitalWrite(ledPins[7], LOW);
    }
    else if (commando == "REED_E") {
      digitalWrite(ledPins[8], HIGH); digitalWrite(ledPins[9], LOW);
      digitalWrite(ledPins[10], HIGH); digitalWrite(ledPins[11], LOW);
      digitalWrite(ledPins[12], HIGH); digitalWrite(ledPins[13], LOW);
    }
    // De TIMER-commando's (REED5_M, N)
    else if (commando == "REED5_M") { startTijd[0] = millis(); timerActief[0] = true; Serial.println("Timer S5 gestart"); }
    else if (commando == "REED6_N") { startTijd[1] = millis(); timerActief[1] = true; Serial.println("Timer S6 gestart"); }
    // Bezetmeldingen
    else if (commando.startsWith("BM")) { 
       int id = commando.substring(2).toInt();
       if (id >= 5 && id <= 11) {
         int index = id - 5;
         bezetStatus[index] = !bezetStatus[index];
         if (bezetStatus[index]) bitSet(regData[2], index); else bitClear(regData[2], index);
         updateRegisters();
       }
    }
  }

// --- ZONE 3: Timer aftellen EN display aansturen ---
  if (digitalRead(A15) == LOW) { // Schakelaar AAN
    for (int i = 0; i < 2; i++) {
      if (timerActief[i]) {
        unsigned long verstreken = millis() - startTijd[i];
        int resterend = (duur > verstreken) ? (duur - verstreken) / 1000 : 0;
        
        static int oudeSeconde[2] = {99, 99}; 
        if (resterend != oudeSeconde[i]) {
          updateDisplay(i, resterend);
          Serial.print("Spoor "); Serial.print(i + 5); 
          Serial.print(" - Resterend: "); Serial.println(resterend);
          oudeSeconde[i] = resterend;
        }

        if (verstreken >= duur) {
          if (i == 0) { 
            stelWisselsVoorSein(1); 
            digitalWrite(ledPins[2], LOW); 
            digitalWrite(ledPins[3], HIGH); 
          } else { 
            stelWisselsVoorSein(0); 
            digitalWrite(ledPins[0], LOW); 
            digitalWrite(ledPins[1], HIGH); 
          }
          timerActief[i] = false;
        }
      }
    }
  } else {
    // Schakelaar UIT: zet de displays op "uit"
    regData[3] = 0b11111111;
    regData[4] = 0b11111111;
    updateRegisters();
  }
}