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

*/



constexpr unsigned long SCHRITTZEIT        =   1;
constexpr unsigned long ABWARTEZEIT        = 2000;
constexpr int      GLASFUELLSCHRITTE       =  10000;
constexpr int      FLASCHENFUELLSCHRITTE   =  12000;
constexpr byte directionPin1        = 5;   // Glasantrieb (links)
constexpr byte stepPin1             = 4;
constexpr byte directionPin2        = 2;   // Flaschenantrieb (rechts)
constexpr byte stepPin2             = 3;
constexpr byte glasEndSchalter      = 6;
constexpr byte flaschenEndSchalter  = 7;
constexpr byte startTaster          = 8;
constexpr byte grueneLED            = 9;
constexpr byte roteLED              = 10;

class deviceType {
  private:
    byte stepPin;
    byte dirPin;
    byte endSwitchPin;
    int  position;
    int  direction;
    unsigned long lastLOW = 0;
  public:
    void init(byte aStepPin, byte aDirPin, byte aEndSwitchPin);
    void setDirectionUp();
    void setDirectionDown();
    void step();
    int getPosition() {
      return position;
    };
    bool endPositionReached();
};

void deviceType::init(byte aStepPin, byte aDirPin, byte aEndSwitchPin) {
  stepPin = aStepPin;
  dirPin  = aDirPin;
  endSwitchPin = aEndSwitchPin;
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(endSwitchPin, INPUT_PULLUP);
};

bool deviceType::endPositionReached() {
  byte state = digitalRead(endSwitchPin);
  if (state ==  LOW) {
    lastLOW = millis();
    return true;
  };
  if (state == HIGH && millis() - lastLOW > 100) {
    return false;
  }
}

void deviceType::setDirectionUp() {
  digitalWrite(dirPin, LOW);
  direction = 1;
}
void deviceType::setDirectionDown() {
  digitalWrite(dirPin, HIGH);
  direction = -1;
}

void deviceType::step() {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(15);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(15);
  position += direction;
}

deviceType glas;
deviceType flasche;

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

#define SIMULATION

boolean glasUnten() {
#ifdef SIMULATION
  return (glas.getPosition() <= 0);
#else
  return glas.endPositionReached();
#endif
}

boolean flascheUnten() {
#ifdef SIMULATION
  return (flasche.getPosition() <= 0);
#else
  return flasche.endPositionReached();
#endif
}

void setup() {
  Serial.begin(115200);
  digitalWrite(roteLED, LOW);
  pinMode(roteLED, OUTPUT);
  digitalWrite(grueneLED, LOW);
  pinMode(grueneLED, OUTPUT);
  pinMode(startTaster, INPUT_PULLUP);
  glas.init(stepPin1, directionPin1, glasEndSchalter);
  flasche.init(stepPin2, directionPin2, flaschenEndSchalter);
  glas.setDirectionDown();
  flasche.setDirectionDown();
  zustand = ZURUECKFAHREN;
}

void loop() {
  Maschine();
}

void Maschine() {
  static unsigned long letzterSchritt;
  static int schrittAnzahl = 0;
  switch (zustand) {
    case  BELADESTELLUNG:
      if (fuellVorgangStarten()) {
        zustand = GLASANHEBEN;
        schrittAnzahl = 0;
        letzterSchritt = 0;
        glas.setDirectionUp();
        nichtEingabeBereit();
        Serial.println("Glas anheben");
      }
      break;
    case  GLASANHEBEN:
      if (millis() - letzterSchritt >= SCHRITTZEIT) {
        glas.step();
        schrittAnzahl++;
        letzterSchritt = millis();
      }
      if (schrittAnzahl >= GLASFUELLSCHRITTE) {
        zustand = UMFUELLEN;
        glas.setDirectionDown();
        flasche.setDirectionUp();
        schrittAnzahl = 0;
        Serial.println("Umfüllen");
      }
      break;
    case  UMFUELLEN:
      if (millis() - letzterSchritt >= SCHRITTZEIT) {
        if (schrittAnzahl < GLASFUELLSCHRITTE) {
          glas.step();
        }
        flasche.step();
        schrittAnzahl++;
        letzterSchritt = millis();
      }
      if (schrittAnzahl >= FLASCHENFUELLSCHRITTE) {
        zustand = ABWARTEN;
        letzterSchritt = millis();
        Serial.println("Abwarten");
      }
      break;
    case  ABWARTEN:
      if (millis() - letzterSchritt >= ABWARTEZEIT) {
        glas.setDirectionDown();
        flasche.setDirectionDown();
        zustand = ZURUECKFAHREN;
        Serial.println("Zurückfahren");
      }
      break;
    case  ZURUECKFAHREN:
      if (glasUnten() && flascheUnten()) {
        zustand = BELADESTELLUNG;
        Serial.println("Beladestellung erreicht");
        eingabeBereit();
      }
      if (millis() - letzterSchritt > SCHRITTZEIT) {
        letzterSchritt = millis();
        if (!glasUnten()) {
          glas.step();
        }
        if (!flascheUnten()) {
          flasche.step();
        }
      }
      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;
}