int roteLEDx = 2;
int gelbeLEDx = 3;
int grueneLEDx = 4;  // LED's der Ampel Achse X (Auto)

int roteLEDy = 5;
int gelbeLEDy = 6;
int grueneLEDy = 7;  // LED's der Ampel Achse Y (Auto)

int roteLEDFU = 8;
int grueneLEDFU = 9; // LED für Fussgänger

int FussgaengerTaster = 11; // Fussgänger-Taster

int Naeherungssensor = 10;  // Näherungssensor für den Bus X

// Variablen zur Steuerung des Systems
bool BusErkannt = false;   // Flag, ob der Bus erkannt wurde
bool BusRichtungX = true;  // True: Bus soll X priorisieren

// Zeitslot-Steuerung
unsigned int Zeitslot = 0;  // Aktueller Zeitslot
unsigned long Startzeit_Zeitslot_ms = 0;  // Startzeit des aktuellen Zeitslots
unsigned long Startzeit_FU_Zeitslot_ms = 0; // Startzeit des Fussgänger-Zeitslots
unsigned int FU_Zeitslot = 0;  // Zeit für Fussgänger

// Phasenzeiten in Zeitslots (1 Zeitslot = 1000 ms)
const int ROT_PHASE_X = 30;     // 30 Sekunden Rot für Achse X, wenn Y durchläuft
const int ROT_PHASE_Y = 30;     // 30 Sekunden Rot für Achse Y, wenn X durchläuft
const int GELB_PHASE = 2;       // 3 Sekunden Gelb für beide Achsen (vor und nach Grün)
const int GRUEN_PHASE = 20;     // 20 Sekunden Grün für beide Achsen
const int GRUEN_PHASE_FU = 20;  // 20 Sekunden Grün für Fussgänger
const int ROT_PHASE_FU = 5;     // 5 Sekunden Rot, bevor auf Grün für Fussgänger geschaltet wird

// Logik für den Status der Fussgängerampel
bool FussgaengerAmpelAktiv = false;
bool FussgaengerTasterGedrueckt = false;  // Speichert, ob der Taster gedrückt wurde
bool FussgaengerZyklusBeendet = true;     // Verhindert mehrfaches Drücken während des Zyklus

