#include <TM1637Display.h>
#include <AccelStepper.h>
#include <EEPROM.h> // NEU: Für persistente Speicherung des Zählers

// -----------------------------
// EEPROM KONSTANTEN
// -----------------------------
const int EEPROM_ADDR = 0; // Speicheradresse für den Zähler

// -----------------------------
// HARDWARE KONSTANTEN (Active LOW Logik)
// -----------------------------
const int MODE_SWITCH     = 11; // D11: Modus-Schalter (LOW = Handmodus, HIGH = Normalmodus)
const int BTN_CCW         = 7;  // D7: Taster für Homing Start (LOW = gedrückt)
const int ENDSTOP_CCW     = 6;  // D6: Endschalter CCW (LOW = erreicht)
const int BESTAETIG_NORM_MOD = 12; // D12: Taster zur Bestätigung des Normalmodus

// PINS FÜR AUTOMATIK
const int BTN_INC         = 4;  // D4: Roboter Signal/Taster (LOW = aktiv)
const int SCHALTELLER_POS = 5;  // D5: Roboter Kopf erkannt (LOW = aktiv)
const int SCHLEIFSCHEIBE_OK = A1; // A1: Signal an Roboter (HIGH = OK)
const int LEER_OUTPUT_PIN = A2; // NEU: A2: Signal an Roboter/SPS bei leeren Magazin (HIGH = Leer)

const int CLK             = 2;  // TM1637 CLK
const int DIO             = 3;  // TM1637 DIO
const int HAND_LED_PIN    = 13; // D13: Blaue LED
const int NORMAL_LED_PIN  = A0; // A0: Grüne LED
const int MOTOR_EN        = 10; // D10: Motor Enable Pin
const int stepPin         = 8;  
const int dirPin          = 9;  

// -----------------------------
// MOTOR & STEUERUNGSPARAMETER
// -----------------------------
const long STEPS_PER_REV  = 200; 
const long MAX_SPEED      = 800; 
const long ACCELERATION   = 400; 

// Initialfahrt und Wechsel-Schritte (Diese Werte können später angepasst werden!)
const long MAX_DISCS      = 5; // Für Testzwecke auf 5 gesetzt (Später auf 150 ändern)
const long INITIAL_STEPS  = 2 * STEPS_PER_REV; // 2 Umdrehungen CW nach Bestätigung
const long AUTO_CW_STEPS  = 1 * STEPS_PER_REV; // 1 Umdrehung CW (Positionierung)
const long AUTO_CCW_STEPS = -(STEPS_PER_REV / 2); // 0.5 Umdrehung CCW (Korrektur/Entnahme)

// Debounce/Longpress Zeiten
const unsigned long REF_LONGPRESS_MS = 1000;  
const unsigned long NORM_CONFIRM_MS  = 500;   
const unsigned long BLINK_INTERVAL_MS = 1000; // 1s Takt für Hnd und LeeR
const unsigned long COMBINED_DEBOUNCE_MS = 50; 
const unsigned long OK_SIGNAL_DURATION = 500; // 0.5s Bestätigungssignal
const unsigned long LOW_STOCK_BLINK_MS = 1000; // 1s Blinktakt bei geringem Vorrat

// -----------------------------
// VARIABLEN ZUSTAND
// -----------------------------
long counter = 0; // Wert wird jetzt beim Start aus EEPROM geladen
bool homed = false; 
bool handMode = false;
bool homingActive = false; 
bool initMoveActive = false;
bool autoMoveActive = false; 
bool normalModeConfirmed = false; 

// Helfer für Taster/Timing
unsigned long refPressStart = 0;
unsigned long confirmPressStart = 0;
bool prevCombined = false;
unsigned long combinedSince = 0;

// Timer und Flags für Blinken (separat gehalten, um Konflikte zu vermeiden)
unsigned long lastBlink = 0;          // Für Hnd-Anzeige
bool blinkState = false;              // Für Hnd-Anzeige

unsigned long lowStockBlinkTimer = 0; // Timer für die Low-Stock LED
bool ledBlinkState = false;           // Blink-Flag für die Low-Stock LED

unsigned long leerBlinkTimer = 0;     // Timer für die LeeR Display-Anzeige
bool dispBlinkState = false;          // Blink-Flag für die LeeR Display-Anzeige


