/*******************************************************************************
 * BitBiber ByteBuddy - Technik entdecken, Barrieren überwinden.
 * Version: 1.0
 * 
 * Entwickelt von: BitBiber TechTalente e.V.
 * Adresse: Schweidnitzer Str. 17, 45891 Gelsenkirchen
 * Kontakt: maik@bitbiber.de
 * 
 * Beschreibung:
 * ByteBuddy ist ein Lernroboter, der mit RFID-Karten programmiert werden kann.
 * Kinder können durch einfaches Auflegen von Karten Bewegungsabläufe erstellen
 * und dabei spielerisch Programmierkonzepte wie Schleifen und Bedingungen lernen.
 * 
 * Copyright (c) 2026 BitBiber TechTalente e.V.
 * Alle Rechte vorbehalten.
 ******************************************************************************/

#include <SPI.h>
#include <MFRC522.h>
#include <Servo.h>

/* ==================== SYSTEMKONSTANTEN ==================== */

// Software-Informationen
const char* SOFTWARE_VERSION = "1.0";
const char* ORGANIZATION = "BitBiber TechTalente e.V.";
const char* ROBOT_MODEL = "ByteBuddy";

/* ==================== HARDWARE-PINS ==================== */

// RFID-Kartenleser Konfiguration
#define RST_PIN 5    // Reset-Pin für RFID
#define SS_PIN  8    // Slave-Select-Pin für RFID

// Pin-Definitionen für alle Hardware-Komponenten
const int PIN_TASTER        = A0;  // Analoger Eingang für die 6 Taster
const int PIN_SERVO_LINKS   = 9;   // PWM-Pin für linken Servomotor
const int PIN_SERVO_RECHTS  = 10;  // PWM-Pin für rechten Servomotor
const int PIN_SUMMER        = 3;   // Pin für den Piezo-Summer
const int PIN_ULTRA_TRIG    = 7;   // Trigger-Pin für Ultraschallsensor
const int PIN_ULTRA_ECHO    = 6;   // Echo-Pin für Ultraschallsensor

// RFID-Leser Objekt erstellen
MFRC522 mfrc522(SS_PIN, RST_PIN);

/* ==================== BEWEGUNGSKOMMANDOS ==================== */

// Definition der vier Grundbewegungen
const uint8_t AKTION_VOR      = 0;  // Vorwärts fahren
const uint8_t AKTION_ZURUECK  = 1;  // Rückwärts fahren
const uint8_t AKTION_LINKS    = 2;  // Nach links drehen
const uint8_t AKTION_RECHTS   = 3;  // Nach rechts drehen

/* ==================== SERVO-EINSTELLUNGEN ==================== */

// Servo-Neutralstellungen (1500 = Stillstand bei Continuous Rotation Servos)
const int STOPP_L = 1500;  // Neutralstellung linker Servo
const int STOPP_R = 1487;  // Neutralstellung rechter Servo

// Geschwindigkeit (Abweichung von der Neutralstellung)
const int GESCHWINDIGKEIT = 250;  // Je höher, desto schneller

/* ==================== TIMING-EINSTELLUNGEN ==================== */

const int  TOLERANZ                = 3;     // Toleranz für Taster-Erkennung
const int  SICHERHEITS_ABSTAND_CM  = 10;    // Mindestabstand zu Hindernissen
const unsigned long ENTPRELL_MS    = 200;   // Entprellzeit für Taster
const unsigned long IDLE_DETACH_MS = 800;   // Nach dieser Zeit werden inaktive Servos abgeschaltet
const int  ACTION_MS               = 600;   // Dauer einer Bewegungsaktion in Millisekunden

/* ==================== TASTER-KONFIGURATION ==================== */

// Struktur für die Taster-Definition
struct Taster {
  const char* name;  // Name des Tasters
  int wert;         // Analogwert wenn gedrückt
  int ton;          // Frequenz des Feedback-Tons
};

// Die 6 Taster mit ihren spezifischen Werten
Taster tasterListe[] = {
  {"Aufnahme", 18, 880},   // Startet/stoppt Aufnahme
  {"Starten",   0, 988},   // Startet Programmausführung
  {"Vor",       155, 1047},  // Vorwärts-Befehl
  {"Zurueck",   82, 740},   // Rückwärts-Befehl
  {"Links",    40, 659},   // Links-Befehl
  {"Rechts",   662, 831}    // Rechts-Befehl
};

const int ANZAHL_TASTER = sizeof(tasterListe) / sizeof(tasterListe[0]);

/* ==================== SERVO-VERWALTUNG ==================== */

// Servo-Objekte
Servo servoLinks, servoRechts;
bool servosAngeschlossen = false;  // Status: Sind Servos aktiv?

// Bewegungszustände des Roboters
enum Bewegungszustand { 
  ZUSTOPP,     // Roboter steht
  ZUVOR,       // Roboter fährt vorwärts
  ZUZURUECK,   // Roboter fährt rückwärts
  ZULINKS,     // Roboter dreht links
  ZURECHTS     // Roboter dreht rechts
};

Bewegungszustand bewegungszustand = ZUSTOPP;
unsigned long zeitpunktStopp = 0;  // Zeitpunkt des letzten Stopps

/* ==================== SICHERHEITSSYSTEM ==================== */

bool crashBlockiert = false;  // Flag für Hinderniserkennung
bool conditionCheckActive = false;  // Flag für aktive Bedingungsprüfung
unsigned long conditionCheckEndTime = 0;  // Zeitpunkt bis wann Sicherheit deaktiviert

/* ==================== AUFNAHME & WIEDERGABE ==================== */

bool aufnahmeAktiv = false;   // Läuft gerade eine Aufnahme?
bool abspielenAktiv = false;  // Wird gerade ein Programm abgespielt?
unsigned long stepStartMs = 0;  // Startzeit der aktuellen Aktion

/* ==================== PROGRAMMIERSPRACHE ==================== */

