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

// Define stepper driver pins
#define X_STEP 1
#define X_DIR 5
#define Y1_STEP 3  // First Y motor
#define Y1_DIR 6
#define Y2_STEP 8  // Second Y motor (opposite direction)
#define Y2_DIR 9
#define Z_STEP 4
#define Z_DIR 7
#define HOME_SWITCH_X 2
#define HOME_SWITCH_Y 11
#define HOME_SWITCH_Z 12

#define STEPS_PER_MM 2.5 // Adjust based on your stepper motor & driver settings
#define MAX_SPEED 500   // Steps per second
#define ACCELERATION 500  // Steps per second^2

// LCD setup
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Keypad setup
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {A0, A1, A2, A3};
byte colPins[COLS] = {A4, A5,10, 13}; // Fixed: Replaced pin 12 with A3
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Initialize stepper motors
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP, X_DIR);
AccelStepper stepperY1(AccelStepper::DRIVER, Y1_STEP, Y1_DIR);
AccelStepper stepperY2(AccelStepper::DRIVER, Y2_STEP, Y2_DIR);
AccelStepper stepperZ(AccelStepper::DRIVER, Z_STEP, Z_DIR);

int X = 0, Y = 0, Z = 0;
int step = 0; // Step to track input order
String input = "";

void setup() {
    lcd.init();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("Initializing...");
    delay(1000);
    
    pinMode(HOME_SWITCH_X, INPUT_PULLUP);
    pinMode(HOME_SWITCH_Y, INPUT_PULLUP);
    pinMode(HOME_SWITCH_Z, INPUT_PULLUP);
    
    // Set acceleration & max speed
    stepperX.setMaxSpeed(MAX_SPEED);
    stepperX.setAcceleration(ACCELERATION);
    
    stepperY1.setMaxSpeed(MAX_SPEED);
    stepperY1.setAcceleration(ACCELERATION);
    
    stepperY2.setMaxSpeed(MAX_SPEED);
    stepperY2.setAcceleration(ACCELERATION);
    stepperY2.setPinsInverted(true, false, false); // Invert direction for second Y motor
    
    stepperZ.setMaxSpeed(MAX_SPEED);
    stepperZ.setAcceleration(ACCELERATION);
    
    homeMachine();
    displayPrompt();
}

void homeMachine() {
    lcd.clear();
    lcd.setCursor(0, 1);
    lcd.print("Homing...");
    
    // Home X axis
    stepperX.moveTo(-10000);
    while (digitalRead(HOME_SWITCH_X) == HIGH) {
        stepperX.run();
    }
    stepperX.setCurrentPosition(0);
    
    // Home Y axis
    stepperY1.moveTo(-10000);
    stepperY2.moveTo(-10000);
    while (digitalRead(HOME_SWITCH_Y) == HIGH) {
        stepperY1.run();
        stepperY2.run();
    }
    stepperY1.setCurrentPosition(0);
    stepperY2.setCurrentPosition(0);
    
    // Home Z axis
    stepperZ.moveTo(-10000);
    while (digitalRead(HOME_SWITCH_Z) == HIGH) {
        stepperZ.run();
    }
    stepperZ.setCurrentPosition(0);
    
    lcd.clear();
    lcd.setCursor(0, 1);
    lcd.print("Homing Complete!");
    delay(2000);
    displayPrompt(); // Move to the next axis prompt
  

}

void moveToPosition(int x, int y, int z) {
    stepperX.moveTo(x * STEPS_PER_MM);
    stepperY1.moveTo(y * STEPS_PER_MM);
    stepperY2.moveTo(y * STEPS_PER_MM);
    stepperZ.moveTo(z * STEPS_PER_MM);
    
    while (stepperX.distanceToGo() != 0 || stepperY1.distanceToGo() != 0 || stepperY2.distanceToGo() != 0 || stepperZ.distanceToGo() != 0) {
        stepperX.run();
        stepperY1.run();
        stepperY2.run();
        stepperZ.run();
    }
    
    EEPROM.put(0, x);
    EEPROM.put(4, y);
    EEPROM.put(8, z);
    
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Position Set:");
    lcd.setCursor(0, 1);
    lcd.print("X:"); lcd.print(x);
    lcd.setCursor(10, 1);
    lcd.print("Y:"); lcd.print(y);
    lcd.setCursor(0, 2);
    lcd.print("Z:"); lcd.print(z);
}



void loop() {
    char key = keypad.getKey();
    if (key) {
        if (key >= '0' && key <= '9') { // Number input
            if (input.length() < 5) { // Limit input size
                input += key;
                lcd.setCursor(0, 2);
                lcd.print("Value:      "); // Clear previous value
                lcd.setCursor(7, 2);
                lcd.print(input);

            }
        } else if (key == '*') { // Reset process
            step = 0;
            input = "";
            X = 0;
            Y = 0;
            Z = 0;
            displayPrompt();
        } 
          else if (key == 'D') { // Reset process
            homeMachine();
          }
         else if (key == '#') { // Confirm input
            if (input.length() > 0) {
                int value = input.toInt(); // Convert string to integer

                if (step == 0) {
                    X = value;
                    step = 1;  // Move to next axis
                } else if (step == 1) {
                    Y = value;
                    step = 2;  // Move to next axis
                } else if (step == 2) {
                    Z = value;
                    step = 3;  // Move to final display
                }
                input = ""; // Reset input
                if (step == 3) {
                    displayFinalValues(); // Show final values and move motors
                } else {
                    displayPrompt(); // Move to the next axis prompt
                }
            }
        }
    }
}

void displayPrompt() {
    lcd.clear();
    lcd.setCursor(0, 0);
    if (step == 0) {
        lcd.print("Enter X value:");
    } else if (step == 1) {
        lcd.print("Enter Y value:");
    } else if (step == 2) {
        lcd.print("Enter Z value:");
    }
    lcd.setCursor(0, 1);
    lcd.print("Use # to confirm");
    lcd.setCursor(0, 2);
    lcd.print("Value:      "); // Ensure clear input field
}

void displayFinalValues() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("X-axis:"); lcd.print(X);
    lcd.setCursor(10, 0);
    lcd.print("Y-axis:"); lcd.print(Y);
    lcd.setCursor(0, 1);
    lcd.print("Z-axis:"); lcd.print(Z);
    lcd.setCursor(0, 2);
    lcd.print("All values set");
    moveToPosition(X,Y,Z);
}