#include <TM1637Display.h>

// ==========================================================
// POLARITEIT INSTELLING (JOUW INSTELLING: AAN DE PLUS / LOW = AAN)
// ==========================================================
const int SEIN_AAN  = LOW;   
const int SEIN_UIT  = HIGH;  

// In te stellen wachttijd voor het station (bijv. 5000 ms = 5 seconden)
const unsigned long STATION_WACHTTIJD = 5000; 

// ==========================================================
// PIN DEFINITIES TM1637 DISPLAY (AANGEPAST: PIN 14 EN 15)
// ==========================================================
const int PIN_CLK = 14;
const int PIN_DIO = 15;
TM1637Display display(PIN_CLK, PIN_DIO);

// ==========================================================
// PIN DEFINITIES KATO WISSELS
// ==========================================================
const int PIN_W1_R  = 2;   const int PIN_W1_A  = 3;   // Wissel 1
const int PIN_W2_R  = 4;   const int PIN_W2_A  = 5;   // Wissel 2
const int PIN_W3_R  = 6;   const int PIN_W3_A  = 7;   // Wissel 3
const int PIN_W11_R = 8;   const int PIN_W11_A = 9;   // Wissel 11
const int PIN_W12_R = 10;  const int PIN_W12_A = 11;  // Wissel 12

// ==========================================
// PIN DEFINITIES STOP/REM RELAIS 11 T/M 18 (PIN 22 T/M 29)
// ==========================================
const int PIN_SR11 = 22; const int PIN_SR12 = 23; const int PIN_SR13 = 24; const int PIN_SR14 = 25;
const int PIN_SR15 = 26; const int PIN_SR16 = 27; const int PIN_SR17 = 28; const int PIN_SR18 = 29;

// ==========================================
// PIN DEFINITIES LICHTGROEPEN 19 T/M 24 (PIN 30 T/M 35)
// ==========================================
const int PIN_LG19 = 30; const int PIN_LG20 = 31; const int PIN_LG21 = 32;
const int PIN_LG22 = 33; const int PIN_LG23 = 34; const int PIN_LG24 = 35;

// ==========================================
// PIN DEFINITIES INPUTS: OPTO'S EN REEDS (PIN 36 T/M 43)
// ==========================================
const int PIN_OPTO1 = 36; const int PIN_OPTO2 = 37; const int PIN_OPTO3 = 38; const int PIN_OPTO4 = 39;
const int PIN_REED_A = 40; const int PIN_REED_B = 41; const int PIN_REED_K = 42; const int PIN_REED_L = 43;

bool vorigeReedA = HIGH; bool vorigeReedB = HIGH;
bool vorigeReedK = HIGH; bool vorigeReedL = HIGH;

// ==========================================
// PIN DEFINITIES VIESSMANN SEINEN (ANALOGE PINNEN)
// ==========================================
const int PIN_S1R_LED = A0; const int PIN_S1G_LED = A1; 
const int PIN_S2R_LED = A2; const int PIN_S2G_LED = A3; 
const int PIN_S3R_LED = A4; const int PIN_S3G_LED = A5; 
const int PIN_S4R_LED = A6; const int PIN_S4G_LED = A7; 

// Wissel-puls variabelen
unsigned long wisselPulsStartTijd = 0;
int actieveWisselPin = -1;
bool wisselPulsActief = false;

// Station Timer variabelen
unsigned long stationTimerStartTijd = 0;
bool stationTimerActief = false;
int spoorDatMoetVertrekken = 0; // 1 = Spoor 1, 2 = Spoor 2
int vorigeLaatsteSeconde = -1;  // Om het display niet onnodig vaak te verversen

void startWisselPuls(int pin);
void controleerWisselPuls();
void controleerStationTimer();
void setSein(int seinNummer, bool rood);
void leesMasterCommando();
void controleerInputs();

void setup() {
  Serial.begin(9600);
  delay(200);
  Serial.println("Systeem Gestart: Seinen op ROOD, KATO Wissels paraat.");
  
  // Initialiseer het display
  display.setBrightness(0x0f); // Maximale helderheid
  display.showNumberDec(0, true); // Startstand: 0000
  
  // 1. Relais 1 t/m 10 (Wissels) als OUTPUT
  for (int pin = 2; pin <= 11; pin++) {
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW); 
  }

  // 2. Stop/Rem relais 11 t/m 18 als OUTPUT
  for (int pin = 22; pin <= 29; pin++) {
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW);
  }

  // 3. Lichtgroepen 19 t/m 24 als OUTPUT
  for (int pin = 30; pin <= 35; pin++) {
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW);
  }

  // 4. Inputs als INPUT_PULLUP
  for (int pin = 36; pin <= 43; pin++) {
    pinMode(pin, INPUT_PULLUP);
  }

  // 5. Seinpinnen als OUTPUT
  int seinPins[] = { PIN_S1R_LED, PIN_S1G_LED, PIN_S2R_LED, PIN_S2G_LED, PIN_S3R_LED, PIN_S3G_LED, PIN_S4R_LED, PIN_S4G_LED };
  for (int pin : seinPins) {
    pinMode(pin, OUTPUT);
  }
  
  // Startstatus: Alle seinen op rood
  setSein(1, true); setSein(2, true); setSein(3, true); setSein(4, true); 
}