// Die verschiedenen Befehle, die der Roboter versteht
enum OpCode : uint8_t {
  OP_ACT = 0,              // Bewegungsaktion ausführen
  OP_FOR_BEGIN,            // Beginn einer FOR-Schleife
  OP_END,                  // Ende eines Blocks
  OP_IF_BEGIN_FREE,        // Wenn vorne frei ist
  OP_IF_BEGIN_BLOCKED,     // Wenn vorne blockiert ist
  OP_WHILE_BEGIN_FREE,     // Solange vorne frei ist
  OP_WHILE_BEGIN_BLOCKED,  // Solange vorne blockiert ist
  OP_SET_N                 // Setze Schleifenzähler
};

// Struktur für einen einzelnen Befehl
struct Instr {
  uint8_t op;     // Der Befehl
  uint8_t param;  // Parameter zum Befehl
};

// Programmspeicher
const int MAX_PROG = 128;  // Maximale Anzahl von Befehlen
Instr prog[MAX_PROG];      // Das gespeicherte Programm
int progLen = 0;           // Aktuelle Programmlänge

// Virtuelle Maschine (VM) Zustand
int pc = 0;                // Programmzähler (aktuelle Position im Programm)
bool stepActive = false;   // Wird gerade ein Schritt ausgeführt?
int pendingForN = -1;      // Wartet eine FOR-Schleife auf ihre Anzahl?

// Stack für verschachtelte Strukturen (Schleifen, Bedingungen)
struct Frame {
  uint8_t type;     // Typ des Frames (FOR, IF, WHILE)
  int beginPc;      // Startposition im Programm
  int remaining;    // Verbleibende Durchläufe (für FOR-Schleifen)
};

const int MAX_STACK = 16;  // Maximale Verschachtelungstiefe
Frame st[MAX_STACK];       // Der Stack
int sp = 0;                // Stack-Pointer

/* ==================== ULTRASCHALL-FUNKTIONEN ==================== */

/*
 * Misst die Entfernung zum nächsten Hindernis
 * Rückgabe: Entfernung in cm, oder -1 bei Fehler
 */
long messeAbstandCm() {
  // Sende Ultraschall-Puls
  digitalWrite(PIN_ULTRA_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_ULTRA_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_ULTRA_TRIG, LOW);
  
  // Messe wie lange das Echo braucht
  unsigned long dauer = pulseIn(PIN_ULTRA_ECHO, HIGH, 30000UL);
  if (dauer == 0) return -1;  // Timeout - kein Echo empfangen
  
  // Berechne Entfernung (Schall legt 58 Mikrosekunden für 1cm hin und zurück zurück)
  long cm = (long)(dauer / 58);
  if (cm <= 0 || cm > 400) return -1;  // Ungültiger Wert
  
  return cm;
}

/*
 * Prüft ob der Weg frei ist
 */
inline bool frontFree() {
  long d = messeAbstandCm();
  return (d < 0) ? true : (d >= SICHERHEITS_ABSTAND_CM + 1);
}

/*
 * Prüft ob ein Hindernis im Weg ist
 */
inline bool frontBlocked() {
  long d = messeAbstandCm();
  return (d < 0) ? false : (d <= SICHERHEITS_ABSTAND_CM);
}

/* ==================== BEWEGUNGSFUNKTIONEN ==================== */

/*
 * Aktiviert die Servomotoren falls nötig
 */
void attachWennNoetig() {
  if (!servosAngeschlossen) {
    servoLinks.attach(PIN_SERVO_LINKS);
    servoRechts.attach(PIN_SERVO_RECHTS);
    servosAngeschlossen = true;
  }
}

/*
 * Deaktiviert inaktive Servomotoren zur Energieeinsparung
 */
void detachNachIdle() {
  if (servosAngeschlossen && bewegungszustand == ZUSTOPP && 
      (millis() - zeitpunktStopp > IDLE_DETACH_MS)) {
    servoLinks.detach();
    servoRechts.detach();
    servosAngeschlossen = false;
  }
}

/*
 * Stoppt den Roboter sofort
 */
void stopp() {
  attachWennNoetig();
  servoLinks.writeMicroseconds(STOPP_L);
  servoRechts.writeMicroseconds(STOPP_R);
  bewegungszustand = ZUSTOPP;
  zeitpunktStopp = millis();
  Serial.println(F("Bewegung: STOPP"));
}

/*
 * Führt eine Bewegungsaktion aus
 */
void akt(uint8_t a) {
  attachWennNoetig();
  switch(a) {
    case AKTION_VOR:
      // Beide Räder vorwärts
      servoLinks.writeMicroseconds(STOPP_L + GESCHWINDIGKEIT);
      servoRechts.writeMicroseconds(STOPP_R - GESCHWINDIGKEIT);
      bewegungszustand = ZUVOR;
      Serial.println(F("Bewegung: VOR"));
      break;
    case AKTION_ZURUECK:
      // Beide Räder rückwärts
      servoLinks.writeMicroseconds(STOPP_L - GESCHWINDIGKEIT);
      servoRechts.writeMicroseconds(STOPP_R + GESCHWINDIGKEIT);
      bewegungszustand = ZUZURUECK;
      Serial.println(F("Bewegung: ZURUECK"));
      break;
    case AKTION_LINKS:
      // Auf der Stelle nach links drehen
      servoLinks.writeMicroseconds(STOPP_L - GESCHWINDIGKEIT);
      servoRechts.writeMicroseconds(STOPP_R - GESCHWINDIGKEIT);
      bewegungszustand = ZULINKS;
      Serial.println(F("Bewegung: LINKS"));
      break;
    case AKTION_RECHTS:
      // Auf der Stelle nach rechts drehen
      servoLinks.writeMicroseconds(STOPP_L + GESCHWINDIGKEIT);
      servoRechts.writeMicroseconds(STOPP_R + GESCHWINDIGKEIT);
      bewegungszustand = ZURECHTS;
      Serial.println(F("Bewegung: RECHTS"));
      break;
  }
}

/* ==================== EINGABE-FUNKTIONEN ==================== */

/*
 * Liest welcher Taster gedrückt wurde
 * Rückgabe: Taster-Index (0-5) oder -1 wenn kein Taster gedrückt
 */
int leseTaster() {
  int v = analogRead(PIN_TASTER);
  for (int i = 0; i < ANZAHL_TASTER; i++) {
    if (v >= tasterListe[i].wert - TOLERANZ && 
        v <= tasterListe[i].wert + TOLERANZ) {
      return i;
    }
  }
  return -1;
}

