#include <AccelStepper.h>  
#include <LiquidCrystal_I2C.h> // Include the LiquidCrystal_I2C library  

// Define stepper motor connections and motor interface type  
#define MOTOR1_STEP_PIN 2   
#define MOTOR1_DIR_PIN 3   

#define MOTOR2_STEP_PIN 6  
#define MOTOR2_DIR_PIN 7   

// Define button pins  
#define BUTTON1_PIN 9  
#define BUTTON2_PIN 10  
#define BUTTON3_PIN 11  
#define BUTTON4_PIN 12  
#define MOVE_FORWARD_PIN 14  
#define MOVE_BACKWARD_PIN 15  
#define RESET_MOTORS_PIN 16  

// Define LED pins  
#define LED1_PIN A5  
#define LED2_PIN A6   
#define LED3_PIN A7   
#define LED4_PIN A8   
#define LED_MOVE_FORWARD_PIN A10  
#define LED_MOVE_BACKWARD_PIN A11  
#define LED_RESET_PIN 52  
#define LED_MOVE_PIN A9 // Define move pin LED  

// Create stepper motor objects  
AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);  
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);  

// Create LiquidCrystal_I2C object  
LiquidCrystal_I2C lcd(0x27, 16, 2); // Adjust the address (0x27) if necessary  

// Define positions  
const int position1 = 0;  
const int position2 = 200; // Adjust based on your motor's step count  
const int position3 = 400; // Adjust based on your motor's step count  

unsigned long previousMillis = 0; // Store the last time the display was updated  
bool button1Pressed = false; // Track if button 1 was pressed  

void setup() {  
    // Initialize motors  
    motor1.setMaxSpeed(50);   
    motor1.setAcceleration(5.0); // Adjust as needed  
    motor2.setMaxSpeed(50);  
    motor2.setAcceleration(5.0); // Adjust as needed  
    
    // Initialize the LCD with the number of columns and rows  
    lcd.begin(16, 2); // Initialize with 16 columns and 2 rows  
    lcd.backlight(); // Turn on the backlight  

    // Set button pins as input  
    pinMode(BUTTON1_PIN, INPUT_PULLUP);  
    pinMode(BUTTON2_PIN, INPUT_PULLUP);  
    pinMode(BUTTON3_PIN, INPUT_PULLUP);  
    pinMode(BUTTON4_PIN, INPUT_PULLUP);  
    pinMode(MOVE_FORWARD_PIN, INPUT_PULLUP);  
    pinMode(MOVE_BACKWARD_PIN, INPUT_PULLUP);  
    pinMode(RESET_MOTORS_PIN, INPUT_PULLUP);  
    
    // Set LED pins as output  
    pinMode(LED1_PIN, OUTPUT);  
    pinMode(LED2_PIN, OUTPUT);  
    pinMode(LED3_PIN, OUTPUT);  
    pinMode(LED4_PIN, OUTPUT);  
    pinMode(LED_MOVE_FORWARD_PIN, OUTPUT);  
    pinMode(LED_MOVE_BACKWARD_PIN, OUTPUT);  
    pinMode(LED_RESET_PIN, OUTPUT);  
    pinMode(LED_MOVE_PIN, OUTPUT); // Set move pin LED as output  
    
    // Start motors at position 1  
    motor1.moveTo(position1);  
    motor2.moveTo(position1);  
}  

