void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}


switch (aktualnyStan) 
{
    case OCZEKIWANIE:
      digitalWrite(DIODA_ZIELONA, HIGH);
      digitalWrite(DIODA_CZERWONA, LOW);
      
      if (digitalRead(PIN_IR_WEJSCIE) == LOW) //wykrycie puszki
      {
        digitalWrite(DIODA_ZIELONA, LOW); 
        czasRozpoczeciaStanu = millis();    
        aktualnyStan = PAUZA_STARTOWA;   
      }
      break;
      
    case PAUZA_STARTOWA:
      if (millis() - czasRozpoczeciaStanu >= CZAS_UKLADANIA_PUSZKI)
      {
        digitalWrite(PIN_PRZEKAZNIK_A, LOW);  // Start silnika w przód
        digitalWrite(PIN_PRZEKAZNIK_B, HIGH);
        
        czasRozpoczeciaStanu = millis();       
        aktualnyStan = ZGNIATANIE;         
      }
      break;

    case ZGNIATANIE:
      float prad = odczytajPrad(); 
      unsigned long czasKtoryMinal = millis() - czasRozpoczeciaStanu;

      // Zabezpieczenie przeciążeniowe
      if (prad > MAKS_PRAD)
      {
        poprzedniStan = aktualnyStan; 
        aktualnyStan = BLAD; 
        break;
      }

      // Zabezpieczenie czasowe
      if (czasKtoryMinal >= MAKS_CZAS_ZGNIATANIA)
      {
        poprzedniStan = aktualnyStan; 
        aktualnyStan = BLAD; 
        break;
      }

      // Wykrycie końca zgniatania (krańcówka siłownika odcina prąd)
      if (czasKtoryMinal > MIN_CZAS_ROZRUCHU && prad < PROG_NISKIEGO_PRADU)
      {
        digitalWrite(PIN_PRZEKAZNIK_A, HIGH); // Stop ruch w przód
        delay(250);
        
        digitalWrite(PIN_PRZEKAZNIK_B, LOW);  // Start silnika w tył
        puszkaOpuscilaKomore = false;        
        czasRozpoczeciaStanu = millis();       
        aktualnyStan = KONTROLA_WYLOTU;
      }
      break;

    case KONTROLA_WYLOTU:
      unsigned long czasKtoryMinal = millis() - czasRozpoczeciaStanu;

      // Puszka opuściła komorę
      if (digitalRead(PIN_IR_WYJSCIE) == LOW)
      {
        puszkaOpuscilaKomore = true;
        czasRozpoczeciaStanu = millis();
        aktualnyStan = POWROT_DO_BAZY; 
        break;
      }

      // Awaria: Puszka utknęła przy wylocie
      if (czasKtoryMinal >= MAKS_CZAS_WYJSCIA)
      {
        poprzedniStan = aktualnyStan; 
        aktualnyStan = BLAD; 
        break;
      }
      break;

    case POWROT_DO_BAZY:
      float prad = odczytajPrad();
      unsigned long czasKtoryMinal = millis() - czasRozpoczeciaStanu;

      // Wykrycie pełnego powrotu do pozycji zero
      if (czasKtoryMinal > MIN_CZAS_ROZRUCHU && prad < PROG_NISKIEGO_PRADU)
      {
        digitalWrite(PIN_PRZEKAZNIK_B, HIGH); // Wyłączenie silnika
        delay(200);
        czasRozpoczeciaStanu = millis();                      
        aktualnyStan = OCZEKIWANIE;          // Automatyczny powrót do czuwania
      }
      
      if (czasKtoryMinal >= MAKS_CZAS_ZGNIATANIA)
      {
        poprzedniStan = aktualnyStan;
        aktualnyStan = BLAD;
      }
      break;

    case TRYB_MANUALNY:
      // Sygnalizacja trybu serwisowego (miganie zielonej diody)
      digitalWrite(DIODA_ZIELONA, (millis() / 500) % 2); 
      digitalWrite(DIODA_CZERWONA, LOW);

      // Ruch monostabilny (JOG)
      if (digitalRead(PIN_MANUAL_WYSUW_RESET) == LOW) {
        digitalWrite(PIN_PRZEKAZNIK_A, LOW);  
        digitalWrite(PIN_PRZEKAZNIK_B, HIGH);
      }
      else if (digitalRead(PIN_MANUAL_POWROT) == LOW) {
        digitalWrite(PIN_PRZEKAZNIK_A, HIGH);
        digitalWrite(PIN_PRZEKAZNIK_B, LOW);  
      }
      else {
        digitalWrite(PIN_PRZEKAZNIK_A, HIGH);
        digitalWrite(PIN_PRZEKAZNIK_B, HIGH);
      }
      break;

    case BLAD:
      // Natychmiastowe, bezwzględne odcięcie silnika
      digitalWrite(PIN_PRZEKAZNIK_A, HIGH); 
      digitalWrite(PIN_PRZEKAZNIK_B, HIGH); 
      
      digitalWrite(DIODA_ZIELONA, LOW);    
      digitalWrite(DIODA_CZERWONA, HIGH);     

      // Obsługa powrotu do przerwanej pracy przez przycisk RESET
      if (digitalRead(PIN_MANUAL_WYSUW_RESET) == LOW) {
        delay(50); // Debouncing
        Serial.println("Wciśnięto RESET. Wznowienie procesu...");
        
        digitalWrite(DIODA_CZERWONA, LOW);   
        czasRozpoczeciaStanu = millis();    // Wyzerowanie stoperów dla kontynuowanego stanu
        
        // Odtworzenie fizycznego sterowania przekaźnikami przed powrotem do stanu
        if (poprzedniStan == ZGNIATANIE) {
          digitalWrite(PIN_PRZEKAZNIK_A, LOW);  
          digitalWrite(PIN_PRZEKAZNIK_B, HIGH);
        } 
        else if (poprzedniStan == KONTROLA_WYLOTU || poprzedniStan == POWROT_DO_BAZY) {
          digitalWrite(PIN_PRZEKAZNIK_A, HIGH); 
          digitalWrite(PIN_PRZEKAZNIK_B, LOW);   
        }
        
        aktualnyStan = poprzedniStan; // Powrót do punktu przerwania
      }
      break;
  }