/*
 * Spielt einen Ton für den gedrückten Taster
 */
void piepFuerTaste(int idx, int ms) {
  if (idx >= 0 && idx < ANZAHL_TASTER) {
    tone(PIN_SUMMER, tasterListe[idx].ton, ms);
  }
}

/* ==================== PROGRAMM-AUSGABE ==================== */

/*
 * Zeigt das gespeicherte Programm im Serial Monitor an
 */
void progDump() {
  Serial.println(F("========================================"));
  Serial.println(F("   BitBiber ByteBuddy - Programm-Dump"));
  Serial.println(F("   Software Version 1.0"));
  Serial.println(F("========================================"));
  Serial.print(F("Programmlänge: "));
  Serial.print(progLen);
  Serial.println(F(" Befehle"));
  Serial.println(F("----------------------------------------"));
  
  if (progLen == 0) {
    Serial.println(F("[Programmspeicher ist leer]"));
  } else {
    // Zeige alle gespeicherten Befehle
    for (int i = 0; i < progLen; i++) {
      Serial.print(F("["));
      if (i < 10) Serial.print(F("0"));
      Serial.print(i);
      Serial.print(F("] "));
      
      // Übersetze Befehl in lesbare Form
      switch(prog[i].op) {
        case OP_ACT:
          Serial.print(F("AKTION      -> "));
          switch(prog[i].param) {
            case AKTION_VOR:     Serial.println(F("VOR")); break;
            case AKTION_ZURUECK: Serial.println(F("ZURUECK")); break;
            case AKTION_LINKS:   Serial.println(F("LINKS")); break;
            case AKTION_RECHTS:  Serial.println(F("RECHTS")); break;
            default: Serial.println(prog[i].param); break;
          }
          break;
        case OP_FOR_BEGIN:
          Serial.print(F("FOR_BEGIN   -> "));
          Serial.print(prog[i].param);
          Serial.println(F(" Wiederholungen"));
          break;
        case OP_END:
          Serial.println(F("END"));
          break;
        case OP_IF_BEGIN_FREE:
          Serial.println(F("IF_BEGIN    -> Wenn vorne FREI"));
          break;
        case OP_IF_BEGIN_BLOCKED:
          Serial.println(F("IF_BEGIN    -> Wenn vorne BLOCKIERT"));
          break;
        case OP_WHILE_BEGIN_FREE:
          Serial.println(F("WHILE_BEGIN -> Solange vorne FREI"));
          break;
        case OP_WHILE_BEGIN_BLOCKED:
          Serial.println(F("WHILE_BEGIN -> Solange vorne BLOCKIERT"));
          break;
        case OP_SET_N:
          Serial.print(F("SET_N       -> Setze Zähler auf "));
          Serial.println(prog[i].param);
          break;
        default:
          Serial.print(F("UNBEKANNT   -> OpCode "));
          Serial.println(prog[i].op);
          break;
      }
    }
  }
  
  // Zeige VM-Status
  Serial.println(F("----------------------------------------"));
  Serial.println(F("VM-Status:"));
  Serial.print(F("  Programmzähler: "));
  Serial.println(pc);
  Serial.print(F("  Stack-Tiefe:    "));
  Serial.println(sp);
  Serial.print(F("  Wiedergabe:     "));
  Serial.println(abspielenAktiv ? F("AKTIV") : F("INAKTIV"));
  Serial.println(F("========================================"));
}

/* ==================== VIRTUELLE MASCHINE ==================== */

/*
 * Fügt einen neuen Befehl zum Programm hinzu
 */
void addInstr(uint8_t op, uint8_t param = 0) {
  if (progLen >= MAX_PROG) {
    Serial.println(F("[FEHLER] Programmspeicher voll!"));
    return;
  }
  
  if (op == OP_FOR_BEGIN) {
    // Spezialbehandlung für FOR-Schleifen
    uint8_t n = (pendingForN > 0) ? (uint8_t)pendingForN : 1;
    prog[progLen++] = {OP_FOR_BEGIN, n};
    pendingForN = -1;
    Serial.print(F("+ FOR_BEGIN mit N="));
    Serial.println(n);
  }
  else if (op == OP_SET_N) {
    // Merke Anzahl für nächste FOR-Schleife
    pendingForN = param;
    Serial.print(F("+ SET_N auf "));
    Serial.println((int)param);
  }
  else {
    // Normale Instruktion hinzufügen
    prog[progLen++] = {op, param};
    Serial.print(F("+ Instruktion: op="));
    Serial.print(op);
    Serial.print(F(", param="));
    Serial.println(param);
  }
}

/*
 * Setzt die Virtuelle Maschine zurück
 */
void vmReset() {
  pc = 0;
  sp = 0;
  stepActive = false;
}

/*
 * Prüft ob das Programm fertig ist
 */
bool vmFinished() {
  return pc >= progLen;
}

/*
 * Führt den nächsten Programmschritt aus
 */
