#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_MCP23X17.h>

// --- I2C Adressen ---
LiquidCrystal_I2C lcd(0x27, 20, 4); 
Adafruit_MCP23X17 mcp_relais;     // Adresse 0x20 (Weichen)
Adafruit_MCP23X17 mcp_gleisstrom; // Adresse 0x22 (Strom & Umpolung)
Adafruit_MCP23X17 mcp_eingaenge;  // Adresse 0x23 (Sensoren & Taster)

// --- PIN-DEFINITIONEN EINGÄNGE (mcp_eingaenge) ---
const int SENSOR_1 = 0; // IN1
const int SENSOR_2 = 1; // IN2
const int SENSOR_3 = 2; // IN3
const int SENSOR_4 = 3; // IN4

const int TASTER_TEST_1 = 4; // IN5
const int TASTER_TEST_2 = 5; // IN6
const int TASTER_TEST_3 = 6; // IN7
const int TASTER_TEST_4 = 7; // IN8

const int TASTER_START = 8;    // IN9
const int SCHALTER_ENDLOS = 9; // IN10

// --- PIN-DEFINITIONEN WEICHEN (mcp_relais) ---
const int W1_GERADE = 0; const int W1_ABZWEIG = 1;
const int W2_GERADE = 2; const int W2_ABZWEIG = 3;
const int W3_GERADE = 4; const int W3_ABZWEIG = 5;
const int W4_GERADE = 6; const int W4_ABZWEIG = 7;

// --- PIN-DEFINITIONEN STROM (mcp_gleisstrom) ---
const int STROM_GL1 = 0; // OUT9
const int STROM_GL2 = 1; // OUT10
const int STROM_GL3 = 2; // OUT11
const int STROM_GL4 = 3; // OUT12
const int POLWENDE_A = 4; // OUT13
const int POLWENDE_B = 5; // OUT14

// --- Konfiguration Auslöse-Pegel ---
// HIGH = 5V liegen an (Taster gedrückt oder Sensor löst aus)
const int AUSLOESE_PEGEL = HIGH; 


void setup() {
  Serial.begin(9600);
  Wire.begin();
  lcd.init(); lcd.backlight();
  
  lcd.setCursor(0, 0); lcd.print("Anlage startet...");

  mcp_relais.begin_I2C(0x20);
  mcp_gleisstrom.begin_I2C(0x22);
  mcp_eingaenge.begin_I2C(0x23);

  // Eingänge setzen
  for(int i = 0; i <= 9; i++) {
    mcp_eingaenge.pinMode(i, INPUT);
  }

  // Ausgänge setzen & Grundzustand (Strom AUS)
  for(int i = 0; i <= 7; i++) {
    mcp_relais.pinMode(i, OUTPUT);
    mcp_relais.digitalWrite(i, LOW);
  }
  for(int i = 0; i <= 5; i++) {
    mcp_gleisstrom.pinMode(i, OUTPUT);
    mcp_gleisstrom.digitalWrite(i, LOW);
  }

  lcd.clear();
}


