#include <AccelStepper.h>
#include <Keypad.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define motorPin1 10    // geändert auf Pin 10
#define motorPin2 11    // geändert auf Pin 11
#define home_switch 12  // geändert auf Pin 12
#define end_switch 13   // geändert auf Pin 13

#define stepsPerRevolution 200
#define maxSpeedRPM 2500
#define maxSpeedSPS (maxSpeedRPM * stepsPerRevolution / 60)  // Steps per second
#define lowSpeedSPS (maxSpeedRPM * stepsPerRevolution / 600)  // Steps per second
#define mmPerRevolution 5.0
#define stepsPerMM (stepsPerRevolution / mmPerRevolution)
long endPosition = 0;

AccelStepper stepper(1, motorPin1, motorPin2);
LiquidCrystal_I2C lcd(0x27, 16, 2);

const byte ROW_NUM = 4;     //four rows
const byte COLUMN_NUM = 4;  //four columns
char keys[ROW_NUM][COLUMN_NUM] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
byte pin_rows[ROW_NUM] = { 9, 8, 7, 6 };       //connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = { 5, 4, 3, 2 };  //connect to the column pinouts of the keypad
Keypad keypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM);

String inputString = "";  // a string to hold incoming data
boolean decimal = false;
boolean homingDone = false;

void setup() {
  lcd.begin(16, 2);
  lcd.backlight();  // Beleuchtung einschalten
  pinMode(home_switch, INPUT_PULLUP);
  pinMode(end_switch, INPUT_PULLUP);
  stepper.setMaxSpeed(maxSpeedSPS);
  stepper.setAcceleration(1500.0);
  lcd.setCursor(0, 0);
  lcd.print("* Taste");
  lcd.setCursor(0, 1);
  lcd.print("zum Nullen");
}

void loop() {
  if (!homingDone) {
    delay(500);
  }
  char key = keypad.getKey();
  if (key) {
    if (key == '*') {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Nullen");
      lcd.setCursor(0, 1);
      lcd.print("Bitte warten...");
      home();
      homingDone = true;
      lcd.clear();
    }
    if (homingDone) {
      if (inputString.length() == 0) {
        lcd.clear();  // Löschen Sie die Eingabe, wenn die erste Eingabe per Taste getätigt wird
        lcd.setCursor(0, 1);
        lcd.print("SOLL: ");
      }
      lcd.setCursor(6, 1);
      if (key == 'A') {
        stepper.move(stepsPerMM * 0.1);
      } else if (key == 'B') {
        stepper.move(-stepsPerMM * 0.1);
      } else if (key == 'C') {
        inputString = "";
        decimal = false;
        lcd.setCursor(0, 1);
        lcd.print("SOLL:                ");  // Löschen Sie die Eingabe in der zweiten Zeile
      } else if (key == 'D') {
        if (!decimal) {
          inputString += '.';
          decimal = true;
        }
      } else if (key == '#') {
        float position = inputString.toFloat();
        stepper.moveTo(position * stepsPerMM);
        inputString = "";
        decimal = false;
      } else {
        inputString += key;
      }
      lcd.print(inputString);
    }
  }
  if (digitalRead(end_switch) == LOW) {
    stepper.stop();  // Stoppen Sie den Motor, wenn der Endschalter erreicht ist
    endPosition = stepper.currentPosition();
    stepper.setCurrentPosition(endPosition);
    //while (digitalRead(end_switch) == LOW) {  // Warten Sie, bis der Endschalter freigegeben ist
      lcd.setCursor(0, 0);
      lcd.print("Endanschlag ");
      //lcd.print(stepper.currentPosition() / stepsPerMM);  // Aktuelle Position anzeigen
      lcd.setCursor(0, 1);
      lcd.print("Taste C          ");
    //}
    inputString = "";  // Leeren Sie die Eingabewerte
  }
  if (homingDone) {
    lcd.setCursor(0, 0);
    lcd.print("IST: ");
    lcd.print(stepper.currentPosition() / stepsPerMM);  // Aktuelle Position anzeigen
  }
  stepper.run();
}

void home() {
  while (digitalRead(home_switch) != LOW) {
    stepper.setMaxSpeed(maxSpeedSPS);
    stepper.setAcceleration(1500.0);
    stepper.moveTo(-100000);
    stepper.run();
    delay(500);
  }
  stepper.move(5 * stepsPerMM);  // Bewegen Sie die Position 5mm ins Positive
  while (stepper.distanceToGo() != 0) {
    stepper.run();
  }
  delay(500);
  while (digitalRead(home_switch) != LOW) {
    stepper.setMaxSpeed(lowSpeedSPS);
    stepper.setAcceleration(100.0);
    stepper.moveTo(-100000);
    stepper.run();
  }
  stepper.setCurrentPosition(0);
  stepper.setMaxSpeed(maxSpeedSPS);
  stepper.setAcceleration(1500.0);
  delay(500);
}