bool vmStepBegin() {
  if (pc < 0 || pc >= progLen) return false;
  
  Instr in = prog[pc];
  
  switch(in.op) {
    case OP_ACT:
      // Bewegung ausführen
      akt(in.param);
      stepStartMs = millis();
      stepActive = true;
      return true;
      
    case OP_FOR_BEGIN:
      // FOR-Schleife beginnen
      if (sp >= MAX_STACK) {
        Serial.println(F("[FEHLER] Stack-Überlauf bei FOR"));
        pc++;
        return false;
      }
      st[sp++] = {OP_FOR_BEGIN, pc + 1, (int)in.param};
      pc++;
      return false;
      
    case OP_IF_BEGIN_FREE:
    case OP_IF_BEGIN_BLOCKED: {
      // Bedingte Ausführung
      bool cond = (in.op == OP_IF_BEGIN_FREE) ? frontFree() : frontBlocked();
      if (!cond) {
        // Bedingung nicht erfüllt - zum END springen
        int depth = 1;
        int i = pc + 1;
        while (i < progLen && depth > 0) {
          if (prog[i].op == in.op) depth++;
          else if (prog[i].op == OP_END) depth--;
          i++;
        }
        pc = i;
      } else {
        // Bedingung erfüllt - Block ausführen
        if (sp >= MAX_STACK) {
          Serial.println(F("[FEHLER] Stack-Überlauf bei IF"));
          pc++;
          return false;
        }
        st[sp++] = {in.op, pc + 1, 0};
        pc++;
      }
      return false;
    }
    
    case OP_WHILE_BEGIN_FREE:
    case OP_WHILE_BEGIN_BLOCKED: {
      // WHILE-Schleife
      bool cond = (in.op == OP_WHILE_BEGIN_FREE) ? frontFree() : frontBlocked();
      if (!cond) {
        // Bedingung nicht erfüllt - zum END springen
        int depth = 1;
        int i = pc + 1;
        while (i < progLen && depth > 0) {
          if (prog[i].op == in.op) depth++;
          else if (prog[i].op == OP_END) depth--;
          i++;
        }
        pc = i;
      } else {
        // Bedingung erfüllt - Schleife beginnen
        if (sp >= MAX_STACK) {
          Serial.println(F("[FEHLER] Stack-Überlauf bei WHILE"));
          pc++;
          return false;
        }
        st[sp++] = {in.op, pc + 1, 0};
        pc++;
      }
      return false;
    }
    
    case OP_END:
      // Ende eines Blocks
      if (sp <= 0) {
        pc++;
        return false;
      }
      
      if (st[sp-1].type == OP_FOR_BEGIN) {
        // Ende einer FOR-Schleife
        st[sp-1].remaining--;
        if (st[sp-1].remaining > 0) {
          pc = st[sp-1].beginPc;  // Zurück zum Anfang
        } else {
          sp--;
          pc++;
        }
      }
      else if (st[sp-1].type == OP_IF_BEGIN_FREE || 
               st[sp-1].type == OP_IF_BEGIN_BLOCKED) {
        // Ende eines IF-Blocks
        sp--;
        pc++;
      }
      else if (st[sp-1].type == OP_WHILE_BEGIN_FREE || 
               st[sp-1].type == OP_WHILE_BEGIN_BLOCKED) {
        // Ende einer WHILE-Schleife
        uint8_t t = st[sp-1].type;
        bool cond = (t == OP_WHILE_BEGIN_FREE) ? frontFree() : frontBlocked();
        if (cond) {
          pc = st[sp-1].beginPc;  // Zurück zum Anfang
        } else {
          sp--;
          pc++;
        }
      }
      else {
        pc++;
      }
      return false;
      
    case OP_SET_N:
      pc++;
      return false;
      
    default:
      pc++;
      return false;
  }
}

/*
 * Beendet den aktuellen Programmschritt
 */
void vmStepEnd() {
  stopp();
  stepActive = false;
  pc++;
}

/* ==================== SERIELLE KOMMUNIKATION ==================== */

/*
 * Liest eine Zeile vom Serial Monitor
 */
String readLine() {
  static String buf = "";
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n' || c == '\r') {
      if (buf.length() == 0) continue;
      String line = buf;
      buf = "";
      return line;
    }
    if (buf.length() < 200) buf += c;
  }
  return String();
}

/*
 * Verarbeitet ein Label/Befehl
 */
void handleLabel(const String& label) {
  bool handled = true;
  
  // Bewegungsbefehle
  if (label == "ACT_VOR") addInstr(OP_ACT, AKTION_VOR);
  else if (label == "ACT_ZURUECK") addInstr(OP_ACT, AKTION_ZURUECK);
  else if (label == "ACT_LINKS") addInstr(OP_ACT, AKTION_LINKS);
  else if (label == "ACT_RECHTS") addInstr(OP_ACT, AKTION_RECHTS);
  
  // Kontrollstrukturen
  else if (label == "FOR_BEGIN") addInstr(OP_FOR_BEGIN);
  else if (label == "END") addInstr(OP_END);
  
  // Schleifenzähler
  else if (label == "SET_N_2") addInstr(OP_SET_N, 2);
  else if (label == "SET_N_3") addInstr(OP_SET_N, 3);
  else if (label == "SET_N_4") addInstr(OP_SET_N, 4);
  else if (label == "SET_N_5") addInstr(OP_SET_N, 5);
  else if (label == "SET_N_10") addInstr(OP_SET_N, 10);
  
  // Bedingungen
  else if (label == "IF_FRONT_FREE") addInstr(OP_IF_BEGIN_FREE);
  else if (label == "IF_FRONT_BLOCKED") addInstr(OP_IF_BEGIN_BLOCKED);
  else if (label == "WHILE_FRONT_FREE") addInstr(OP_WHILE_BEGIN_FREE);
  else if (label == "WHILE_FRONT_BLOCKED") addInstr(OP_WHILE_BEGIN_BLOCKED);
  
  // Steuerung
  else if (label == "REC") {
    aufnahmeAktiv = !aufnahmeAktiv;
    if (aufnahmeAktiv) {
      progLen = 0;
      pendingForN = -1;
      sp = 0;
      Serial.println(F(">>> AUFNAHME GESTARTET <<<"));
    } else {
      Serial.print(F(">>> AUFNAHME BEENDET. Befehle: "));
      Serial.print(progLen);
      Serial.println(F(" <<<"));
    }
  }
  else if (label == "RUN") {
    if (progLen > 0) {
      vmReset();
      abspielenAktiv = true;
      Serial.println(F(">>> PROGRAMM WIRD AUSGEFUEHRT <<<"));
    } else {
      Serial.println(F("[WARNUNG] Kein Programm vorhanden!"));
    }
  }
  else {
    handled = false;
  }
  
  if (handled) {
    tone(PIN_SUMMER, 1000, 150);  // Bestätigungston
  } else {
    Serial.println(F("[FEHLER] Unbekannter Befehl"));
  }
}

/*
 * Verarbeitet serielle Befehle
 */