// Display & Motor Objekte
TM1637Display display(CLK, DIO);
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// Segmentdaten
//const uint8_t SEG_HND[] = { 0x76, 0x54, 0x5E, 0x00 }; // Hnd
const uint8_t SEG_HND[] = { 0x76, 0x77, 0x54, 0x5E }; // HAnd
const uint8_t SEG_RDF[] = { 0x50, 0x79, 0x71, 0x00 }; // rEF
const uint8_t SEG_CONF[] = { 0x39, 0x3F, 0x54, 0x71 }; // ConF
const uint8_t SEG_LEER[] = { 0x38, 0x79, 0x79, 0x50 }; // KORRIGIERT: L, E, E, r
// 0x38=L, 0x79=E, 0x50=r


// ----------------------------------------------------
// *** MOTOR FUNKTIONEN (VORWARD DEKLARATION) ***
// ----------------------------------------------------

void checkAutoMoveLogic(); // MUSS VOR setup/loop DEKLARIERT WERDEN

void startInitialMove() {
    stepper.moveTo(stepper.currentPosition() + INITIAL_STEPS);
    initMoveActive = true;
}

void checkInitialMoveComplete() {
    if (initMoveActive && stepper.distanceToGo() == 0) {
        initMoveActive = false; 
        // Grüne LED AN wird in loop() durch die allgemeine LED Logik übernommen
    }
}

void startAutoMove() {
    stepper.moveTo(stepper.currentPosition() + AUTO_CW_STEPS);
    autoMoveActive = true;
}


// -----------------------------
// SETUP
// -----------------------------
void setup() {
  Serial.begin(115200);

  // Eingänge: Active LOW Logik -> INPUT_PULLUP
  pinMode(MODE_SWITCH, INPUT_PULLUP);
  pinMode(ENDSTOP_CCW, INPUT_PULLUP);
  pinMode(BTN_CCW, INPUT_PULLUP);
  pinMode(BESTAETIG_NORM_MOD, INPUT_PULLUP);
  pinMode(BTN_INC, INPUT_PULLUP);      
  pinMode(SCHALTELLER_POS, INPUT_PULLUP); 

  // Ausgänge: LEDs, Motor & Bestätigung
  pinMode(HAND_LED_PIN, OUTPUT);
  pinMode(NORMAL_LED_PIN, OUTPUT);
  pinMode(MOTOR_EN, OUTPUT);
  pinMode(SCHLEIFSCHEIBE_OK, OUTPUT); 
  pinMode(LEER_OUTPUT_PIN, OUTPUT); 

  digitalWrite(MOTOR_EN, LOW); 
  digitalWrite(SCHLEIFSCHEIBE_OK, LOW); 
  digitalWrite(NORMAL_LED_PIN, LOW);
  digitalWrite(HAND_LED_PIN, LOW);
  digitalWrite(LEER_OUTPUT_PIN, LOW); 

  // Motor initialisieren
  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCELERATION);
  stepper.setCurrentPosition(0);

  // NEU: Zählerstand aus EEPROM laden
  EEPROM.get(EEPROM_ADDR, counter); 
  // Sicherstellen, dass der geladene Zähler nicht ungültig ist
  if (counter < 0 || counter > MAX_DISCS) counter = 0;

  // Display initialisieren
  display.setBrightness(0x0f);
  display.showNumberDec(counter, false); // Zeige den geladenen Zählerstand
}