void loop() {
  controleerWisselPuls(); 
  controleerStationTimer(); 
  leesMasterCommando();   
  controleerInputs();     
}

void leesMasterCommando() {
  if (Serial.available() > 0) {
    String commando = Serial.readString();
    commando.trim(); 

    if (commando.length() == 0) return;

    int pinToTrigger = -1;

    // --- 1. SEINEN & GEKOPPELDE REMRELAIS ---
    if (commando == "S1_R")      { setSein(1, true);  digitalWrite(PIN_SR11, LOW);  digitalWrite(PIN_SR12, LOW);  }
    else if (commando == "S1_G") { setSein(1, false); digitalWrite(PIN_SR11, HIGH); digitalWrite(PIN_SR12, HIGH); }
    
    else if (commando == "S2_R") { setSein(2, true);  digitalWrite(PIN_SR13, LOW);  digitalWrite(PIN_SR14, LOW);  }
    else if (commando == "S2_G") { setSein(2, false); digitalWrite(PIN_SR13, HIGH); digitalWrite(PIN_SR14, HIGH); }
    
    else if (commando == "S3_R") { setSein(3, true);  digitalWrite(PIN_SR15, LOW);  digitalWrite(PIN_SR16, LOW);  }
    else if (commando == "S3_G") { setSein(3, false); digitalWrite(PIN_SR15, HIGH); digitalWrite(PIN_SR16, HIGH); }

    else if (commando == "S4_R") { setSein(4, true);  digitalWrite(PIN_SR17, LOW);  digitalWrite(PIN_SR18, LOW);  }
    else if (commando == "S4_G") { setSein(4, false); digitalWrite(PIN_SR17, HIGH); digitalWrite(PIN_SR18, HIGH); }

    // --- 2. KATO WISSELS ---
    else if (commando == "W1_R")  pinToTrigger = PIN_W1_R; 
    else if (commando == "W1_A")  pinToTrigger = PIN_W1_A; 
    else if (commando == "W2_R")  pinToTrigger = PIN_W2_R; 
    else if (commando == "W2_A")  pinToTrigger = PIN_W2_A; 
    else if (commando == "W3_R")  pinToTrigger = PIN_W3_R; 
    else if (commando == "W3_A")  pinToTrigger = PIN_W3_A; 
    else if (commando == "W11_R") pinToTrigger = PIN_W11_R; 
    else if (commando == "W11_A") pinToTrigger = PIN_W11_A; 
    else if (commando == "W12_R") pinToTrigger = PIN_W12_R; 
    else if (commando == "W12_A") pinToTrigger = PIN_W12_A; 

    // --- 3. LICHTGROEPEN ---
    else if (commando == "L1_A")  digitalWrite(PIN_LG19, HIGH);
    else if (commando == "L1_U")  digitalWrite(PIN_LG19, LOW);
    else if (commando == "L2_A")  digitalWrite(PIN_LG20, HIGH);
    else if (commando == "L2_U")  digitalWrite(PIN_LG20, LOW);
    else if (commando == "L3_A")  digitalWrite(PIN_LG21, HIGH);
    else if (commando == "L3_U")  digitalWrite(PIN_LG21, LOW);
    else if (commando == "L4_A")  digitalWrite(PIN_LG22, HIGH);
    else if (commando == "L4_U")  digitalWrite(PIN_LG22, LOW);
    else if (commando == "L5_A")  digitalWrite(PIN_LG23, HIGH);
    else if (commando == "L5_U")  digitalWrite(PIN_LG23, LOW);
    else if (commando == "L6_A")  digitalWrite(PIN_LG24, HIGH);
    else if (commando == "L6_U")  digitalWrite(PIN_LG24, LOW);

    // --- 4. MASTER ROUTE COMMANDO'S ---
    else if (commando == "CMD_A") {
      setSein(1, true);  digitalWrite(PIN_SR11, LOW);  digitalWrite(PIN_SR12, LOW);
      setSein(2, true);  digitalWrite(PIN_SR13, LOW);  digitalWrite(PIN_SR14, LOW);
      setSein(3, false); digitalWrite(PIN_SR15, HIGH); digitalWrite(PIN_SR16, HIGH);
    }
    else if (commando == "CMD_B") {
      setSein(3, true);  digitalWrite(PIN_SR15, LOW);  digitalWrite(PIN_SR16, LOW);
      setSein(4, true);  digitalWrite(PIN_SR17, LOW);  digitalWrite(PIN_SR18, LOW);
    }

    if (pinToTrigger != -1) {
      startWisselPuls(pinToTrigger);
    }
  }
}