void processSerial() {
  String line = readLine();
  if (line.length() == 0) return;
  line.trim();
  
  // Spezielle Befehle
  if (line.equalsIgnoreCase("HELP")) {
    Serial.println(F("========================================"));
    Serial.println(F("   BitBiber ByteBuddy - Hilfe"));
    Serial.println(F("========================================"));
    Serial.println(F("SYSTEMBEFEHLE:"));
    Serial.println(F("  HELP       - Diese Hilfe anzeigen"));
    Serial.println(F("  REC        - Aufnahme starten/stoppen"));
    Serial.println(F("  RUN        - Programm ausführen"));
    Serial.println(F("  PROG DUMP  - Programm anzeigen"));
    Serial.println(F("  PROG CLEAR - Programm löschen"));
    Serial.println(F(""));
    Serial.println(F("BEWEGUNGEN:"));
    Serial.println(F("  ACT_VOR, ACT_ZURUECK"));
    Serial.println(F("  ACT_LINKS, ACT_RECHTS"));
    Serial.println(F(""));
    Serial.println(F("PROGRAMMIERUNG:"));
    Serial.println(F("  FOR_BEGIN, END"));
    Serial.println(F("  SET_N_2, SET_N_3, SET_N_4"));
    Serial.println(F("  SET_N_5, SET_N_10"));
    Serial.println(F("  IF_FRONT_FREE, IF_FRONT_BLOCKED"));
    Serial.println(F("  WHILE_FRONT_FREE, WHILE_FRONT_BLOCKED"));
    Serial.println(F(""));
    Serial.println(F("BEFEHLSKETTEN:"));
    Serial.println(F("  Mehrere Befehle mit Komma trennen:"));
    Serial.println(F("  Beispiel: ACT_VOR,ACT_LINKS,ACT_VOR"));
    Serial.println(F("========================================"));
    return;
  }
  else if (line.equalsIgnoreCase("PROG DUMP")) {
    progDump();
    return;
  }
  else if (line.equalsIgnoreCase("PROG CLEAR")) {
    progLen = 0;
    pendingForN = -1;
    vmReset();
    Serial.println(F(">>> PROGRAMM GELOESCHT <<<"));
    tone(PIN_SUMMER, 500, 300);
    return;
  }
  
  // *** NEU: BEFEHLSKETTEN-VERARBEITUNG ***
  // Prüfe ob die Zeile Kommas enthält (Befehlskette)
  if (line.indexOf(',') > -1) {
    // Befehlskette erkannt - teile bei Komma auf
    Serial.println(F(">>> BEFEHLSKETTE ERKANNT <<<"));
    int startPos = 0;
    int commaPos = 0;
    int cmdCount = 0;
    
    // Verarbeite jeden Befehl in der Kette
    while (startPos < line.length()) {
      commaPos = line.indexOf(',', startPos);
      
      String cmd;
      if (commaPos == -1) {
        // Letzter Befehl in der Kette
        cmd = line.substring(startPos);
      } else {
        // Befehl bis zum nächsten Komma
        cmd = line.substring(startPos, commaPos);
      }
      
      // Entferne Leerzeichen vom Befehl
      cmd.trim();
      
      // Verarbeite den Befehl wenn nicht leer
      if (cmd.length() > 0) {
        Serial.print(F("  Befehl #"));
        Serial.print(++cmdCount);
        Serial.print(F(": "));
        Serial.println(cmd);
        handleLabel(cmd);
        delay(50);  // Kleine Pause zwischen Befehlen
      }
      
      // Nächster Befehl
      if (commaPos == -1) break;
      startPos = commaPos + 1;
    }
    
    Serial.print(F(">>> "));
    Serial.print(cmdCount);
    Serial.println(F(" BEFEHLE VERARBEITET <<<"));
    return;
  }
  
  // Einzelner Befehl - normale Verarbeitung
  handleLabel(line);
}

/* ==================== RFID-SYSTEM (SPEICHEROPTIMIERT) ==================== */

/*
 * SPEICHEROPTIMIERTES RFID-SYSTEM
 * - Verwendet PROGMEM für Label-Strings im Flash-Speicher
 * - Kompakte UID-Speicherung als 4-Byte Arrays statt Strings
 * - Befehlscodes statt String-Vergleiche
 */

// Befehlscodes für RFID-Karten
enum RfidCmd : uint8_t {
  CMD_ACT_VOR = 0,
  CMD_ACT_ZURUECK,
  CMD_ACT_LINKS,
  CMD_ACT_RECHTS,
  CMD_FOR_BEGIN,
  CMD_END,
  CMD_SET_N_2,
  CMD_SET_N_3,
  CMD_SET_N_4,
  CMD_SET_N_5,
  CMD_SET_N_10,
  CMD_IF_FRONT_FREE,
  CMD_IF_FRONT_BLOCKED,
  CMD_WHILE_FRONT_FREE,
  CMD_WHILE_FRONT_BLOCKED,
  CMD_REC,
  CMD_RUN,
  CMD_INVALID = 255
};

// Kompakte RFID-Karten-Struktur (nur 5 Bytes pro Eintrag!)
struct RfidCard {
  uint8_t uid[4];    // 4 Bytes für UID
  uint8_t cmd;       // 1 Byte für Befehl
};

