#include <LiquidCrystal_I2C.h>
#include <Bounce2.h>

#define I2C_ADDR 0x27
#define LCD_COLUMNS 16
#define LCD_LINES 2

/*SWP (C#,C++)*/
#define PIN_MOTOR 2
#define PIN_RICHTUNG 3

#define PIN_ESU 4
#define PIN_ESO 5


LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define INTERVAL_IN_MS 10
#define PIN_MODE INPUT_PULLUP

Bounce2::Button b1 = Bounce2::Button();
Bounce2::Button b2 = Bounce2::Button();

int pos = 0;
int z = 0;
unsigned long last = millis();

void setup() {

  //Hier nehmen wir als Startzustand z = 3
  z = 3;  

  pinMode(PIN_MOTOR, OUTPUT); 
  pinMode(PIN_RICHTUNG, OUTPUT); 

  //Endschalter auf INPUT setzen 
  pinMode(PIN_ESU, INPUT); 
  pinMode(PIN_ESO, INPUT); 



  lcd.init();
  lcd.backlight();

  b1.attach ( 7, PIN_MODE);
  b1.interval (INTERVAL_IN_MS);
  b1.setPressedState (LOW);

  b2.attach ( 6, PIN_MODE);
  b2.interval (INTERVAL_IN_MS);
  b2.setPressedState (LOW);

  lcd.clear();
}

void loop()
{
  b1.update();
  b2.update();




  if (z == 0)
  {
    digitalWrite(PIN_MOTOR, LOW);
    digitalWrite(PIN_RICHTUNG, LOW);



    if (b2.pressed())
    {
      z = 1;
    }
  }

  else if (z == 1)
  {
    digitalWrite(PIN_MOTOR, HIGH);
    digitalWrite(PIN_RICHTUNG, HIGH);


    if (digitalRead(PIN_ESU))
    {
      z = 2; 
    }

    /*if (millis() - last > 50)
    {
      pos++;
      last = millis();
      lcd.clear();
    }*/

   

    if (b1.pressed())
    {
      z = 4;
    }
  }

  else if (z == 2)
  {
    digitalWrite(PIN_MOTOR, LOW);
    digitalWrite(PIN_RICHTUNG, LOW);


    if (b1.pressed())
    {
      z = 3;
    }
  }

  else if (z == 3)
  {
    digitalWrite(PIN_MOTOR, HIGH);
    digitalWrite(PIN_RICHTUNG, LOW);


    if (b2.pressed())
    {
      z = 4;
    }

    /*if (millis() - last > 50)
    {
      pos--;
      lcd.clear();
      last = millis();
    }*/

    if (digitalRead(PIN_ESO))
    {
      z = 0;
    }
  }

  else if (z == 4)
  {
    digitalWrite(PIN_MOTOR, LOW);
    digitalWrite(PIN_RICHTUNG, LOW);


    if (b2.pressed())
    {
      z = 1;
    }

    if (b1.pressed())
    {
      z = 3;
    }
  }



  lcd.setCursor(0, 0);
  lcd.print(z);

  //lcd.setCursor(0, 1);
  //lcd.print(pos);
  // switch (z)
}