// Pins definieren
const int S1 = 2; // Taster für Start/Stop
const int S2 = 3; // Apfelschorle/Apfelsaft Auswahl
const int B2 = 5; // Füllstandssensor für Apfelschorle (400ml)
const int B4 = 7; // Füllstandssensor für Apfelsaft
const int M1 = 8; // Motor für Förderband
const int Y1 = 9; // Ventil für Wasser (Apfelschorle)
const int Y2 = 10; // Ventil für Apfelsaft

// Ultraschallsensoren für B1 und B3
const int trigPinB1 = 11; // Trigger Pin für Sensor B1 (unter Y1)
const int echoPinB1 = 12; // Echo Pin für Sensor B1 (unter Y1)
const int trigPinB3 = 13; // Trigger Pin für Sensor B3 (unter Y2)
const int echoPinB3 = 14; // Echo Pin für Sensor B3 (unter Y2)

const long distanceThreshold = 10; // Abstand in cm für das Erkennen einer Flasche

// Zustandsvariablen
bool motorOn = false;
bool apfelschorleMode = false;
unsigned long stopTime = 0;
bool waitForB1Clear = false;
bool waitForB3Clear = false;

void setup() {
  pinMode(S1, INPUT_PULLUP);
  pinMode(S2, INPUT_PULLUP);
  pinMode(B2, INPUT_PULLUP);
  pinMode(B4, INPUT_PULLUP);
  pinMode(M1, OUTPUT);
  pinMode(Y1, OUTPUT);
  pinMode(Y2, OUTPUT);

  // Ultraschallsensoren einrichten
  pinMode(trigPinB1, OUTPUT);
  pinMode(echoPinB1, INPUT);
  pinMode(trigPinB3, OUTPUT);
  pinMode(echoPinB3, INPUT);
}

void loop() {
  // Motorsteuerung basierend auf S1
  if (digitalRead(S1) == LOW && !motorOn) {
    motorOn = true;
    digitalWrite(M1, HIGH); // Förderband einschalten
  } else if (digitalRead(S1) == LOW && motorOn) {
    stopTime = millis();
    motorOn = false;
  }

  // Apfelschorle-Modus
  if (digitalRead(S2) == LOW) {
    apfelschorleMode = true;
  } else {
    apfelschorleMode = false;
  }

  // Apfelschorle abfüllen
  if (apfelschorleMode && motorOn && distanceToB1() < distanceThreshold && !waitForB1Clear) {
    digitalWrite(M1, LOW); // Förderband stoppen
    digitalWrite(Y1, HIGH); // Ventil Y1 öffnen
    
    if (digitalRead(B2) == LOW) {
      digitalWrite(Y1, LOW); // Ventil Y1 schließen
      delay(1000); // 1 Sekunde Wartezeit
      digitalWrite(M1, HIGH); // Förderband wieder starten
      waitForB1Clear = true; // Warten, bis Flasche aus dem Erkennungsbereich raus ist
    }
  }

  // Apfelsaft abfüllen
  if (!apfelschorleMode && motorOn && distanceToB3() < distanceThreshold && !waitForB3Clear) {
    digitalWrite(M1, LOW); // Förderband stoppen
    digitalWrite(Y2, HIGH); // Ventil Y2 öffnen

    if (digitalRead(B4) == LOW) {
      digitalWrite(Y2, LOW); // Ventil Y2 schließen
      delay(1000); // 1 Sekunde Wartezeit
      digitalWrite(M1, HIGH); // Förderband wieder starten
      waitForB3Clear = true; // Warten, bis Flasche aus dem Erkennungsbereich raus ist
    }
  }

  // Flaschen aus dem Bereich von B1 entfernen
  if (waitForB1Clear && distanceToB1() > distanceThreshold) {
    waitForB1Clear = false;
  }

  // Flaschen aus dem Bereich von B3 entfernen
  if (waitForB3Clear && distanceToB3() > distanceThreshold) {
    waitForB3Clear = false;
  }

  // Abschaltung des Motors 5 Sekunden nach S1
  if (!motorOn && (millis() - stopTime > 5000)) {
    digitalWrite(M1, LOW); // Förderband nach 5 Sekunden ausschalten
  }
}

// Funktion zur Berechnung der Entfernung zu B1 (Ultraschallsensor unter Y1)
long distanceToB1() {
  digitalWrite(trigPinB1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinB1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinB1, LOW);
  
  long duration = pulseIn(echoPinB1, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}

// Funktion zur Berechnung der Entfernung zu B3 (Ultraschallsensor unter Y2)
long distanceToB3() {
  digitalWrite(trigPinB3, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinB3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinB3, LOW);
  
  long duration = pulseIn(echoPinB3, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}