// -----------------------------
// LOOP
// -----------------------------
void loop() {
  // 1. Motor immer laufen lassen
  stepper.run(); 

  // 2. Zustandswerte aktualisieren
  bool prevHandMode = handMode;
  handMode = (digitalRead(MODE_SWITCH) == LOW); 
  
  // Status des Zählers
  const bool lowStock = (counter >= 120) && (counter < MAX_DISCS);
  const bool isMaxed = (counter >= MAX_DISCS); 

  // Zustandsprüfungen
  checkInitialMoveComplete();
  checkAutoMoveLogic(); 

  // ----------------------------------------------------
  // *** LEER ZUSTAND OUTPUT (A2) ***
  // ----------------------------------------------------
  // Signalisiert den leeren Zustand an Roboter/SPS.
  if (isMaxed && !handMode) {
      digitalWrite(LEER_OUTPUT_PIN, HIGH);
  } else {
      digitalWrite(LEER_OUTPUT_PIN, LOW);
  }


  // --- LED-STEUERUNG (KORRIGIERT: A0 / GRÜN) ---
  digitalWrite(HAND_LED_PIN, handMode ? HIGH : LOW);
  
  if (handMode) {
      // Handmodus: Grüne LED AUS
      digitalWrite(NORMAL_LED_PIN, LOW);
  } else {
      // Normalmodus Logik
      if (isMaxed) {
          // 1. Zähler Max: Grüne LED AUS (Absolute Priorität)
          digitalWrite(NORMAL_LED_PIN, LOW);
      } else if (lowStock) {
          // 2. Niedriger Vorrat: Blinkt (1s Takt)
          if (millis() - lowStockBlinkTimer >= LOW_STOCK_BLINK_MS) {
              lowStockBlinkTimer = millis();
              ledBlinkState = !ledBlinkState; 
          }
          digitalWrite(NORMAL_LED_PIN, ledBlinkState ? HIGH : LOW); 
          
      } else if (normalModeConfirmed) {
          // 3. OK/Bereit (einschließlich während Bewegung): Statisch AN
          digitalWrite(NORMAL_LED_PIN, HIGH);
          
      } else {
          // 4. Warten auf Bestätigung/Unbestätigt: AUS
          digitalWrite(NORMAL_LED_PIN, LOW);
      }
      
      // Setze Blinker-Timer zurück, wenn sie in einem anderen Zustand sind
      if (!lowStock) lowStockBlinkTimer = 0;
  }
  
  // ----------------------------------------------------
  // *** HOMING LOGIK ***
  // ----------------------------------------------------
  
  // Homing stoppen bei Endschalter
  if (homingActive && digitalRead(ENDSTOP_CCW) == LOW) {
      stepper.stop(); 
      stepper.setCurrentPosition(0); 
      counter = 0;
      EEPROM.put(EEPROM_ADDR, counter); // NEU: Zählerstand auf 0 speichern
      homed = true;
      homingActive = false;
      lastBlink = 0;
  }
  
  // Homing starten bei D7 Long-Press - Nur im Handmodus
  if (handMode && !homingActive) {
    if (digitalRead(BTN_CCW) == LOW) { 
      if (refPressStart == 0) refPressStart = millis();
      if (millis() - refPressStart >= REF_LONGPRESS_MS) {
        if (!homingActive) {
            homingActive = true; 
            stepper.moveTo(-100000); 
        }
        refPressStart = 0;
      }
    } else {
      refPressStart = 0;
    }
  }

  // Display-Logik im Homing
  if (homingActive) {
      display.setSegments(SEG_RDF); 
  }
  
  // ----------------------------------------------------
  // *** MODUS BESTÄTIGUNG LOGIK ***
  // ----------------------------------------------------
  
  // Fall 1: Umschaltung auf Handmodus -> Normalmodus ist wieder zu bestätigen
  if (prevHandMode && !handMode) {
      normalModeConfirmed = false;
  }
  
  // Fall 2: Bestätigung des Normalmodus
  if (!handMode && homed && !normalModeConfirmed) {
      
      // Anzeige: Blinke "ConF"
      if (millis() - lastBlink >= 500) {
          lastBlink = millis();
          blinkState = !blinkState;
          if (blinkState) display.setSegments(SEG_CONF);
          else display.showNumberDec(0, false); 
      }
      
      // Prüfe D12 Long-Press
      if (digitalRead(BESTAETIG_NORM_MOD) == LOW) {
          if (confirmPressStart == 0) confirmPressStart = millis();
          if (millis() - confirmPressStart >= NORM_CONFIRM_MS) {
              normalModeConfirmed = true;
              startInitialMove(); 
              confirmPressStart = 0;
              lastBlink = 0;
          }
      } else {
          confirmPressStart = 0;
      }
  }

  // ----------------------------------------------------
  // *** AUTOMATISCHE WECHSEL LOGIK (Blockade) ***
  // ----------------------------------------------------
  if (!handMode && normalModeConfirmed && !homingActive && !initMoveActive && !autoMoveActive && !isMaxed) {
      
      bool combinedNow = (digitalRead(SCHALTELLER_POS) == LOW) && (digitalRead(BTN_INC) == LOW);

      if (combinedNow) {
          if (!prevCombined) {
              if (combinedSince == 0) combinedSince = millis();
              else if (millis() - combinedSince >= COMBINED_DEBOUNCE_MS) {
                  prevCombined = true;
                  combinedSince = 0;
                  startAutoMove();
              }
          }
      } else {
          prevCombined = false;
          combinedSince = 0;
      }
  }


  // ----------------------------------------------------
  // *** HAUPT-ANZEIGELOGIK ***
  // ----------------------------------------------------
  
  if (isMaxed && !handMode) {
      // LEER-Zustand: Display blinkt 'LeeR' / 'MAX_DISCS' im 1s Takt
      if (millis() - leerBlinkTimer >= BLINK_INTERVAL_MS) { 
          leerBlinkTimer = millis();
          dispBlinkState = !dispBlinkState;
      }
      
      if (dispBlinkState) {
          display.setSegments(SEG_LEER);
      } else {
          display.showNumberDec(MAX_DISCS, false); 
      }
      
      // Setze andere Timer zurück
      lastBlink = 0; 
      
  } else if (handMode && !homingActive) {
    // Handmodus Anzeige (Hnd blinkt)
    if (millis() - lastBlink >= BLINK_INTERVAL_MS) {
      lastBlink = millis();
      blinkState = !blinkState; 
      if (blinkState) display.setSegments(SEG_HND);
      else display.showNumberDec(counter, false); 
    }
    leerBlinkTimer = 0;
    
  } else if (normalModeConfirmed && (initMoveActive || autoMoveActive)) {
      // Motorfahrt läuft (Initial oder Auto-Wechsel)
      display.setSegments(SEG_RDF);
      lastBlink = 0;
      leerBlinkTimer = 0;
      
  } else if (!handMode && normalModeConfirmed && !initMoveActive && !autoMoveActive) {
      // Normalbetrieb aktiv und bereit: Zeige Zähler
      display.showNumberDec(counter, false);
      lastBlink = 0;
      leerBlinkTimer = 0;
  }
} // ENDE von loop()


