#include <Arduino.h>

// Pin definitions for stepper motors
#define DIR_X 4     // Direction pin for X-axis motor
#define STEP_X 5    // Step pin for X-axis motor
#define DIR_Y 6     // Direction pin for Y-axis motor
#define STEP_Y 7    // Step pin for Y-axis motor
#define DIR_Z 8     // Direction pin for Z-axis motor
#define STEP_Z 9    // Step pin for Z-axis motor
#define EN_PIN 12   // Enable pin for all motors (LOW = enabled, HIGH = disabled)

// Button pin definitions
#define X_MINUS 25  // -X button (btn1)
#define X_PLUS 24   // +X button (btn2)
#define Y_MINUS 23  // -Y button (btn4)
#define Y_PLUS 22   // +Y button (btn3)
#define Z_MINUS 27  // -Z button (btn6)
#define Z_PLUS 26   // +Z button (btn5)
#define BTN_1MM 30  // 1mm precision movement button
#define BTN_10MM 28 // 10mm movement button

// Limit switch pin definitions
#define LIMIT_SWITCH_X 31  // X-axis limit switch
#define LIMIT_SWITCH_Y 32  // Y-axis limit switch
#define LIMIT_SWITCH_Z 29  // Z-axis limit switch
#define STOP_BTN 33        // Emergency stop button

// Motor control parameters
#define STEP_DELAY 2000   // Microseconds between steps (controls motor speed)
#define STEPS_PER_PRESS 100  // Number of steps per button press (normal mode)
#define STEPS_FOR_15_DEGREES 133  // Steps for 15 degrees (1mm mode)
#define STEPS_FOR_150_DEGREES 1333 // Steps for 150 degrees (10mm mode)

// Direction constants
#define DIR_CLOCKWISE LOW       // Positive direction (may need to be inverted based on your motor setup)
#define DIR_COUNTER_CLOCKWISE HIGH  // Negative direction

// Movement mode enum
enum MoveMode {
  NORMAL_MODE,
  PRECISION_1MM_MODE,
  PRECISION_10MM_MODE
};

// Global variables
MoveMode currentMode = NORMAL_MODE;  // Current movement mode
unsigned long lastDebounceTime = 0;  // Variable for debounce
unsigned long debounceDelay = 100;   // Debounce time in ms

// Variables to track axis enable status
bool xAxisEnabled = true;
bool yAxisEnabled = true;
bool zAxisEnabled = true;

// Variables to track current direction of each motor
bool xDirIsNegative = false;  // true when moving in negative direction
bool yDirIsNegative = false;
bool zDirIsNegative = false;

// Global emergency stop flag
volatile bool emergencyStop = false;

// G28 homing variables
bool isHoming = false;  // Flag to indicate homing is in progress
int homingAxis = 0;     // 0=X, 1=Y, 2=Z, 3=Done
int homingSpeed = 500;  // Slower step delay for more accurate homing (microseconds)
int homingBackoff = 50; // Steps to back off from limit switch after hitting it

// Serial command handling
String inputString = "";  // String to hold incoming data
bool stringComplete = false; // Flag to indicate command is complete

// Function prototypes
void moveMotor(int dirPin, int stepPin, int steps, bool direction);
void moveMotorWithLimitCheck(int dirPin, int stepPin, int limitPin, int steps, bool direction);
void checkLimitSwitches();
void resetAxisEnables();
bool checkEmergencyStop();
void processCommand(String command);
void processG28Command();
void performHomingSequence();
void homeAxis(int axis);
void backoffFromLimitSwitch(int dirPin, int stepPin, int steps);