void loop() {
  lcd.setCursor(0, 0);
  lcd.print("Bereit. Warte auf   ");
  lcd.setCursor(0, 1);
  lcd.print("Start-Taster...     ");
  lcd.setCursor(0, 2);
  lcd.print("                    "); // Zeilen leeren
  lcd.setCursor(0, 3);
  lcd.print("                    ");

  // 1. Warten auf Startfreigabe (Taster oder Endlos-Modus aktiv)
  while (true) {
    if (mcp_eingaenge.digitalRead(TASTER_START) == AUSLOESE_PEGEL) {
      break; // Start-Taster gedrückt
    }
    // Wenn Schalter auf Endlos steht und wir hier landen, starten wir auch sofort
    if (mcp_eingaenge.digitalRead(SCHALTER_ENDLOS) == AUSLOESE_PEGEL) {
      break; 
    }
    delay(100);
  }

  // --- DER ABLAUF ---
  
  // A) Fahrt von 1 nach 2
  ablaufSchritt("Fahrt 1 -> 2", W1_GERADE, W2_GERADE, -1, STROM_GL1, STROM_GL2, SENSOR_2, TASTER_TEST_2, "vorwaerts");

  // B) Fahrt von 4 nach 1
  // Hinweis: Passe W4, W3, W1 an deine exakte Verschaltung (Gerade/Abzweig) an!
  ablaufSchritt("Fahrt 4 -> 1", W4_ABZWEIG, W3_ABZWEIG, W1_ABZWEIG, STROM_GL4, STROM_GL1, SENSOR_1, TASTER_TEST_1, "rueckwaerts");

  // C) Fahrt von 3 nach 4
  ablaufSchritt("Fahrt 3 -> 4", W3_GERADE, W4_GERADE, -1, STROM_GL3, STROM_GL4, SENSOR_4, TASTER_TEST_4, "vorwaerts");

  // D) Fahrt von 2 nach 3
  ablaufSchritt("Fahrt 2 -> 3", W2_ABZWEIG, W3_ABZWEIG, -1, STROM_GL2, STROM_GL3, SENSOR_3, TASTER_TEST_3, "rueckwaerts");

  // Kurze Pause vor dem nächsten möglichen Durchlauf
  delay(2000); 
}


// ==============================================================================
// HILFSFUNKTIONEN FÜR DEN ABLAUF
// ==============================================================================

// Führt einen kompletten Teil-Ablauf durch
void ablaufSchritt(String text, int weicheA, int weicheB, int weicheC, int stromStart, int stromZiel, int sensorZiel, int tasterZiel, String richtung) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(text);
  
  lcd.setCursor(0, 1);
  lcd.print("Stelle Weichen...");
  schalteWeiche(weicheA);
  schalteWeiche(weicheB);
  if (weicheC != -1) schalteWeiche(weicheC); // Manche Routen brauchen 3 Weichen

  lcd.setCursor(0, 1);
  lcd.print("Fahrtrichtung: " + richtung.substring(0, 5));
  setzeFahrtrichtung(richtung);
  delay(500); // Kurz warten, damit Relais sicher umgeschaltet haben

  lcd.setCursor(0, 2);
  lcd.print("Strom AN. Zug fahrt.");
  mcp_gleisstrom.digitalWrite(stromStart, HIGH);
  mcp_gleisstrom.digitalWrite(stromZiel, HIGH);

  lcd.setCursor(0, 3);
  lcd.print("Warte auf Ankunft...");
  
  // Blockieren, bis Zug ankommt
  warteAufZug(sensorZiel, tasterZiel);

  // Zug ist da! Alles abschalten
  lcd.setCursor(0, 3);
  lcd.print("Zug angekommen!     ");
  mcp_gleisstrom.digitalWrite(stromStart, LOW);
  mcp_gleisstrom.digitalWrite(stromZiel, LOW);
  
  delay(2000); // 2 Sekunden durchatmen vor dem nächsten Schritt
}

// Wartet blockierend, bis Sensor oder Taster anschlägt
void warteAufZug(int sensorPin, int tasterPin) {
  while (true) {
    if (mcp_eingaenge.digitalRead(sensorPin) == AUSLOESE_PEGEL || mcp_eingaenge.digitalRead(tasterPin) == AUSLOESE_PEGEL) {
      delay(50); // Entprellen
      return;    // Schleife und Funktion beenden
    }
    delay(50);
  }
}

void schalteWeiche(int relaisPin) {
  mcp_relais.digitalWrite(relaisPin, HIGH);
  delay(300); // 300ms Impuls für Piko-Antriebe
  mcp_relais.digitalWrite(relaisPin, LOW);
  delay(200); 
}

void setzeFahrtrichtung(String richtung) {
  if (richtung == "vorwaerts") {
    mcp_gleisstrom.digitalWrite(POLWENDE_A, LOW);
    mcp_gleisstrom.digitalWrite(POLWENDE_B, LOW);
  } else if (richtung == "rueckwaerts") {
    mcp_gleisstrom.digitalWrite(POLWENDE_A, HIGH);
    mcp_gleisstrom.digitalWrite(POLWENDE_B, HIGH);
  }
}