// ----------------------------------------------------
// *** ZWEITER TEIL DER WECHSEL LOGIK (Muss nach loop() sein, um Fehler zu vermeiden) ***
// ----------------------------------------------------
void checkAutoMoveLogic() {
    static unsigned long moveTimer = 0;
    static int autoMoveStep = 0; // 0=Inaktiv, 1=CW-Fahrt, 2=CCW-Fahrt, 3=Warte 1s, 4=Signal ON, 5=Signal OFF
    
    // Motor fährt gerade auf ein Ziel
    if (stepper.distanceToGo() != 0) return; 

    // Motor hat Ziel erreicht ODER Wechsel wurde gerade gestartet
    switch (autoMoveStep) {
        case 0: 
            // Inaktiv
            if (autoMoveActive) autoMoveStep = 1;
            break;
            
        case 1: 
            // CW-Fahrt beendet -> Starte CCW-Fahrt
            stepper.moveTo(stepper.currentPosition() + AUTO_CCW_STEPS);
            autoMoveStep = 2;
            break;
            
        case 2:
            // CCW-Fahrt beendet -> Inkrementiere und Warte
            if (counter < MAX_DISCS) { 
                counter++; 
                EEPROM.put(EEPROM_ADDR, counter); // NEU: Zählerstand speichern
            }
            moveTimer = millis();
            autoMoveStep = 3;
            break;
            
        case 3:
            // Warte 1 Sekunde
            if (millis() - moveTimer >= 1000) {
                digitalWrite(SCHLEIFSCHEIBE_OK, HIGH); // Signal AN
                moveTimer = millis();
                autoMoveStep = 4;
            }
            break;
            
        case 4:
            // Signal AN für 500ms
            if (millis() - moveTimer >= OK_SIGNAL_DURATION) {
                digitalWrite(SCHLEIFSCHEIBE_OK, LOW); // Signal AUS
                autoMoveStep = 5;
            }
            break;
            
        case 5:
            // Letzter Schritt beendet -> Zurücksetzen
            autoMoveActive = false;
            autoMoveStep = 0;
            break;
    }
}