void setup() {
  // Configure motor control pins as outputs
  pinMode(DIR_X, OUTPUT);
  pinMode(STEP_X, OUTPUT);
  pinMode(DIR_Y, OUTPUT);
  pinMode(STEP_Y, OUTPUT);
  pinMode(DIR_Z, OUTPUT);
  pinMode(STEP_Z, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  
  // Configure button pins as inputs with internal pull-up resistors
  pinMode(X_MINUS, INPUT_PULLUP);
  pinMode(X_PLUS, INPUT_PULLUP);
  pinMode(Y_MINUS, INPUT_PULLUP);
  pinMode(Y_PLUS, INPUT_PULLUP);
  pinMode(Z_MINUS, INPUT_PULLUP);
  pinMode(Z_PLUS, INPUT_PULLUP);
  pinMode(BTN_1MM, INPUT_PULLUP);
  pinMode(BTN_10MM, INPUT_PULLUP);
  
  // Configure limit switches and stop button
  pinMode(LIMIT_SWITCH_X, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_Y, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_Z, INPUT_PULLUP);
  pinMode(STOP_BTN, INPUT_PULLUP);
  
  // Enable motors (LOW = enabled)
  digitalWrite(EN_PIN, LOW);
  
  // Reserve memory for serial input
  inputString.reserve(50);
  
  // Initialize serial communication
  Serial.begin(115200);
  Serial.println("3-Axis Stepper Motor Control with Enhanced Emergency Stop");
  Serial.println("Press the 1mm button for 15 degree movement");
  Serial.println("Press the 10mm button for 150 degree movement");
  Serial.println("Limit switches will stop motion in the negative direction");
  Serial.println("Emergency stop button will immediately stop ALL motors");
  Serial.println("G-code commands supported: G28 (Auto-Homing)");
}

void loop() {
  // First check for emergency stop
  if (checkEmergencyStop()) {
    // Wait until the emergency stop is cleared
    return; // Skip the rest of the loop if in emergency stop mode
  }
  
  // Check for incoming serial data
  if (Serial.available() > 0) {
    char inChar = (char)Serial.read();
    
    // Add character to input string
    if (inChar == '\n') {
      stringComplete = true;
    } else {
      inputString += inChar;
    }
  }
  
  // Process complete serial commands
  if (stringComplete) {
    processCommand(inputString);
    inputString = "";
    stringComplete = false;
  }
  
  // Skip manual controls if homing is in progress
  if (isHoming) {
    return;
  }
  
  // Check and handle limit switches
  checkLimitSwitches();
  
  // Check precision mode buttons
  if (digitalRead(BTN_1MM) == LOW) {
    if ((millis() - lastDebounceTime) > debounceDelay) {
      currentMode = PRECISION_1MM_MODE;
      Serial.println("Precision mode (1mm/15 degrees) activated");
      lastDebounceTime = millis();
      
      // Wait for button release
      while (digitalRead(BTN_1MM) == LOW) {
        if (checkEmergencyStop()) return; // Check for emergency stop while waiting
        delay(10);
      }
      delay(100); // Additional delay to prevent signal bounce
    }
  }

  if (digitalRead(BTN_10MM) == LOW) {
    if ((millis() - lastDebounceTime) > debounceDelay) {
      currentMode = PRECISION_10MM_MODE;
      Serial.println("Precision mode (10mm/150 degrees) activated");
      lastDebounceTime = millis();
      
      // Wait for button release
      while (digitalRead(BTN_10MM) == LOW) {
        if (checkEmergencyStop()) return; // Check for emergency stop while waiting
        delay(10);
      }
      delay(100); // Additional delay to prevent signal bounce
    }
  }
  
  // X axis control
  if (xAxisEnabled) { // Only process if X axis is enabled
    if (digitalRead(X_PLUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        xDirIsNegative = false; // Update direction tracking (positive direction)
        
        if (currentMode == PRECISION_1MM_MODE) {
          Serial.println("Precision move: X motor 15 degrees clockwise");
          moveMotor(DIR_X, STEP_X, STEPS_FOR_15_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE; // Return to normal mode after movement
          Serial.println("Returned to normal mode");
        } 
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: X motor 150 degrees clockwise");
          moveMotor(DIR_X, STEP_X, STEPS_FOR_150_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE; // Return to normal mode after movement
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving X motor clockwise");
          moveMotor(DIR_X, STEP_X, STEPS_PER_PRESS, DIR_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300); // Debounce delay
      }
    }
    
    if (digitalRead(X_MINUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        xDirIsNegative = true; // Update direction tracking (negative direction)
        
        if (currentMode == PRECISION_1MM_MODE) {
          Serial.println("Precision move: X motor 15 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_X, STEP_X, LIMIT_SWITCH_X, STEPS_FOR_15_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE; // Return to normal mode after movement
          Serial.println("Returned to normal mode");
        } 
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: X motor 150 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_X, STEP_X, LIMIT_SWITCH_X, STEPS_FOR_150_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE; // Return to normal mode after movement
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving X motor counter-clockwise");
          moveMotorWithLimitCheck(DIR_X, STEP_X, LIMIT_SWITCH_X, STEPS_PER_PRESS, DIR_COUNTER_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300); // Debounce delay
      }
    }
  }
  
  // Y axis control
  if (yAxisEnabled) { // Only process if Y axis is enabled
    if (digitalRead(Y_PLUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        yDirIsNegative = false; // Update direction tracking (positive direction)
        
        if (currentMode == PRECISION_1MM_MODE) {
          Serial.println("Precision move: Y motor 15 degrees clockwise");
          moveMotor(DIR_Y, STEP_Y, STEPS_FOR_15_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: Y motor 150 degrees clockwise");
          moveMotor(DIR_Y, STEP_Y, STEPS_FOR_150_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving Y motor clockwise");
          moveMotor(DIR_Y, STEP_Y, STEPS_PER_PRESS, DIR_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300);
      }
    }
    
    if (digitalRead(Y_MINUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        yDirIsNegative = true; // Update direction tracking (negative direction)
        
        if (currentMode == PRECISION_1MM_MODE) {
          Serial.println("Precision move: Y motor 15 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_Y, STEP_Y, LIMIT_SWITCH_Y, STEPS_FOR_15_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: Y motor 150 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_Y, STEP_Y, LIMIT_SWITCH_Y, STEPS_FOR_150_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving Y motor counter-clockwise");
          moveMotorWithLimitCheck(DIR_Y, STEP_Y, LIMIT_SWITCH_Y, STEPS_PER_PRESS, DIR_COUNTER_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300);
      }
    }
  }
  
  // Z axis control
  if (zAxisEnabled) { // Only process if Z axis is enabled
    if (digitalRead(Z_PLUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        zDirIsNegative = false; // Update direction tracking (positive direction)
        
        if (currentMode == PRECISION_1MM_MODE) {
          Serial.println("Precision move: Z motor 15 degrees clockwise");
          moveMotor(DIR_Z, STEP_Z, STEPS_FOR_15_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: Z motor 150 degrees clockwise");
          moveMotor(DIR_Z, STEP_Z, STEPS_FOR_150_DEGREES, DIR_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving Z motor clockwise");
          moveMotor(DIR_Z, STEP_Z, STEPS_PER_PRESS, DIR_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300);
      }
    }
    
    if (digitalRead(Z_MINUS) == LOW) {
      if ((millis() - lastDebounceTime) > debounceDelay) {
        zDirIsNegative = true; // Update direction tracking (negative direction)
        
        if (currentMode == PRECISION_1MM_MODE)
        {
          Serial.println("Precision move: Z motor 15 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_Z, STEP_Z, LIMIT_SWITCH_Z, STEPS_FOR_15_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else if (currentMode == PRECISION_10MM_MODE) {
          Serial.println("Precision move: Z motor 150 degrees counter-clockwise");
          moveMotorWithLimitCheck(DIR_Z, STEP_Z, LIMIT_SWITCH_Z, STEPS_FOR_150_DEGREES, DIR_COUNTER_CLOCKWISE);
          currentMode = NORMAL_MODE;
          Serial.println("Returned to normal mode");
        }
        else {
          Serial.println("Moving Z motor counter-clockwise");
          moveMotorWithLimitCheck(DIR_Z, STEP_Z, LIMIT_SWITCH_Z, STEPS_PER_PRESS, DIR_COUNTER_CLOCKWISE);
        }
        lastDebounceTime = millis();
        delay(300);
      }
    }
  }
  
  // Cancel precision mode if no button is pressed for 5 seconds
  static unsigned long precisionModeStartTime = 0;
  if (currentMode != NORMAL_MODE) {
    if (precisionModeStartTime == 0) {
      precisionModeStartTime = millis();
    } else if (millis() - precisionModeStartTime > 5000) {
      currentMode = NORMAL_MODE;
      precisionModeStartTime = 0;
      Serial.println("Precision mode timed out - returned to normal mode");
    }
  } else {
    precisionModeStartTime = 0;
  }
}

// Function to process serial commands
void processCommand(String command) {
  // Trim any leading/trailing whitespace
  command.trim();
  
  // Convert to uppercase for consistency
  command.toUpperCase();
  
  Serial.print("Executing command: ");
  Serial.println(command);
  
  // Parse G-code commands
  if (command.startsWith("G28")) {
    // G28 - Home all axes
    processG28Command();
  } 
  else {
    Serial.println("Unknown command");
  }
}

// G28 command processor
void processG28Command() {
  Serial.println("Starting G28 Auto-Homing Sequence");
  Serial.println("Axes will be homed in sequence: X, Y, Z");
  
  // Set homing flag and start with X axis
  isHoming = true;
  homingAxis = 0;
  
  // Begin homing sequence
  performHomingSequence();
}

// Main homing sequence function
void performHomingSequence() {
  // Process current homing axis
  switch (homingAxis) {
    case 0: // X-axis
      Serial.println("Homing X-axis...");
      homeAxis(0);
      homingAxis++;
      Serial.println("X-axis homing complete.");
      performHomingSequence(); // Continue to next axis
      break;
      
    case 1: // Y-axis
      Serial.println("Homing Y-axis...");
      homeAxis(1);
      homingAxis++;
      Serial.println("Y-axis homing complete.");
      performHomingSequence(); // Continue to next axis
      break;
      
    case 2: // Z-axis
      Serial.println("Homing Z-axis...");
      homeAxis(2);
      homingAxis++;
      Serial.println("Z-axis homing complete.");
      performHomingSequence(); // Finish sequence
      break;
      
    case 3: // All done
      Serial.println("G28 Auto-Homing completed successfully!");
      Serial.println("All axes are now at their home positions");
      isHoming = false;
      break;
  }
}

// Function to home a specific axis
void homeAxis(int axis) {
  int dirPin, stepPin, limitPin;
  
  // Set up axis-specific pins
  switch (axis) {
    case 0: // X-axis
      dirPin = DIR_X;
      stepPin = STEP_X;
      limitPin = LIMIT_SWITCH_X;
      xDirIsNegative = true;  // Moving in negative direction
      break;
      
    case 1: // Y-axis
      dirPin = DIR_Y;
      stepPin = STEP_Y;
      limitPin = LIMIT_SWITCH_Y;
      yDirIsNegative = true;  // Moving in negative direction
      break;
      
    case 2: // Z-axis
      dirPin = DIR_Z;
      stepPin = STEP_Z;
      limitPin = LIMIT_SWITCH_Z;
      zDirIsNegative = true;  // Moving in negative direction
      break;
      
    default:
      return; // Invalid axis
  }
  
  // Set direction to negative (toward limit switch)
  digitalWrite(dirPin, DIR_COUNTER_CLOCKWISE);
  delayMicroseconds(50);
  
  // Move until limit switch is hit or emergency stop
  int maxSteps = 10000; // Safety limit
  int stepCount = 0;
  
  while (digitalRead(limitPin) == HIGH && stepCount < maxSteps) {
    // Check for emergency stop
    if (checkEmergencyStop()) {
      Serial.println("Homing interrupted by emergency stop");
      isHoming = false;
      return;
    }
    
    // Take one step
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(homingSpeed);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(homingSpeed);
    
    stepCount++;
    
    // Add short delay every 100 steps to allow checking for user input
    if (stepCount % 100 == 0) {
      delay(1);
    }
  }
  
  // Check if homing completed successfully
  if (stepCount >= maxSteps) {
    Serial.print("Warning: Limit switch not found for axis ");
    switch (axis) {
      case 0: Serial.println("X"); break;
      case 1: Serial.println("Y"); break;
      case 2: Serial.println("Z"); break;
    }
  } else {
    Serial.print("Limit switch reached for axis ");
    switch (axis) {
      case 0: Serial.println("X"); break;
      case 1: Serial.println("Y"); break;
      case 2: Serial.println("Z"); break;
    }
    
    // Back off slightly from limit switch
    backoffFromLimitSwitch(dirPin, stepPin, homingBackoff);
    
    // Reset position counter for this axis
    switch (axis) {
      case 0: // X-axis
        xAxisEnabled = true;
        xDirIsNegative = false;
        break;
      case 1: // Y-axis
        yAxisEnabled = true;
        yDirIsNegative = false;
        break;
      case 2: // Z-axis
        zAxisEnabled = true;
        zDirIsNegative = false;
        break;
    }
  }
}

// Function to back off slightly from limit switch after hitting it
void backoffFromLimitSwitch(int dirPin, int stepPin, int steps) {
  // Reverse direction to move away from switch
  digitalWrite(dirPin, DIR_CLOCKWISE);
  delayMicroseconds(50);
  
  // Move specified number of steps
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(homingSpeed * 2); // Move slower during backoff
    digitalWrite(stepPin, LOW);
    delayMicroseconds(homingSpeed * 2);
  }
}

// Function to check for emergency stop and handle it
bool checkEmergencyStop() {
  if (digitalRead(STOP_BTN) == LOW) {
    // Emergency stop is activated!
    emergencyStop = true;
    
    // Immediately disable all motors
    digitalWrite(EN_PIN, HIGH);
    
    // Reset all step pins to ensure motors stop immediately
    digitalWrite(STEP_X, LOW);
    digitalWrite(STEP_Y, LOW);
    digitalWrite(STEP_Z, LOW);
    
    Serial.println("!!! EMERGENCY STOP ACTIVATED !!!");
    Serial.println("All motors stopped immediately");
    
    // Wait for stop button to be released
    while(digitalRead(STOP_BTN) == LOW) {
      delay(100); // Check every 100ms to save resources
    }
    
    // Wait additional time for debounce
    delay(200);
    
    // Re-enable motors
    digitalWrite(EN_PIN, LOW);
    
    Serial.println("Emergency stop cleared");
    Serial.println("Motors re-enabled");
    
    // Clear emergency stop flag
    emergencyStop = false;
    return true; // Return true to indicate emergency stop was activated
  }
  
  return false; // Return false if no emergency stop was detected
}

// Function to check limit switches and disable appropriate axes
void checkLimitSwitches() {
  // Check X-axis limit switch
  if (digitalRead(LIMIT_SWITCH_X) == LOW) {
    if (xDirIsNegative) {  // Only stop if moving in negative direction
      xAxisEnabled = false;
      Serial.println("X-AXIS LIMIT REACHED - X-axis movement disabled in negative direction");
    }
  } else {
    // If limit switch is not pressed, ensure axis is enabled
    if (!xAxisEnabled) {
      xAxisEnabled = true;
      Serial.println("X-axis re-enabled");
    }
  }
  
  // Check Y-axis limit switch
  if (digitalRead(LIMIT_SWITCH_Y) == LOW) {
    if (yDirIsNegative) {  // Only stop if moving in negative direction
      yAxisEnabled = false;
      Serial.println("Y-AXIS LIMIT REACHED - Y-axis movement disabled in negative direction");
    }
  } else {
    // If limit switch is not pressed, ensure axis is enabled
    if (!yAxisEnabled) {
      yAxisEnabled = true;
      Serial.println("Y-axis re-enabled");
    }
  }
  
  // Check Z-axis limit switch
  if (digitalRead(LIMIT_SWITCH_Z) == LOW) {
    if (zDirIsNegative) {  // Only stop if moving in negative direction
      zAxisEnabled = false;
      Serial.println("Z-AXIS LIMIT REACHED - Z-axis movement disabled in negative direction");
    }
  } else {
    // If limit switch is not pressed, ensure axis is enabled
    if (!zAxisEnabled) {
      zAxisEnabled = true;
      Serial.println("Z-axis re-enabled");
    }
  }
}

// Function to reset all axis enable flags
void resetAxisEnables() {
  xAxisEnabled = true;
  yAxisEnabled = true;
  zAxisEnabled = true;
}

// Standard function to move a stepper motor a specified number of steps
void moveMotor(int dirPin, int stepPin, int steps, bool direction) {
  // Set direction
  digitalWrite(dirPin, direction);
  
  // Add a small delay after setting direction
  delayMicroseconds(50);
  
  // Move the specified number of steps
  for (int i = 0; i < steps; i++) {
    // Check for emergency stop at each step
    if (checkEmergencyStop()) {
      return; // Immediately exit if emergency stop is activated
    }
    
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(STEP_DELAY);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(STEP_DELAY);
    
    // Check every 10 steps to avoid too frequent checks which could slow down the motor
    if (i % 10 == 0) {
      if (checkEmergencyStop()) {
        return;
      }
    }
  }
}

// Function to move a motor with limit switch checking for negative movement
void moveMotorWithLimitCheck(int dirPin, int stepPin, int limitPin, int steps, bool direction) {
  // Set direction
  digitalWrite(dirPin, direction);
  
  // Add a small delay after setting direction
  delayMicroseconds(50);
  
  // Move the specified number of steps, checking limit switch at each step
  for (int i = 0; i < steps; i++) {
    // Check for emergency stop first
    if (checkEmergencyStop()) {
      return; // Immediately exit if emergency stop is activated
    }
    
    // Then check if limit switch is activated
    if (digitalRead(limitPin) == LOW) {
      Serial.print("Limit switch activated during movement. Stopped after ");
      Serial.print(i);
      Serial.println(" steps");
      return;  // Exit immediately if limit switch is hit
    }
    
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(STEP_DELAY);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(STEP_DELAY);
    
    // Check every 10 steps to avoid too frequent checks which could slow down the motor
    if (i % 10 == 0) {
      if (checkEmergencyStop() || digitalRead(limitPin) == LOW) {
        return;
      }
    }
  }
}