// RFID-Karten-Datenbank im PROGMEM (Flash-Speicher)
const RfidCard PROGMEM rfidCards[] = {
  // === HAUPTKARTEN ===
  {{0x09, 0x7D, 0x4C, 0x05}, CMD_ACT_VOR},
  {{0xBD, 0x1C, 0x4D, 0x05}, CMD_ACT_ZURUECK},
  {{0x21, 0x02, 0x4C, 0x05}, CMD_ACT_LINKS},
  {{0x7E, 0x00, 0x4C, 0x05}, CMD_ACT_RECHTS},
  {{0x29, 0x5C, 0x42, 0x05}, CMD_FOR_BEGIN},
  {{0x26, 0x98, 0x4B, 0x05}, CMD_END},
  {{0x72, 0xD9, 0x38, 0x05}, CMD_SET_N_2},
  {{0x3B, 0x9C, 0x39, 0x05}, CMD_SET_N_3},
  {{0xA3, 0x88, 0xF9, 0x04}, CMD_SET_N_4},
  {{0xFA, 0x3C, 0xF7, 0x04}, CMD_SET_N_5},
  {{0x27, 0x89, 0x37, 0x02}, CMD_SET_N_10},
  {{0xA2, 0xD7, 0xF9, 0x04}, CMD_IF_FRONT_FREE},
  {{0xF6, 0x2E, 0xF8, 0x04}, CMD_IF_FRONT_BLOCKED},
  {{0xE2, 0xBE, 0xF9, 0x04}, CMD_WHILE_FRONT_FREE},
  {{0xC1, 0x7B, 0xF8, 0x04}, CMD_WHILE_FRONT_BLOCKED},
  {{0xFC, 0x1C, 0x4C, 0x05}, CMD_REC},
  {{0x2C, 0x70, 0x4B, 0x05}, CMD_RUN},
  
  // 1
  {{0xDE, 0xFE, 0xF6, 0x04}, CMD_ACT_VOR},
  {{0x0C, 0x5F, 0x3A, 0x05}, CMD_ACT_ZURUECK},
  {{0x52, 0x8D, 0x41, 0x05}, CMD_ACT_LINKS},
  {{0x66, 0xB3, 0xF4, 0x04}, CMD_ACT_RECHTS},
  {{0xAD, 0x16, 0xF6, 0x04}, CMD_FOR_BEGIN},
  {{0x4A, 0x24, 0xBA, 0x16}, CMD_END},
  {{0x0D, 0xFD, 0x4B, 0x05}, CMD_SET_N_2},
  {{0x0C, 0x19, 0xF6, 0x04}, CMD_SET_N_3},
  {{0x67, 0x2D, 0x3A, 0x05}, CMD_SET_N_4},
  {{0x65, 0x48, 0x3A, 0x05}, CMD_SET_N_5},
  {{0x67, 0x76, 0x38, 0x05}, CMD_SET_N_10},
  {{0x56, 0x6C, 0x3A, 0x05}, CMD_IF_FRONT_FREE},
  {{0x04, 0xB5, 0x38, 0x05}, CMD_IF_FRONT_BLOCKED},
  {{0x83, 0x38, 0x59, 0x1A}, CMD_WHILE_FRONT_FREE},
  {{0x8D, 0x99, 0x49, 0x05}, CMD_WHILE_FRONT_BLOCKED},
  {{0x25, 0x3C, 0xF9, 0x04}, CMD_REC},
  {{0xC4, 0xBC, 0xF9, 0x04}, CMD_RUN},

  // 2
  {{0x5B, 0xAA, 0xF6, 0x04}, CMD_ACT_VOR},
  {{0xF1, 0xE4, 0x4A, 0x05}, CMD_ACT_ZURUECK},
  {{0xBC, 0xDF, 0x4B, 0x05}, CMD_ACT_LINKS},
  {{0xB1, 0x3D, 0xF6, 0x04}, CMD_ACT_RECHTS},
  {{0x86, 0xB5, 0xF9, 0x04}, CMD_FOR_BEGIN},
  {{0x8D, 0x6A, 0xF8, 0x04}, CMD_END},
  {{0x59, 0x9D, 0x4B, 0x05}, CMD_SET_N_2},
  {{0xF1, 0xA1, 0x41, 0x05}, CMD_SET_N_3},
  {{0x8D, 0x99, 0xF9, 0x04}, CMD_SET_N_4},
  {{0x2E, 0x91, 0x39, 0x05}, CMD_SET_N_5},
  {{0x16, 0xF1, 0x4C, 0x05}, CMD_SET_N_10},
  {{0x04, 0xE7, 0x40, 0x05}, CMD_IF_FRONT_FREE},
  {{0xC2, 0x33, 0x4C, 0x05}, CMD_IF_FRONT_BLOCKED},
  {{0x8D, 0x93, 0x4C, 0x05}, CMD_WHILE_FRONT_FREE},
  {{0x63, 0x93, 0xD2, 0x1C}, CMD_WHILE_FRONT_BLOCKED},
  {{0x2E, 0xFA, 0xF6, 0x04}, CMD_REC},
  {{0x2E, 0xBD, 0xF9, 0x04}, CMD_RUN},

  // 3
  {{0xB2, 0x34, 0x4C, 0x05}, CMD_ACT_VOR},
  {{0xEA, 0xDF, 0x41, 0x05}, CMD_ACT_ZURUECK},
  {{0xD7, 0x91, 0x38, 0x05}, CMD_ACT_LINKS},
  {{0x0D, 0xCF, 0xF6, 0x04}, CMD_ACT_RECHTS},
  {{0x86, 0xE4, 0xF4, 0x04}, CMD_FOR_BEGIN},
  {{0x3E, 0x87, 0x4C, 0x05}, CMD_END},
  {{0x61, 0x3C, 0x39, 0x05}, CMD_SET_N_2},
  {{0x09, 0x7C, 0x4C, 0x05}, CMD_SET_N_3},
  {{0xC6, 0xCD, 0xF5, 0x04}, CMD_SET_N_4},
  {{0xBB, 0x50, 0x4C, 0x05}, CMD_SET_N_5},
  {{0x4C, 0x4D, 0x4C, 0x05}, CMD_SET_N_10},
  {{0xC7, 0x61, 0x4B, 0x05}, CMD_IF_FRONT_FREE},
  {{0x5D, 0x79, 0x4C, 0x05}, CMD_IF_FRONT_BLOCKED},
  {{0x83, 0x9E, 0xE7, 0x24}, CMD_WHILE_FRONT_FREE},
  {{0x37, 0xDD, 0x47, 0x05}, CMD_WHILE_FRONT_BLOCKED},
  {{0xC6, 0xC4, 0xF7, 0x04}, CMD_REC},
  {{0x57, 0x36, 0xF9, 0x04}, CMD_RUN},

  // 4
  {{0x4B, 0xA1, 0x4B, 0x05}, CMD_ACT_VOR},
  {{0xC6, 0xA9, 0x38, 0x05}, CMD_ACT_ZURUECK},
  {{0xEE, 0xE0, 0x4C, 0x05}, CMD_ACT_LINKS},
  {{0x04, 0x67, 0xF9, 0x04}, CMD_ACT_RECHTS},

  {{0xEC, 0xB3, 0xF9, 0x04}, CMD_FOR_BEGIN},
  {{0x37, 0x35, 0x4B, 0x05}, CMD_END},

  {{0xC4, 0x8A, 0x4C, 0x05}, CMD_SET_N_2},
  {{0x0B, 0xB7, 0x39, 0x05}, CMD_SET_N_3},
  {{0x2D, 0xB3, 0x38, 0x05}, CMD_SET_N_4},
  {{0x3D, 0x66, 0x4B, 0x05}, CMD_SET_N_5},
  {{0x45, 0xA1, 0x38, 0x05}, CMD_SET_N_10},

  {{0x7D, 0x71, 0x40, 0x05}, CMD_IF_FRONT_FREE},
  {{0x0E, 0x6E, 0x4B, 0x05}, CMD_IF_FRONT_BLOCKED},
  {{0x99, 0xAF, 0x3A, 0x05}, CMD_WHILE_FRONT_FREE},
  {{0x63, 0xA9, 0xE6, 0x24}, CMD_WHILE_FRONT_BLOCKED},

  {{0x0C, 0xDA, 0xF6, 0x04}, CMD_REC},
  {{0xBD, 0x71, 0xF6, 0x04}, CMD_RUN},
};

