#include <Arduino.h>

// --- PIN DEFINITIES ---
const int DS_PIN = 53; const int STCP_PIN = 52; const int SHCP_PIN = 51; 
const int SR4_DS = A4; const int SR4_STCP = A5; const int SR4_SHCP = A6; 
const int SR5_DS = A7; const int SR5_STCP = A8; const int SR5_SHCP = A9; 

// --- VARIABELEN ---
byte regData[5] = {0, 0, 0, B11111111, B11111111}; 
bool bezetStatus[8] = {false, false, false, false, false, false, false, false};
byte cijfers[] = {B11000000, B11111001, B10100100, B10110000, B10011001, B10010010, B10000010, B11111000, B10000000, B10010000};

// Onafhankelijke timer variabelen
unsigned long timerStart5 = 0; bool timerActief5 = false; int displayWaarde5 = 99;
unsigned long timerStart6 = 0; bool timerActief6 = false; int displayWaarde6 = 99;

int activeGroepSein = 0; 
unsigned int schakelPauze = 2000;

// --- VOORUITGELOPEN FUNCTIES ---
void updateRegisters();
void setReg(int regNr, int bitNr, bool status);
void setWisselInitial(int nr, bool recht);
void setSeinInitial(int nr, bool groen);
void setSeinDirect(int nr, bool groen);
void startMasterTimer(int spoor);
void setBezetmelder(int nr);
void resetSysteem();

void setup() {
  Serial.begin(9600);
  pinMode(DS_PIN, OUTPUT); pinMode(STCP_PIN, OUTPUT); pinMode(SHCP_PIN, OUTPUT);
  pinMode(A15, INPUT_PULLUP);
  pinMode(SR4_DS, OUTPUT); pinMode(SR4_STCP, OUTPUT); pinMode(SR4_SHCP, OUTPUT);
  pinMode(SR5_DS, OUTPUT); pinMode(SR5_STCP, OUTPUT); pinMode(SR5_SHCP, OUTPUT);
  
  for(int i=2; i<=11; i++) pinMode(i, INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);
  for(int i=22; i<=39; i++) pinMode(i, INPUT_PULLUP);
  for(int i=40; i<=49; i++) { pinMode(i, OUTPUT); digitalWrite(i, HIGH); }
  for(int i=A0; i<=A3; i++) { pinMode(i, OUTPUT); digitalWrite(i, HIGH); }
  
  regData[3] = B11111111; regData[4] = B11111111;
  updateRegisters();
  
  for(int w=4; w<=10; w++) setWisselInitial(w, true);
  for(int s=5; s<=11; s++) setSeinInitial(s, false);
  
  Serial.println("Systeem gestart.");
}

void loop() {
  if (digitalRead(12) == LOW) { resetSysteem(); delay(500); }

  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n'); input.trim();
    if (input == "REED5_M") startMasterTimer(5);
    if (input == "REED6_N") startMasterTimer(6);
    if (input.startsWith("BM")) setBezetmelder(input.substring(2).toInt());
  }

  // Timer 5 (SR4)
  if (timerActief5) {
    unsigned long verstreken = millis() - timerStart5;
    int rest = 5 - (verstreken / 1000);
    if (rest >= 0 && displayWaarde5 != rest) {
      displayWaarde5 = rest;
      regData[3] = cijfers[displayWaarde5];
      updateRegisters();
    }
    if (verstreken >= 5000) {
      setSeinDirect(5, true);
      regData[3] = B11111111;
      updateRegisters();
      timerActief5 = false;
    }
  }

  // Timer 6 (SR5)
  if (timerActief6) {
    unsigned long verstreken = millis() - timerStart6;
    int rest = 5 - (verstreken / 1000);
    if (rest >= 0 && displayWaarde6 != rest) {
      displayWaarde6 = rest;
      regData[4] = cijfers[displayWaarde6];
      updateRegisters();
    }
    if (verstreken >= 5000) {
      setSeinDirect(6, true);
      regData[4] = B11111111;
      updateRegisters();
      timerActief6 = false;
    }
  }
}

// --- LOGICA FUNCTIES ---
void startMasterTimer(int spoor) {
  if (digitalRead(A15) == LOW) {
    if (spoor == 5) { timerStart5 = millis(); timerActief5 = true; }
    if (spoor == 6) { timerStart6 = millis(); timerActief6 = true; }
  } else {
    setSeinDirect(spoor, true);
  }
}

void setReg(int regNr, int bitNr, bool status) {
  if (status) regData[regNr] |= (1 << bitNr);
  else regData[regNr] &= ~(1 << bitNr);
}

void setBezetmelder(int nr) {
  if (nr >= 5 && nr <= 11) {
    int index = nr - 5;
    bezetStatus[index] = !bezetStatus[index];
    setReg(2, index, bezetStatus[index]);
    updateRegisters();
  }
}

void updateRegisters() {
  digitalWrite(STCP_PIN, LOW);
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[2]); 
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[1]); 
  shiftOut(DS_PIN, SHCP_PIN, MSBFIRST, regData[0]); 
  digitalWrite(STCP_PIN, HIGH);

  digitalWrite(SR4_STCP, LOW);
  shiftOut(SR4_DS, SR4_SHCP, MSBFIRST, regData[3]); 
  digitalWrite(SR4_STCP, HIGH);

  digitalWrite(SR5_STCP, LOW);
  shiftOut(SR5_DS, SR5_SHCP, MSBFIRST, regData[4]); 
  digitalWrite(SR5_STCP, HIGH);
}

void setWisselInitial(int nr, bool recht) {
  switch(nr) {
    case 4: setReg(0, 0, recht); setReg(0, 1, !recht); break;
    case 5: setReg(0, 2, recht); setReg(0, 3, !recht); break;
    case 6: setReg(0, 4, recht); setReg(0, 5, !recht); break;
    case 7: setReg(0, 6, recht); setReg(0, 7, !recht); break;
    case 8: setReg(1, 1, !recht); setReg(1, 0, recht); break;
    case 9: setReg(1, 3, !recht); setReg(1, 2, recht); break; 
    case 10: setReg(1, 5, !recht); setReg(1, 4, recht); break;
  }
}

void setSeinInitial(int nr, bool groen) {
  int pinG = 0, pinR = 0;
  switch(nr) {
    case 5: pinG = 40; pinR = 41; break; case 6: pinG = 42; pinR = 43; break;
    case 7: pinG = 44; pinR = 45; break; case 8: pinG = 46; pinR = 47; break;
    case 9: pinG = 48; pinR = 49; break; case 10: pinG = A0; pinR = A1; break;
    case 11: pinG = A2; pinR = A3; break;
  }
  if(pinG != 0) {
    if(groen) { digitalWrite(pinR, HIGH); digitalWrite(pinG, LOW); }
    else { digitalWrite(pinR, LOW); digitalWrite(pinG, HIGH); }
  }
}

void setSeinDirect(int nr, bool groen) {
  if (groen) {
    switch(nr) {
      case 5: setWisselInitial(4, false); delay(schakelPauze); setWisselInitial(5, true); delay(schakelPauze); setWisselInitial(6, false); break;
      case 6: setWisselInitial(4, true); delay(schakelPauze); setWisselInitial(5, true); delay(schakelPauze); setWisselInitial(6, true); break;
      // ... voeg hier je overige cases toe
    }
    setSeinInitial(nr, true);
  } else {
    setSeinInitial(nr, false);
  }
  updateRegisters();
}

void resetSysteem() { for(int i = 5; i <= 11; i++) setSeinDirect(i, false); activeGroepSein = 0; }