void setup() {
  // Setze die LED's für die Achsen X und Y auf Ausgang
  pinMode(roteLEDx, OUTPUT);
  pinMode(gelbeLEDx, OUTPUT);
  pinMode(grueneLEDx, OUTPUT);
  pinMode(roteLEDy, OUTPUT);
  pinMode(gelbeLEDy, OUTPUT);
  pinMode(grueneLEDy, OUTPUT);

  // Setze die LED's der Fussgängerampel auf Ausgang
  pinMode(roteLEDFU, OUTPUT);
  pinMode(grueneLEDFU, OUTPUT);

  // Setze den Fussgänger-Taster auf Eingang
  pinMode(FussgaengerTaster, INPUT_PULLUP);
  // Setze den Näherungssensor auf Eingang
  pinMode(Naeherungssensor, INPUT); 

  // Initialisiere die serielle Kommunikation für Debug-Ausgaben
  Serial.begin(9600);

  // Setze das System in den Anfangszustand
  Initzustand();
}
void loop() {
  unsigned long aktuelleMillis = millis();  // Aktuelle Zeit abrufen
  // Überprüfe, ob der Bus erkannt wird und entsprechende Task ausführen
  Task_BusErkennung();
  // Wenn kein Bus erkannt wurde, reguliere die normale Ampelschaltung
  if (!BusErkannt) {
    Task_ZeitslotAktualisierung(aktuelleMillis);  // Zeitslot aktualisieren
    Task_FussgaengerTaster(aktuelleMillis);       // Fussgänger-Taster abfragen
    Task_AmpelschaltungOderFussgaengerAmpel();    // Normale Ampelschaltung oder Fussgängersteuerung
  }
  // Gib den aktuellen Zeitslot aus (optional für Debugging)
  Task_ZeitslotAusgabe();
}
// Funktion setzt die Ampeln in den Anfangszustand
void Initzustand() {
  // Ampel Achse X auf Rot setzen
  digitalWrite(roteLEDx, HIGH);
  digitalWrite(gelbeLEDx, LOW);
  digitalWrite(grueneLEDx, LOW);

  // Ampel Achse Y auf Rot setzen
  digitalWrite(roteLEDy, HIGH);
  digitalWrite(gelbeLEDy, LOW);
  digitalWrite(grueneLEDy, LOW);

  // Fussgänger-Ampel auf Rot setzen
  digitalWrite(roteLEDFU, HIGH);
  digitalWrite(grueneLEDFU, LOW);
}
// Funktion zur Priorisierung der Bus-Richtung
void Task_BusPriorisierung() {
  unsigned long aktuelleMillis = millis();
  // Aktualisiere den Zeitslot auch während der Buspriorisierung
  if (aktuelleMillis - Startzeit_Zeitslot_ms >= 1000) {
    Zeitslot++;
    Startzeit_Zeitslot_ms = aktuelleMillis;
    Serial.print("Bus Priorisierung - Aktueller Zeitslot: ");
    Serial.println(Zeitslot);
  }
  // Verwende den Zeitslot wie in der normalen Steuerung, aber nur für Achse X (Buspriorisierung)
  Task_AmpelX();  // Führe nur die Ampelschaltung für X aus
  // Wenn der Zeitslot über 30 geht, ist der Bus durch
  if (Zeitslot > 30) {
    BusErkannt = false;  // Buspriorisierung beenden
    FussgaengerTasterGedrueckt = false;  // Fussgänger-Taster zurücksetzen
    Serial.println("Bus durch. Normale Ampelschaltung wird fortgesetzt.");
  }
}
// Funktion zur Erkennung des Busses
void Task_BusErkennung() {
  if (digitalRead(Naeherungssensor) == HIGH) {
    BusErkannt = true;
    FussgaengerAmpelAktiv = false;
    FussgaengerZyklusBeendet = true;
    FU_Zeitslot = 0;
    Zeitslot = 0;
    Startzeit_Zeitslot_ms = millis();
    
    // Y-Ampel auf Rot schalten
    digitalWrite(grueneLEDy, LOW);
    digitalWrite(gelbeLEDy, LOW);
    digitalWrite(roteLEDy, HIGH);

    // Fussgängerampel auf Rot schalten
    digitalWrite(grueneLEDFU, LOW);
    digitalWrite(roteLEDFU, HIGH);

    Serial.println("Bus erkannt! Priorisiere Richtung X.");
  }

  // Priorisiere X-Ampel, wenn Bus erkannt wurde
  if (BusErkannt) {
    Task_BusPriorisierung();
  }
}
// Funktion zur Aktualisierung des Zeitslots
void Task_ZeitslotAktualisierung(unsigned long aktuelleMillis) {
  if (aktuelleMillis - Startzeit_Zeitslot_ms >= 1000) {
    Zeitslot++;
    Startzeit_Zeitslot_ms = aktuelleMillis;
    Serial.print("Aktueller Zeitslot: ");
    Serial.println(Zeitslot);
  }
}
// Funktion zur Abfrage des Fussgänger-Tasters
void Task_FussgaengerTaster(unsigned long aktuelleMillis) {
  if (digitalRead(FussgaengerTaster) == LOW && !FussgaengerTasterGedrueckt && FussgaengerZyklusBeendet) {
    FussgaengerTasterGedrueckt = true;
    FussgaengerAmpelAktiv = true;
    FussgaengerZyklusBeendet = false;
    FU_Zeitslot = 0;
    Startzeit_FU_Zeitslot_ms = aktuelleMillis;
    Serial.println("Fussgängerampel aktiviert!");
  }
}
void Task_AmpelschaltungOderFussgaengerAmpel() {
  if (FussgaengerAmpelAktiv) {
    Task_FussgaengerAmpel();
  } else {
    Task_Ampelschaltung();
  }
}
// Funktion zur Steuerung der Fussgängerampel
void Task_FussgaengerAmpel() {
  unsigned long aktuelleMillis = millis();
  // Zuerst: Alle Ampeln auf Rot schalten
  if (FU_Zeitslot == 0 && aktuelleMillis - Startzeit_FU_Zeitslot_ms >= 0) {
    digitalWrite(roteLEDx, HIGH);
    digitalWrite(gelbeLEDx, LOW);
    digitalWrite(grueneLEDx, LOW);

    digitalWrite(roteLEDy, HIGH);
    digitalWrite(gelbeLEDy, LOW);
    digitalWrite(grueneLEDy, LOW);

    digitalWrite(roteLEDFU, HIGH);
    digitalWrite(grueneLEDFU, LOW);
    FU_Zeitslot++;  // Phase 1: Alle Ampeln auf Rot
    Startzeit_FU_Zeitslot_ms = millis();  // Timer für Sicherheitsrot
  }
  // Sicherheitsrot für 5 Sekunden
  if (FU_Zeitslot == 1 && aktuelleMillis - Startzeit_FU_Zeitslot_ms >= ROT_PHASE_FU * 1000) {
    digitalWrite(roteLEDFU, LOW);
    digitalWrite(grueneLEDFU, HIGH);
    FU_Zeitslot++;  // Phase 2: Fussgänger auf Grün
    Startzeit_FU_Zeitslot_ms = millis();  // Timer für Fussgänger-Grün
  }
  // Fussgänger auf Grün für 20 Sekunden
  if (FU_Zeitslot == 2 && aktuelleMillis - Startzeit_FU_Zeitslot_ms >= GRUEN_PHASE_FU * 1000) {
    digitalWrite(grueneLEDFU, LOW);
    digitalWrite(roteLEDFU, HIGH);
    FU_Zeitslot++;  // Phase 3: Zurück zu Rot für Fussgänger
    Startzeit_FU_Zeitslot_ms = millis();  // Timer für Sicherheitsrot
  }
  // Nach weiteren 5 Sekunden Sicherheitsrot
  if (FU_Zeitslot == 3 && aktuelleMillis - Startzeit_FU_Zeitslot_ms >= ROT_PHASE_FU * 1000) {
    FussgaengerAmpelAktiv = false;  // Zyklus beendet
    FussgaengerTasterGedrueckt = false;  // Taster kann wieder gedrückt werden
    FussgaengerZyklusBeendet = true;  // Zyklus ist wieder frei
  }
}
// Funktion zur Steuerung der normalen Ampelschaltung
void Task_Ampelschaltung() {
  // (alle 60 Sekunden wird der Zyklus gewechselt)
  if (Zeitslot % 60 < 30) {  // Achse X durchläuft den Zyklus, Y bleibt auf Rot
    Task_AmpelX();
    // Setze Y auf Rot
    digitalWrite(roteLEDy, HIGH);
    digitalWrite(gelbeLEDy, LOW);
    digitalWrite(grueneLEDy, LOW);
  } else {  // Achse Y durchläuft den Zyklus, X bleibt auf Rot
    Task_AmpelY();
    // Setze X auf Rot
    digitalWrite(roteLEDx, HIGH);
    digitalWrite(gelbeLEDx, LOW);
    digitalWrite(grueneLEDx, LOW);
  }
}
// Ampelschaltung für Achse X
void Task_AmpelX() {
  // Achse X: Rot -> Gelb -> Grün -> Gelb -> Rot
  int phase = Zeitslot % 30;
  if (phase < ROT_PHASE_X - 2 * GELB_PHASE - GRUEN_PHASE) {  // Rot für Achse X
    digitalWrite(roteLEDx, HIGH);
    digitalWrite(gelbeLEDx, LOW);
    digitalWrite(grueneLEDx, LOW);
  } else if (phase < ROT_PHASE_X - GRUEN_PHASE - GELB_PHASE) {  // Gelb vor Grün für Achse X
    digitalWrite(roteLEDx, LOW);
    digitalWrite(gelbeLEDx, HIGH);
    digitalWrite(grueneLEDx, LOW);
  } else if (phase < ROT_PHASE_X - GELB_PHASE) {  // Grün für Achse X
    digitalWrite(roteLEDx, LOW);
    digitalWrite(gelbeLEDx, LOW);
    digitalWrite(grueneLEDx, HIGH);
  } else {  // Gelb nach Grün für Achse X
    digitalWrite(roteLEDx, LOW);
    digitalWrite(gelbeLEDx, HIGH);
    digitalWrite(grueneLEDx, LOW);
  }
}
// Ampelschaltung für Achse Y
void Task_AmpelY() {
  // Achse Y: Rot -> Gelb -> Grün -> Gelb -> Rot
  int phase = Zeitslot % 30;
  if (phase < ROT_PHASE_Y - 2 * GELB_PHASE - GRUEN_PHASE) {  // Rot für Achse Y
    digitalWrite(roteLEDy, HIGH);
    digitalWrite(gelbeLEDy, LOW);
    digitalWrite(grueneLEDy, LOW);
  } else if (phase < ROT_PHASE_Y - GRUEN_PHASE - GELB_PHASE) {  // Gelb vor Grün für Achse Y
    digitalWrite(roteLEDy, LOW);
    digitalWrite(gelbeLEDy, HIGH);
    digitalWrite(grueneLEDy, LOW);
  } else if (phase < ROT_PHASE_Y - GELB_PHASE) {  // Grün für Achse Y
    digitalWrite(roteLEDy, LOW);
    digitalWrite(gelbeLEDy, LOW);
    digitalWrite(grueneLEDy, HIGH);
  } else {  // Gelb nach Grün für Achse Y
    digitalWrite(roteLEDy, LOW);
    digitalWrite(gelbeLEDy, HIGH);
    digitalWrite(grueneLEDy, LOW);
  }
}
//Ausgabe des aktuellen Zeitslots
void Task_ZeitslotAusgabe() {
  Serial.print("Zeitslot: ");
  Serial.println(Zeitslot);
}