const uint8_t ANZAHL_RFID_KARTEN = sizeof(rfidCards) / sizeof(rfidCards[0]);

/*
 * Führt einen RFID-Befehl aus
 */
void executeRfidCommand(uint8_t cmd) {
  switch(cmd) {
    case CMD_ACT_VOR:
      handleLabel(F("ACT_VOR"));
      break;
    case CMD_ACT_ZURUECK:
      handleLabel(F("ACT_ZURUECK"));
      break;
    case CMD_ACT_LINKS:
      handleLabel(F("ACT_LINKS"));
      break;
    case CMD_ACT_RECHTS:
      handleLabel(F("ACT_RECHTS"));
      break;
    case CMD_FOR_BEGIN:
      handleLabel(F("FOR_BEGIN"));
      break;
    case CMD_END:
      handleLabel(F("END"));
      break;
    case CMD_SET_N_2:
      handleLabel(F("SET_N_2"));
      break;
    case CMD_SET_N_3:
      handleLabel(F("SET_N_3"));
      break;
    case CMD_SET_N_4:
      handleLabel(F("SET_N_4"));
      break;
    case CMD_SET_N_5:
      handleLabel(F("SET_N_5"));
      break;
    case CMD_SET_N_10:
      handleLabel(F("SET_N_10"));
      break;
    case CMD_IF_FRONT_FREE:
      handleLabel(F("IF_FRONT_FREE"));
      break;
    case CMD_IF_FRONT_BLOCKED:
      handleLabel(F("IF_FRONT_BLOCKED"));
      break;
    case CMD_WHILE_FRONT_FREE:
      handleLabel(F("WHILE_FRONT_FREE"));
      break;
    case CMD_WHILE_FRONT_BLOCKED:
      handleLabel(F("WHILE_FRONT_BLOCKED"));
      break;
    case CMD_REC:
      handleLabel(F("REC"));
      break;
    case CMD_RUN:
      handleLabel(F("RUN"));
      break;
  }
}

/*
 * Prüft ob eine RFID-Karte aufgelegt wurde und führt den Befehl aus
 */
void checkRFID() {
  // Prüfe ob neue Karte vorhanden
  if (!mfrc522.PICC_IsNewCardPresent()) return;
  if (!mfrc522.PICC_ReadCardSerial()) return;
  
  // Prüfe ob UID 4 Bytes hat (Standard Mifare)
  if (mfrc522.uid.size != 4) {
    Serial.println(F("[WARNUNG] Nur 4-Byte UIDs werden unterstützt"));
    mfrc522.PICC_HaltA();
    mfrc522.PCD_StopCrypto1();
    return;
  }
  
  // Suche Karte in der Datenbank
  uint8_t cmd = CMD_INVALID;
  for (uint8_t i = 0; i < ANZAHL_RFID_KARTEN; i++) {
    // Lade Karte aus PROGMEM
    RfidCard card;
    memcpy_P(&card, &rfidCards[i], sizeof(RfidCard));
    
    // Vergleiche UID (4 Bytes)
    bool match = true;
    for (uint8_t j = 0; j < 4; j++) {
      if (mfrc522.uid.uidByte[j] != card.uid[j]) {
        match = false;
        break;
      }
    }
    
    if (match) {
      cmd = card.cmd;
      break;
    }
  }
  
  if (cmd != CMD_INVALID) {
    // Karte erkannt - führe Befehl aus
    executeRfidCommand(cmd);
    tone(PIN_SUMMER, 1200, 150);  // Spezieller Ton für RFID
  } else {
    // Unbekannte Karte - zeige UID für einfaches Hinzufügen
    Serial.print(F("[INFO] Neue RFID-Karte. Füge diese Zeile zu rfidCards hinzu:"));
    Serial.print(F("\n  {{0x"));
    for (byte i = 0; i < 4; i++) {
      if (mfrc522.uid.uidByte[i] < 16) Serial.print(F("0"));
      Serial.print(mfrc522.uid.uidByte[i], HEX);
      if (i < 3) Serial.print(F(", 0x"));
    }
    Serial.println(F("}, CMD_ACT_VOR},  // Befehl anpassen!"));
    tone(PIN_SUMMER, 400, 500);  // Warnton
  }
  
  // Beende Kommunikation mit Karte
  mfrc522.PICC_HaltA();
  mfrc522.PCD_StopCrypto1();
}

/* ==================== HAUPTPROGRAMM ==================== */

/*
 * Setup-Funktion - wird einmal beim Start ausgeführt
 */