void controleerInputs() {
  // --- REEDCONTACT A (BTN1) ---
  bool momenteelReedA = digitalRead(PIN_REED_A);
  if (momenteelReedA == LOW && vorigeReedA == HIGH) { 
    setSein(1, true);  digitalWrite(PIN_SR11, LOW);  digitalWrite(PIN_SR12, LOW);  
    setSein(2, true);  digitalWrite(PIN_SR13, LOW);  digitalWrite(PIN_SR14, LOW);  
    setSein(3, false); digitalWrite(PIN_SR15, HIGH); digitalWrite(PIN_SR16, HIGH); 
    Serial.println("REED_A_TRIG"); 
    delay(100); 
  }
  vorigeReedA = momenteelReedA;

  // --- REEDCONTACT B (BTN2) ---
  bool momenteelReedB = digitalRead(PIN_REED_B);
  if (momenteelReedB == LOW && vorigeReedB == HIGH) { 
    setSein(3, true);  digitalWrite(PIN_SR15, LOW);  digitalWrite(PIN_SR16, LOW);  
    setSein(4, true);  digitalWrite(PIN_SR17, LOW);  digitalWrite(PIN_SR18, LOW);  
    Serial.println("REED_B_TRIG"); 
    delay(100); 
  }
  vorigeReedB = momenteelReedB;

  // --- REEDCONTACT K (BTN3 - SPOOR 1) ---
  bool momenteelReedK = digitalRead(PIN_REED_K);
  if (momenteelReedK == LOW && vorigeReedK == HIGH) {
    stationTimerStartTijd = millis();
    stationTimerActief = true;
    spoorDatMoetVertrekken = 2; 
    vorigeLaatsteSeconde = -1; 

    Serial.println("REED_K_TRIG"); 
    Serial.println("Automatische timer INgeschakeld");
    delay(100);
  }
  vorigeReedK = momenteelReedK;

  // --- REEDCONTACT L (BTN4 - SPOOR 2) ---
  bool momenteelReedL = digitalRead(PIN_REED_L);
  if (momenteelReedL == LOW && vorigeReedL == HIGH) {
    stationTimerStartTijd = millis();
    stationTimerActief = true;
    spoorDatMoetVertrekken = 1; 
    vorigeLaatsteSeconde = -1; 

    Serial.println("REED_L_TRIG"); 
    Serial.println("Automatische timer INgeschakeld");
    delay(100);
  }
  vorigeReedL = momenteelReedL;
}

void controleerStationTimer() {
  if (stationTimerActief) {
    unsigned long verstrekenTijd = millis() - stationTimerStartTijd;
    
    if (verstrekenTijd >= STATION_WACHTTIJD) {
      if (spoorDatMoetVertrekken == 1) {
        setSein(1, false); 
        digitalWrite(PIN_SR11, HIGH); 
        digitalWrite(PIN_SR12, HIGH);
        Serial.println("-> Spoor 1 naar GROEN gezet door timer!");
      } 
      else if (spoorDatMoetVertrekken == 2) {
        setSein(2, false); 
        digitalWrite(PIN_SR13, HIGH); 
        digitalWrite(PIN_SR14, HIGH);
        Serial.println("-> Spoor 2 naar GROEN gezet door timer!");
      }

      display.showNumberDec(0, true);
      stationTimerActief = false;
      spoorDatMoetVertrekken = 0;
    } 
    else {
      int resterendeSeconden = ((STATION_WACHTTIJD - verstrekenTijd) + 999) / 1000;
      if (resterendeSeconden != vorigeLaatsteSeconde) {
        display.showNumberDec(resterendeSeconden, true);
        vorigeLaatsteSeconde = resterendeSeconden;
      }
    }
  }
}

void startWisselPuls(int pin) {
  if (wisselPulsActief && actieveWisselPin != -1) {
    digitalWrite(actieveWisselPin, LOW);
  }
  actieveWisselPin = pin;
  wisselPulsStartTijd = millis();
  wisselPulsActief = true;
  digitalWrite(pin, HIGH); 
}

void controleerWisselPuls() {
  if (wisselPulsActief && (millis() - wisselPulsStartTijd >= 150)) {
    digitalWrite(actieveWisselPin, LOW); 
    wisselPulsActief = false;
    actieveWisselPin = -1;
  }
}

void setSein(int seinNummer, bool rood) {
  int pinRood = -1; int pinGroen = -1;
  if (seinNummer == 1)      { pinRood = PIN_S1R_LED; pinGroen = PIN_S1G_LED; }
  else if (seinNummer == 2) { pinRood = PIN_S2R_LED; pinGroen = PIN_S2G_LED; }
  else if (seinNummer == 3) { pinRood = PIN_S3R_LED; pinGroen = PIN_S3G_LED; }
  else if (seinNummer == 4) { pinRood = PIN_S4R_LED; pinGroen = PIN_S4G_LED; }

  if (pinRood != -1 && pinGroen != -1) {
    if (rood) {
      digitalWrite(pinRood, SEIN_AAN); digitalWrite(pinGroen, SEIN_UIT); 
    } else {
      digitalWrite(pinRood, SEIN_UIT);  digitalWrite(pinGroen, SEIN_AAN); 
    }
  }
}