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

#define dirPin 11
#define stepPin 10
#define home_switch 12
#define break_switch 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)

#define maxDistance 50  // Maximaler Verfahrweg, dass kein Endschalter eingebaut werden muss

AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

const byte ROW_NUM = 4;
const byte COL_NUM = 4;
char keys[ROW_NUM][COL_NUM] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
byte pin_rows[ROW_NUM] = { 9, 8, 7, 6 };
byte pin_column[COL_NUM] = { 5, 4, 3, 2 };
Keypad keypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COL_NUM);

LiquidCrystal_I2C lcd(0x27, 16, 2);

String soll = "";
bool decimal = false;
bool homing = false;
bool homing_done = false;

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("For Zeroing");
  lcd.setCursor(0, 1);
  lcd.print("Press * Key");
  pinMode(home_switch, INPUT_PULLUP);
  pinMode(break_switch, INPUT_PULLUP);
  stepper.setMaxSpeed(2500.0);
  stepper.setAcceleration(1000.0);
}

void loop() {
  char key = keypad.getKey();
  if (key) {
    switch (key) {
      case '*':
        if (homing_done) {
          //do nothing
          break;
        } else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Zeroing");
          lcd.setCursor(0, 1);
          lcd.print("........");
          home();
          homing = false;
          homing_done = true;
          break;
        }
      case 'A':
        if (!homing && stepper.currentPosition() < maxDistance * stepsPerMM) {
          stepper.move(stepsPerMM * 0.1);
        }
        break;
      case 'B':
        if (!homing && stepper.currentPosition() > 0.1 * stepsPerMM) {
          stepper.move(-stepsPerMM * 0.1);
        }
        break;
      case 'C':
        if (!homing) {
          soll = "";
          decimal = false;
          lcd.setCursor(0, 1);
          lcd.print("SOLL:           ");
        }
        break;
      case '#':
        if (!homing) {
          if (soll.toFloat() <= 0) {
            break;
          }
          if (soll.toFloat() >= maxDistance) {
            lcd.setCursor(0, 1);
            lcd.print("         Error");
            soll = "";
            break;
          } else {
            stepper.moveTo(soll.toFloat() * stepsPerMM);
            soll = "";
            decimal = false;
          }
        }
        break;
      case 'D':
        if (!homing && !decimal) {
          soll += '.';
          decimal = true;
        }
        break;
      default:
        if (!homing) {
          soll += key;
        }
    }
  }
  if (homing_done) {
    lcd.setCursor(0, 0);
    lcd.print("Current: ");
    lcd.print(stepper.currentPosition() / stepsPerMM, 1);
    lcd.setCursor(0, 1);
    lcd.print("Move to: ");
    lcd.print(soll);
  }
  if (!digitalRead(break_switch)) {
    stepper.stop();
    soll = "";
    stepper.setCurrentPosition(stepper.currentPosition());
    lcd.setCursor(0, 1);
    lcd.print("Move to:        ");
  } else {
    stepper.run();
  }
}

void home() {
  stepper.setMaxSpeed(1000.0);
  while (digitalRead(home_switch)) {
    stepper.move(-1);
    stepper.run();
  }
  stepper.setCurrentPosition(0);
  stepper.move(100);
  stepper.runToPosition();
  stepper.setMaxSpeed(100.0);
  while (digitalRead(home_switch)) {
    stepper.move(-1);
    stepper.run();
  }
  stepper.setCurrentPosition(0);
}