void setup() {
  // Serielle Kommunikation initialisieren
  Serial.begin(9600);
  
  // SPI-Bus für RFID initialisieren
  SPI.begin();
  mfrc522.PCD_Init();
  
  // Pins konfigurieren
  pinMode(PIN_SUMMER, OUTPUT);
  pinMode(PIN_ULTRA_TRIG, OUTPUT);
  pinMode(PIN_ULTRA_ECHO, INPUT);
  
  // Anfangszustand setzen
  bewegungszustand = ZUSTOPP;
  zeitpunktStopp = millis();
  noTone(PIN_SUMMER);
  
  // Willkommensnachricht
  Serial.println(F("========================================"));
  Serial.println(F("   BitBiber ByteBuddy v1.0"));
  Serial.println(F("   Technik entdecken, Barrieren überwinden."));
  Serial.println(F(""));
  Serial.println(F("   (c) 2024 BitBiber TechTalente e.V."));
  Serial.println(F("   Schweidnitzer Str. 17"));
  Serial.println(F("   45891 Gelsenkirchen"));
  Serial.println(F("   Kontakt: maik@bitbiber.de"));
  Serial.println(F(""));
  Serial.println(F("   System bereit!"));
  Serial.println(F("   Tippe 'HELP' für Befehle"));
  Serial.println(F("========================================"));
  
  // Startmelodie abspielen
  tone(PIN_SUMMER, 523, 200);  // C
  delay(250);
  tone(PIN_SUMMER, 659, 200);  // E
  delay(250);
  tone(PIN_SUMMER, 784, 400);  // G
  delay(500);
  
  Serial.println(F("[INFO] Initialisierung abgeschlossen"));
  Serial.print(F("[INFO] RFID-Karten registriert: "));
  Serial.println(ANZAHL_RFID_KARTEN);
}

/*
 * Loop-Funktion - wird kontinuierlich wiederholt
 */
void loop() {
  // ========== SICHERHEITSPRÜFUNG ==========
  // Prüfe ob Bedingungsprüfung noch aktiv ist
  if (conditionCheckActive && millis() > conditionCheckEndTime) {
    conditionCheckActive = false;
  }

  // Prüfe auf Hindernisse vor dem Roboter (nur wenn keine Bedingungsprüfung läuft)
  if (!conditionCheckActive) {
    long abstand = messeAbstandCm();
    if (abstand > 0 && abstand <= SICHERHEITS_ABSTAND_CM && abspielenAktiv == true ) {
      // Hindernis erkannt - Notbremsung!
      if (!crashBlockiert) {
        abspielenAktiv = false;
        vmReset();
        stopp();
        
        // Servos sofort abschalten
        if (servosAngeschlossen) {
          servoLinks.detach();
          servoRechts.detach();
          servosAngeschlossen = false;
        }
        
        // Alarm ausgeben
        tone(PIN_SUMMER, 400, 3000);
        crashBlockiert = true;
        Serial.println(F(""));
        Serial.println(F("!!! HINDERNIS ERKANNT !!!"));
        Serial.println(F("!!! NOTBREMSUNG AKTIVIERT !!!"));
        Serial.println(F(""));
      }
      delay(50);
    } else {
      crashBlockiert = false;
    }
  }
  
  // ========== RFID-KARTENLESUNG ==========
  checkRFID();
  
  // ========== TASTER-ABFRAGE ==========
  int taste = leseTaster();
  static int lastBtn = -1;

  
  // Feedback-Ton bei neuem Tastendruck
  if (taste != -1 && taste != lastBtn) {
    piepFuerTaste(taste, 120);
  }
  lastBtn = taste;
  
  // Taster 0: Aufnahme
  if (taste == 0) {
    aufnahmeAktiv = !aufnahmeAktiv;
    if (aufnahmeAktiv) {
      progLen = 0;
      pendingForN = -1;
      sp = 0;
      Serial.println(F(">>> AUFNAHME GESTARTET (Taste) <<<"));
    } else {
      Serial.print(F(">>> AUFNAHME BEENDET (Taste). Befehle: "));
      Serial.print(progLen);
      Serial.println(F(" <<<"));
    }
    delay(ENTPRELL_MS);
  }
  
  // Taster 1: Programm starten
  if (taste == 1 && !aufnahmeAktiv) {
    if (progLen > 0) {
      vmReset();
      abspielenAktiv = true;
      Serial.println(F(">>> PROGRAMM WIRD AUSGEFUEHRT (Taste) <<<"));
    } else {
      Serial.println(F("[WARNUNG] Kein Programm vorhanden!"));
    }
    delay(ENTPRELL_MS);
  }
  
  // Taster 2-5: Bewegungen (nur während Aufnahme)
  if (aufnahmeAktiv && taste >= 2 && taste <= 5) {
    addInstr(OP_ACT, (uint8_t)(taste - 2));
    Serial.print(F("+ Bewegung aufgenommen: "));
    Serial.println(tasterListe[taste].name);
    delay(ENTPRELL_MS);
  }
  
  // ========== PROGRAMMAUSFÜHRUNG ==========
  if (abspielenAktiv) {
    if (stepActive) {
      // Warte bis aktuelle Aktion fertig ist
      if (millis() - stepStartMs >= ACTION_MS) {
        vmStepEnd();
        if (vmFinished()) {
          // Programm beendet
          abspielenAktiv = false;
          stopp();
          Serial.println(F(">>> PROGRAMM BEENDET <<<"));
          
          // Abschlussmelodie
          tone(PIN_SUMMER, 784, 200);  // G
          delay(250);
          tone(PIN_SUMMER, 659, 200);  // E
          delay(250);
          tone(PIN_SUMMER, 523, 400);  // C
        }
      }
    } else {
      // Starte nächsten Schritt
      bool wait = vmStepBegin();
      if (!wait && vmFinished()) {
        // Programm beendet (ohne Wartezeit)
        abspielenAktiv = false;
        stopp();
        Serial.println(F(">>> PROGRAMM BEENDET <<<"));
        
        // Abschlussmelodie
        tone(PIN_SUMMER, 784, 200);  // G
        delay(250);
        tone(PIN_SUMMER, 659, 200);  // E
        delay(250);
        tone(PIN_SUMMER, 523, 400);  // C
      }
    }
  }
  
  // ========== SERIELLE BEFEHLE ==========
  processSerial();
  
  // ========== ENERGIESPARMODUS ==========
  if (!aufnahmeAktiv && !abspielenAktiv && taste == -1) {
    detachNachIdle();
  }
  
  // Kleine Pause für Stabilität
  delay(10);
}