void loop() {  
    // Run both motors  
    motor1.run();  
    motor2.run();  

    // Turn on move pin LED if either motor is running  
    if (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0) {  
        digitalWrite(LED_MOVE_PIN, HIGH); // Turn on move pin LED  
    } else {  
        digitalWrite(LED_MOVE_PIN, LOW); // Turn off move pin LED  
    }  

    // Update the LCD with the current position  
    int currentPosition = 1; // Default to position 1  
    if (motor1.currentPosition() == position2 && motor2.currentPosition() == position2) {  
        currentPosition = 2;  
    } else if (motor1.currentPosition() == position3 && motor2.currentPosition() == position3) {  
        currentPosition = 3;  
    }  
    lcd.setCursor(0, 0); // Set cursor to the first line  
    lcd.print("Position: ");  
    lcd.print(currentPosition); // Display current position  

    // Check Button 1  
    if (digitalRead(BUTTON1_PIN) == LOW && !button1Pressed) {  
        button1Pressed = true; // Mark button 1 as pressed  
        digitalWrite(LED1_PIN, HIGH); // Turn on LED1  

        // Update the LCD display before the delay  
        lcd.setCursor(0, 1); // Set cursor to the second line  
        lcd.print("Waiting..."); // Display a waiting message  
        lcd.display(); // Force the display to update  
        
        // Use a non-blocking delay  
        previousMillis = millis();  
        while (millis() - previousMillis < 500) {  
            // Update the motors or perform other tasks here if needed  
            motor1.run();  
            motor2.run();  
        }  
        
        // Move both motors to position 2  
        motor1.moveTo(position2);  
        motor2.moveTo(position2);  
        
        // Wait until both motors reach the target position  
        while (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0) {  
            motor1.run();  
            motor2.run();  
        }  

        // Update the LCD to show that motors are at position 2  
        lcd.setCursor(0, 1); // Set cursor to the second line  
        lcd.print("At Position 2 "); // Display the new position  
        lcd.display(); // Force the display to update  

        // Add delay before moving forward  
        previousMillis = millis();  
        while (millis() - previousMillis < 5000) {  
            // Update motors or perform other tasks here  
            motor1.run();  
            motor2.run();  
        }  

        // Gradually raise motor 1  
        while (digitalRead(BUTTON2_PIN) == HIGH) {  
            motor1.move(10); // Move up 10 steps  
            motor1.run();  
            motor2.run(); // Keep motor 2 running  
            delay(100); // Wait 100 ms  
        }  
        
        // Lower motor 1 to match motor 2  
        motor1.moveTo(motor2.currentPosition());  
        while (motor1.distanceToGo() != 0) {  
            motor1.run();  
            motor2.run();  
        }  
        
        // Move both motors down to position 3  
        motor1.moveTo(position3);  
        motor2.moveTo(position3);  
        while (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0) {  
            motor1.run();  
            motor2.run();  
        }  
        digitalWrite(LED1_PIN, LOW); // Turn off LED1  
    } else if (digitalRead(BUTTON1_PIN) == HIGH) {  
        button1Pressed = false; // Reset button state when released  
    }  

    // Check Move Forward Button  
    if (digitalRead(MOVE_FORWARD_PIN) == LOW) {  
        digitalWrite(LED_MOVE_FORWARD_PIN, HIGH); // Turn on forward LED  
        motor1.move(10); // Move both motors forward by 10 steps  
        motor2.move(10);  
        delay(100); // Wait 100 ms  
        digitalWrite(LED_MOVE_FORWARD_PIN, LOW); // Turn off forward LED  
    }  

    // Check Move Backward Button  
    if (digitalRead(MOVE_BACKWARD_PIN) == LOW) {  
        digitalWrite(LED_MOVE_BACKWARD_PIN, HIGH); // Turn on backward LED  
        motor1.move(-10); // Move both motors backward by 10 steps  
        motor2.move(-10);  
        delay(100); // Wait 100 ms  
        digitalWrite(LED_MOVE_BACKWARD_PIN, LOW); // Turn off backward LED  
    }  

    // Check Reset Motors Button  
    if (digitalRead(RESET_MOTORS_PIN) == LOW) {  
        digitalWrite(LED_RESET_PIN, HIGH); // Turn on reset LED  
        motor1.moveTo(position1); // Reset motor1 to position 1  
        motor2.moveTo(position1); // Reset motor2 to position 1  
        while (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0) {  
            motor1.run();  
            motor2.run();  
        }  
        digitalWrite(LED_RESET_PIN, LOW); // Turn off reset LED  
    }  

    // Check Button 2 (optional for demonstration)  
    if (digitalRead(BUTTON2_PIN) == LOW) {  
        digitalWrite(LED2_PIN, HIGH); // Turn on LED2  
        delay(1000); // Wait 1 second  
        digitalWrite(LED2_PIN, LOW); // Turn off LED2  
    }  

    // Check Button 3  
    if (digitalRead(BUTTON3_PIN) == LOW) {  
        digitalWrite(LED3_PIN, HIGH); // Turn on LED3  
        delay(1000); // Wait 1 second  
        
        // Move both motors back to position 1  
        motor1.moveTo(position1);  
        motor2.moveTo(position1);  
        while (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0) {  
            motor1.run();  
            motor2.run();  
        }  
        
        digitalWrite(LED3_PIN, LOW); // Turn off LED3  
    }  

    // Check Button 4 (optional for demonstration)  
    if (digitalRead(BUTTON4_PIN) == LOW) {  
        digitalWrite(LED4_PIN, HIGH); // Turn on LED4  
        delay(1000); // Wait 1 second  
        digitalWrite(LED4_PIN, LOW); // Turn off LED4  
    }  
}