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

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define NUM_SWITCHES 3

#define  STEPS_PER_MM_X 200
#define  STEPS_PER_MM_Y 200
#define  STEPS_PER_MM_Z 200

#define HOME_DIRECTION_X -1
#define HOME_DIRECTION_Y -1
#define HOME_DIRECTION_Z -1

const int MOTOR_X_DIR_PIN = 8;
const int MOTOR_X_STEP_PIN = 9;

const int MOTOR_Y_DIR_PIN = 5;
const int MOTOR_Y_STEP_PIN = 6;

const int MOTOR_Z_DIR_PIN = 2;
const int MOTOR_Z_STEP_PIN = 3;

const int STEPPERS_ENABLE_PIN = 10;

const int endSwitchPin[NUM_SWITCHES] = {A0, A1, A2};

const int buttonPin = A3;

Bounce2::Button button = Bounce2::Button();

Bounce2::Button endSwitch[NUM_SWITCHES] = {Bounce2::Button()};

AccelStepper myStepperX = AccelStepper(1, MOTOR_X_STEP_PIN, MOTOR_X_DIR_PIN);
AccelStepper myStepperY = AccelStepper(1, MOTOR_Y_STEP_PIN, MOTOR_Y_DIR_PIN);
AccelStepper myStepperZ = AccelStepper(1, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN);

int distance = 0;
bool distChanged = false;

void homeXY()
{
  bool homeX = false;
  bool homeY = false;

  myStepperX.setAcceleration(50);
  myStepperX.setSpeed(100);

  myStepperY.setAcceleration(50);
  myStepperY.setSpeed(100);

  lcd.setCursor(0, 0);
  lcd.print("Homing axis XY  ");
  Serial.println("Homing XY");

  while ((homeX == false) || (homeY == false))
  {
    endSwitch[0].update();

    if (endSwitch[0].read() == LOW)
    {
      homeX = true;
    }
    else
    {
      myStepperX.move(STEPS_PER_MM_X * HOME_DIRECTION_X);
      myStepperX.run();
    }

    endSwitch[1].update();

    if (endSwitch[1].read() == LOW)
    {
      homeY = true;
    }
    else
    {
      myStepperY.move(STEPS_PER_MM_Y * HOME_DIRECTION_Y);
      myStepperY.run();
    }
  }

  myStepperX.setCurrentPosition(0);
  myStepperY.setCurrentPosition(0);
  
  lcd.setCursor(0, 0);
  lcd.print("Homing XY finish");
  Serial.println("Homing XY finished");
}

void homeZ()
{
  bool homeZ = false;

  myStepperZ.setAcceleration(50);
  myStepperZ.setSpeed(100);

  lcd.setCursor(0, 0);
  lcd.print("Homing axis Z   ");
  Serial.println("Homing Z");

  while (homeZ == false)
  {
    endSwitch[2].update();

    if (endSwitch[2].read() == LOW)
    {
      homeZ = true;
    }
    else
    {
      myStepperZ.move(STEPS_PER_MM_Z * HOME_DIRECTION_Z);
      myStepperZ.run();
    }
  }
  myStepperZ.setCurrentPosition(0);

  lcd.setCursor(0, 0);
  lcd.print("Homing Z finish ");
  Serial.println("Homing Z finished");
}

void setup()
{
  pinMode(STEPPERS_ENABLE_PIN, OUTPUT);
  digitalWrite(STEPPERS_ENABLE_PIN, LOW);

  Serial.begin(9600);

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

  for (byte i = 0; i < NUM_SWITCHES; i++)
  {
    endSwitch[i].attach(endSwitchPin[i], INPUT_PULLUP);
    endSwitch[i].setPressedState(LOW);
    endSwitch[i].interval(5);
  }

  button.attach(buttonPin, INPUT_PULLUP);
  button.setPressedState(LOW);
  button.interval(5);

  myStepperX.setMaxSpeed(1000);

  myStepperY.setMaxSpeed(1000);

  myStepperZ.setMaxSpeed(1000);

  lcd.setCursor(0, 0);
  lcd.print("Homing axis");
  Serial.println("Homing axis");

  homeXY();
  homeZ();

  lcd.setCursor(0, 0);
  lcd.print("Homing Complete!");
  Serial.println("Homing Complete!");
}

void loop()
{
  button.update();

  if (button.pressed() && (distChanged == false))
  {
    lcd.setCursor(0, 0);
    lcd.print("Move:+10mm at XY");
    Serial.println("Move:+10mm at XY");

    distChanged = true;
    distance += 10;
  }

  if (distChanged == true)
  {
    myStepperX.setSpeed(1000);
    myStepperX.moveTo(distance * STEPS_PER_MM_X);
    myStepperX.runSpeedToPosition();

    myStepperY.setSpeed(1000);
    myStepperY.moveTo(distance * STEPS_PER_MM_Y);
    myStepperY.runSpeedToPosition();

    if ((myStepperX.currentPosition() == (distance * STEPS_PER_MM_X))  &&
        (myStepperY.currentPosition() == (distance * STEPS_PER_MM_Y)))
    {
      homeXY();
      distChanged = false;
    }
  }
}