/*
    Forum: https://forum.arduino.cc/t/endschalter-programieren-funktioniert-nicht/1180400
    Wokwi: https://wokwi.com/projects/379139190355998721
*/



constexpr unsigned long SCHRITTZEIT =   20;
constexpr unsigned long ABWARTEZEIT = 5000;
constexpr int         FUELLSCHRITTE =  100;
constexpr byte directionPin1 = 5;
constexpr byte stepPin1      = 4;
constexpr byte directionPin2 = 2;
constexpr byte stepPin2      = 3;
constexpr byte glasEndSchalter  = 6;
constexpr byte flaschenEndSchalter  = 7;
constexpr byte startTaster  = 8;
constexpr byte grueneLED   = 9;
constexpr byte roteLED    = 10;


enum Zustaende {BELADESTELLUNG, UMFUELLEN, ABWARTEN, ZURUECKFAHREN};
Zustaende zustand = ZURUECKFAHREN;


/*
  Bei den folgenden Funktionen debouncen wir ausschliesslich das Schliessen der Endschalter,
  da uns das Öffnen nicht interessiert

*/
boolean glasUnten() {
  static unsigned long zuletztLOW;
  byte state = digitalRead(glasEndSchalter);
  if (state ==  LOW) {
    zuletztLOW = millis();
    return true;
  };
  if (state == HIGH && millis()-zuletztLOW > 100){
    return false;
  } 
}

boolean flascheUnten() {
  static unsigned long zuletztLOW;
  byte state = digitalRead(flaschenEndSchalter);
  if (state ==  LOW) {
    zuletztLOW = millis();
    return true;
  };
  if (state == HIGH && millis()-zuletztLOW > 100){
    return false;
  } 
}


void setup() {
  Serial.begin(115200);
  digitalWrite(roteLED,LOW);
  pinMode(roteLED, OUTPUT);
  digitalWrite(grueneLED,LOW);
  pinMode(grueneLED, OUTPUT);
  pinMode(directionPin1, OUTPUT);
  pinMode(stepPin1, OUTPUT);
  pinMode(directionPin2, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(glasEndSchalter, INPUT_PULLUP);
  pinMode(flaschenEndSchalter, INPUT_PULLUP);
  pinMode(startTaster, INPUT_PULLUP);
  motor1NachUnten();
  motor2NachUnten();
  Serial.println(glasUnten());
  Serial.println(flascheUnten());
}

void loop() {
  Maschine();
}

void Maschine() {
  static unsigned long letzterSchritt;
  static int schritteNachOben;
  switch (zustand) {
    case  BELADESTELLUNG:
      if (fuellVorgangStarten()) {
        zustand = UMFUELLEN;
        schritteNachOben = 0;
        Serial.println("Umfüllvorgang gestartet");
        letzterSchritt = 0;
        nichtEingabeBereit();
      }
      break;
    case  UMFUELLEN:
      if (millis() - letzterSchritt > SCHRITTZEIT) {
        einSchrittMotor1();
        einSchrittMotor2();
        schritteNachOben++;
        letzterSchritt = millis();
      }
      if (schritteNachOben >= FUELLSCHRITTE) {
        zustand = ABWARTEN;
        letzterSchritt = millis();
        Serial.println("Abwarten");
      }
      break;
    case  ABWARTEN:
      if (millis() - letzterSchritt > ABWARTEZEIT) {
        motor1NachUnten();
        motor2NachUnten();
        zustand = ZURUECKFAHREN;
        Serial.println("Zurückfahren");
      }
      break;
    case  ZURUECKFAHREN:
      if (glasUnten() && flascheUnten()) {
        motor1NachOben();
        motor2NachOben();
        zustand = BELADESTELLUNG;
        Serial.println("Beladestellung erreicht");
        eingabeBereit();
      }
      if (millis() - letzterSchritt > SCHRITTZEIT) {
        letzterSchritt = millis();
        if (!glasUnten()) {
          einSchrittMotor1();
        }
        if (!flascheUnten()) {
          einSchrittMotor2();
        }
      }
      break;
    default:
      Serial.println("Unbekannter Zustand!?");
      break;
  }
}

void  eingabeBereit(){
     digitalWrite(roteLED, LOW) ;
     digitalWrite(grueneLED, HIGH) ;
}

void  nichtEingabeBereit(){
     digitalWrite(roteLED, HIGH) ;
     digitalWrite(grueneLED, LOW) ;
}

// Hier benötigen wir kein Debouncing, da wir auf die erste Signalflanke reagieren
// und danach die Taste sehr lange nicht mehr abfragen ...
boolean fuellVorgangStarten() {
  byte state = digitalRead(startTaster);
  return !state;
}


void einSchrittMotor1() {
  einSchrittBei(stepPin1);
}

void einSchrittMotor2() {
  einSchrittBei(stepPin2);
}



void einSchrittBei(byte stepPin) {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(15);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(15);
}

void motor1NachOben() {
  richtungAendern(true, directionPin1);
}

void motor1NachUnten() {
  richtungAendern(false, directionPin1);
}

void motor2NachOben() {
  richtungAendern(true, directionPin2);
}

void motor2NachUnten() {
  richtungAendern(false, directionPin2);
}

void richtungAendern(boolean Oben, byte dirPin) {
  if (Oben) {
    digitalWrite(dirPin, LOW);
  } else {
    digitalWrite(dirPin, HIGH);
  }
}