//x-axis
#define DIR_X 4
#define STEP_X 5

//y-axis
#define DIR_Y 6
#define STEP_Y 7

//Z- axis
#define DIR_Z 8
#define STEP_Z 9

//Extruder
#define DIR_EX 10
#define STEP_EX 11

//Switch pin
#define Xp  24
#define Xm  25
#define Yp  22
#define Ym  23
#define Zp  26
#define Zm  27

// New position control switches
#define POS_1MM 30  // Switch for 1mm movement (5 degrees rotation)
#define POS_10MM 28 // Switch for 10mm movement (50 degrees rotation)

// Stop button and limit switches
#define STOP_BTN 33      // Emergency stop button
#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

// เพิ่มการกำหนด PIN สำหรับปุ่ม Print
#define PRINT_BTN 34  // สมมติว่าใช้ PIN 34, ปรับตามที่ต้องการ

// เพิ่มตัวแปรสำหรับเก็บสถานะการพิมพ์
bool isPrinting = false;
int currentPrintStep = 0;

// กำหนดคำสั่ง G-code ที่จะรันเมื่อกดปุ่ม Print
const int PRINT_SEQUENCE_SIZE = 5;
String printSequence[PRINT_SEQUENCE_SIZE] = {
  "G28",         // Home
  "X10 Y20 Z5",  // ไปยังจุดเริ่มต้น
  "X10 Y25 Z5",  // วาดเส้นแรก
  "X5 Y25 Z5",   // วาดเส้นที่สอง
  "X5 Y20 Z5"    // วาดเส้นที่สาม (กลับมาจุดเริ่มต้น)
};

// Enable pin (not defined in original code)
#define EN_PIN 12  // Assuming this is pin 12, adjust as needed

int step_rev = 3200; // 360*16/1.8 = 3200
int dir = 1;
int i = 0;
bool STATE = LOW;
bool STATE_DIR = HIGH;

// Variables to track current position
int currentStepsX = 0;
int currentStepsY = 0;
int currentStepsZ = 0;

// Conversion factors (steps per unit)
float stepsPerUnitX = 8.8; // Assuming 80 steps = 1mm for X-axis
float stepsPerUnitY = 8.8; // Assuming 80 steps = 1mm for Y-axis
float stepsPerUnitZ = 8.8; // Assuming 80 steps = 1mm for Z-axis

// Button debouncing variables
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50; // Debounce time in ms

// Motor speed and step settings
int stepDelay = 500; // Microseconds between steps (adjust for speed)
int stepsPerButtonPress = 100; // Default steps per button press

// Homing speed (slower for accuracy)
int homingStepDelay = 1000; // Slower step delay for homing

// Movement settings
int stepsFor1mm = 44;  // Steps needed for 5 degrees (1mm) - approximately step_rev * (5/360)
int stepsFor10mm = 444; // Steps needed for 50 degrees (10mm) - approximately step_rev * (50/360)

// System state
bool systemEnabled = true; // Flag to indicate if the system is enabled or stopped
bool isHoming = false;     // Flag to indicate if system is currently homing
bool isAutoLeveling = false; // Flag to indicate if system is currently auto-leveling

// Serial command handling
String inputString = "";         // String to hold incoming data
boolean stringComplete = false;  // Whether the string is complete

// Auto leveling points
// Format: X, Y, Z coordinates
float levelingPoints[][3] = {
  {1, 1, 1},    // Point 1
  {1, 1, 5},    // Point 2
  {30, 1, 1},   // Point 3
  {30, 30, 5},  // Point 4
  {30, 30, 1},    // Point 5
  {30, 30, 5},    //Point 6
  {1, 30, 1},   //Point 7
  {1, 30, 5}    //Point 8
 };
int numLevelingPoints = 8; // Number of points in the leveling array
int currentLevelingPoint = 0; // Current point being processed

void setup() {
  // put your setup code here, to run once:
  //Motor
  pinMode(EN_PIN, OUTPUT);
  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(DIR_EX, OUTPUT);
  pinMode(STEP_EX, OUTPUT);

  //Sw
  pinMode(Xp, INPUT_PULLUP);
  pinMode(Xm, INPUT_PULLUP);
  pinMode(Yp, INPUT_PULLUP);
  pinMode(Ym, INPUT_PULLUP);
  pinMode(Zp, INPUT_PULLUP);
  pinMode(Zm, INPUT_PULLUP);
  
  // New position control switches
  pinMode(POS_1MM, INPUT_PULLUP);
  pinMode(POS_10MM, INPUT_PULLUP);
  
  // Stop button and limit switches
  pinMode(STOP_BTN, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_X, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_Y, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_Z, INPUT_PULLUP);

  pinMode(PRINT_BTN, INPUT_PULLUP);

  // Enable motors (LOW = enabled, HIGH = disabled)
  digitalWrite(EN_PIN, LOW);

  // Reserve 200 bytes for the inputString
  inputString.reserve(200);

  // Serial begin
  Serial.begin(115200);
  Serial.println("CNC Control System Ready");
  Serial.println("G-Code support enabled:");
  Serial.println("  G28: Home all axes");
  Serial.println("  G29: Auto bed leveling");
}

void loop() {
  // Process any incoming serial commands
  // Process any incoming serial commands
  if (stringComplete) {
    processCommand(inputString);
    inputString = "";
    stringComplete = false;
  }
  
  // ตรวจสอบปุ่ม Print
  checkPrintButton();
  
  // ดำเนินการพิมพ์ตามลำดับถ้ากำลังพิมพ์อยู่
  if (isPrinting) {
    executePrintSequence();
  }

  // Check if auto-leveling sequence needs to continue
  if (isAutoLeveling) {
    executeAutoLevelingSequence();
  }
  
  // Check if stop button or limit switches are activated (except during homing)
  if (!isHoming) {
    checkStopAndLimits();
  }
  
  // Only process movements if system is enabled and not in auto modes
  if (systemEnabled && !isHoming && !isAutoLeveling) {
    // Update movement distance based on position control switches
    updateMovementDistance();
    
    // Check buttons and move motors accordingly
    checkAndMoveX();
    checkAndMoveY();
    checkAndMoveZ();
  }
  
  // Read any incoming serial data
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    
    // Add character to input string
    if (inChar == '\n') {
      stringComplete = true;
    } else {
      inputString += inChar;
    }
  }
}

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
    homeAllAxes();
  } 
  else if (command.startsWith("G29")) {
    // G29 - Auto bed leveling
    startAutoLeveling();
  }
  else if (command.startsWith("X") || command.startsWith("Y") || command.startsWith("Z")) {
    // คำสั่งเคลื่อนที่โดยตรง (X, Y, Z)
    moveToCoordinates(command);
  }
  else {
    Serial.println("Unknown command");
  }
}

void startAutoLeveling() {
  if (!systemEnabled) {
    Serial.println("Cannot perform auto-leveling: System is disabled");
    return;
  }
  
  Serial.println("Starting auto bed leveling sequence");
  isAutoLeveling = true;
  currentLevelingPoint = 0;
  
  // Move to the first point immediately
  moveToPoint(levelingPoints[currentLevelingPoint][0], 
              levelingPoints[currentLevelingPoint][1], 
              levelingPoints[currentLevelingPoint][2]);
  
  currentLevelingPoint++;
}

// Update the executeAutoLevelingSequence function to move directly to the next point 
// without waiting between points
void executeAutoLevelingSequence() {
  // Check if we still have points to process
  if (currentLevelingPoint < numLevelingPoints) {
    Serial.print("Moving to auto-leveling point ");
    Serial.print(currentLevelingPoint + 1);
    Serial.print(" of ");
    Serial.println(numLevelingPoints);
    
    moveToPoint(levelingPoints[currentLevelingPoint][0], 
                levelingPoints[currentLevelingPoint][1], 
                levelingPoints[currentLevelingPoint][2]);
    
    currentLevelingPoint++;
  } else {
    // We've completed all points
    Serial.println("Auto bed leveling sequence completed");
    isAutoLeveling = false;
  }
}

// The moveToPoint function needs to be modified to move all axes simultaneously
void moveToPoint(float x, float y, float z) {
  Serial.print("Moving to X:");
  Serial.print(x);
  Serial.print(" Y:");
  Serial.print(y);
  Serial.print(" Z:");
  Serial.println(z);
  
  // Calculate steps needed for each axis
  int targetStepsX = round(x * stepsPerUnitX);
  int targetStepsY = round(y * stepsPerUnitY);
  int targetStepsZ = round(z * stepsPerUnitZ);
  
  // Calculate step differences
  int diffX = targetStepsX - currentStepsX;
  int diffY = targetStepsY - currentStepsY;
  int diffZ = targetStepsZ - currentStepsZ;
  
  // Set directions for all axes
  boolean dirX = (diffX > 0) ? LOW : HIGH;
  boolean dirY = (diffY > 0) ? LOW : HIGH;
  boolean dirZ = (diffZ > 0) ? LOW : HIGH;
  
  digitalWrite(DIR_X, dirX);
  digitalWrite(DIR_Y, dirY);
  digitalWrite(DIR_Z, dirZ);
  
  // Find maximum number of steps needed
  int maxSteps = max(abs(diffX), max(abs(diffY), abs(diffZ)));
  
  // Calculate step ratios (used to determine when to step each axis)
  float ratioX = (float)abs(diffX) / maxSteps;
  float ratioY = (float)abs(diffY) / maxSteps;
  float ratioZ = (float)abs(diffZ) / maxSteps;
  
  // Accumulators for fractional steps
  float accX = 0, accY = 0, accZ = 0;
  
  // Move all axes simultaneously
  for (int i = 0; i < maxSteps; i++) {
    // Check stop conditions during movement
    if (digitalRead(STOP_BTN) == LOW || 
        (!isHoming && (digitalRead(LIMIT_SWITCH_X) == LOW || 
                      digitalRead(LIMIT_SWITCH_Y) == LOW || 
                      digitalRead(LIMIT_SWITCH_Z) == LOW))) {
      
      stopAllMotors();
      Serial.println("Movement interrupted by stop condition");
      isAutoLeveling = false;
      return;
    }
    
    // Accumulate step fractions for each axis
    accX += ratioX;
    accY += ratioY;
    accZ += ratioZ;
    
    // Step X axis if needed
    if (accX >= 1.0) {
      digitalWrite(STEP_X, HIGH);
      accX -= 1.0;
    }
    
    // Step Y axis if needed
    if (accY >= 1.0) {
      digitalWrite(STEP_Y, HIGH);
      accY -= 1.0;
    }
    
    // Step Z axis if needed
    if (accZ >= 1.0) {
      digitalWrite(STEP_Z, HIGH);
      accZ -= 1.0;
    }
    
    // Allow time for pulse HIGH
    delayMicroseconds(stepDelay);
    
    // Reset all step pins to LOW
    digitalWrite(STEP_X, LOW);
    digitalWrite(STEP_Y, LOW);
    digitalWrite(STEP_Z, LOW);
    
    // Allow time between steps
    delayMicroseconds(stepDelay);
  }
  
  // Update current positions
  currentStepsX = targetStepsX;
  currentStepsY = targetStepsY;
  currentStepsZ = targetStepsZ;
  
  Serial.println("Simultaneous movement complete");
}

void homeAllAxes() {
  if (!systemEnabled) {
    Serial.println("Cannot home: System is disabled");
    return;
  }
  
  isHoming = true;
  Serial.println("Homing all axes (X, Y, Z)...");
  
  // Home X axis
  homeAxis('X', DIR_X, STEP_X, LIMIT_SWITCH_X);
  
  // Home Y axis
  homeAxis('Y', DIR_Y, STEP_Y, LIMIT_SWITCH_Y);
  
  // Home Z axis
  homeAxis('Z', DIR_Z, STEP_Z, LIMIT_SWITCH_Z);
  
  // Reset all position counters
  currentStepsX = 0;
  currentStepsY = 0;
  currentStepsZ = 0;
  
  Serial.println("Homing complete. All axes at home position (0,0,0)");
  isHoming = false;
}

void homeAxis(char axis, int dirPin, int stepPin, int limitPin) {
  Serial.print("Homing axis ");
  Serial.print(axis);
  Serial.println("...");
  
  // Set direction to home (usually negative direction)
  digitalWrite(dirPin, HIGH);
  
  // Move the axis until limit switch is triggered
  int maxSteps = 10000; // Safety limit to prevent endless movement
  int stepCount = 0;
  
  while (digitalRead(limitPin) == HIGH && stepCount < maxSteps) {
    // Check emergency stop during homing
    if (digitalRead(STOP_BTN) == LOW) {
      stopAllMotors();
      Serial.println("Homing interrupted by emergency stop");
      isHoming = false;
      return;
    }
    
    // Move one step
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(homingStepDelay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(homingStepDelay);
    
    stepCount++;
    
    // Short delay every 100 steps to check for user input
    if (stepCount % 100 == 0) {
      delay(1);
    }
  }
  
  if (stepCount >= maxSteps) {
    Serial.print("WARNING: Axis ");
    Serial.print(axis);
    Serial.println(" limit switch not found");
  } else {
    Serial.print("Axis ");
    Serial.print(axis);
    Serial.println(" homed successfully");
    
    // Back off slightly from the limit switch
    digitalWrite(dirPin, LOW); // Reverse direction
    for (int i = 0; i < 50; i++) {
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(homingStepDelay * 2);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(homingStepDelay * 2);
    }
  }
}

void checkStopAndLimits() {
  // Check stop button
  if (digitalRead(STOP_BTN) == LOW) {
    stopAllMotors();
    Serial.println("EMERGENCY STOP ACTIVATED");
    
    // Also cancel any auto-leveling in progress
    isAutoLeveling = false;
  }
  
  // Check limit switches (only if not homing)
  if (!isHoming) {
    if (digitalRead(LIMIT_SWITCH_X) == LOW) {
      stopAllMotors();
      Serial.println("X-AXIS LIMIT REACHED");
      isAutoLeveling = false;
    }
    
    if (digitalRead(LIMIT_SWITCH_Y) == LOW) {
      stopAllMotors();
      Serial.println("Y-AXIS LIMIT REACHED");
      isAutoLeveling = false;
    }
    
    if (digitalRead(LIMIT_SWITCH_Z) == LOW) {
      stopAllMotors();
      Serial.println("Z-AXIS LIMIT REACHED");
      isAutoLeveling = false;
    }
  }
  
  // If system was disabled and all buttons are now released, re-enable system
  if (!systemEnabled && 
      digitalRead(STOP_BTN) == HIGH && 
      digitalRead(LIMIT_SWITCH_X) == HIGH && 
      digitalRead(LIMIT_SWITCH_Y) == HIGH && 
      digitalRead(LIMIT_SWITCH_Z) == HIGH) {
    
    // Wait a bit to make sure switches are stable
    delay(100);
    
    // Check again (double-check debounce)
    if (digitalRead(STOP_BTN) == HIGH && 
        digitalRead(LIMIT_SWITCH_X) == HIGH && 
        digitalRead(LIMIT_SWITCH_Y) == HIGH && 
        digitalRead(LIMIT_SWITCH_Z) == HIGH) {
      
      systemEnabled = true;
      digitalWrite(EN_PIN, LOW); // Re-enable motor drivers
      Serial.println("System re-enabled");
    }
  }
}

void stopAllMotors() {
  systemEnabled = false;
  digitalWrite(EN_PIN, HIGH); // Disable motor drivers
}

void updateMovementDistance() {
  // Check position control switches and set movement distance accordingly
  if (digitalRead(POS_1MM) == LOW) {
    stepsPerButtonPress = stepsFor1mm;
    Serial.println("Movement mode: 1mm (5 degrees)");
    delay(300); // Simple debounce to prevent multiple prints
  }
  
  if (digitalRead(POS_10MM) == LOW) {
    stepsPerButtonPress = stepsFor10mm;
    Serial.println("Movement mode: 10mm (50 degrees)");
    delay(300); // Simple debounce to prevent multiple prints
  }
}

void checkAndMoveX() {
  // X positive direction
  if (digitalRead(Xp) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_X, LOW); // Set direction positive
      moveMotor(STEP_X, stepsPerButtonPress);
      currentStepsX += stepsPerButtonPress;
      Serial.print("X position: ");
      Serial.println(currentStepsX);
      lastDebounceTime = millis();
    }
  }
  
  // X negative direction
  if (digitalRead(Xm) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_X, HIGH); // Set direction negative
      moveMotor(STEP_X, stepsPerButtonPress);
      currentStepsX -= stepsPerButtonPress;
      Serial.print("X position: ");
      Serial.println(currentStepsX);
      lastDebounceTime = millis();
    }
  }
}

void checkAndMoveY() {
  // Y positive direction
  if (digitalRead(Yp) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_Y, LOW); // Set direction positive
      moveMotor(STEP_Y, stepsPerButtonPress);
      currentStepsY += stepsPerButtonPress;
      Serial.print("Y position: ");
      Serial.println(currentStepsY);
      lastDebounceTime = millis();
    }
  }
  
  // Y negative direction
  if (digitalRead(Ym) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_Y, HIGH); // Set direction negative
      moveMotor(STEP_Y, stepsPerButtonPress);
      currentStepsY -= stepsPerButtonPress;
      Serial.print("Y position: ");
      Serial.println(currentStepsY);
      lastDebounceTime = millis();
    }
  }
}

void checkAndMoveZ() {
  // Z positive direction
  if (digitalRead(Zp) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_Z, LOW); // Set direction positive
      moveMotor(STEP_Z, stepsPerButtonPress);
      currentStepsZ += stepsPerButtonPress;
      Serial.print("Z position: ");
      Serial.println(currentStepsZ);
      lastDebounceTime = millis();
    }
  }
  
  // Z negative direction
  if (digitalRead(Zm) == LOW) {
    // Debounce
    if ((millis() - lastDebounceTime) > debounceDelay) {
      digitalWrite(DIR_Z, HIGH); // Set direction negative
      moveMotor(STEP_Z, stepsPerButtonPress);
      currentStepsZ -= stepsPerButtonPress;
      Serial.print("Z position: ");
      Serial.println(currentStepsZ);
      lastDebounceTime = millis();
    }
  }
}

void moveMotor(int stepPin, int steps) {
  // Check for stop conditions during motor movement  
  for (int i = 0; i < steps; i++) {
    // Check stop conditions during movement
    if (digitalRead(STOP_BTN) == LOW || 
        (!isHoming && (digitalRead(LIMIT_SWITCH_X) == LOW || 
                      digitalRead(LIMIT_SWITCH_Y) == LOW || 
                      digitalRead(LIMIT_SWITCH_Z) == LOW))) {
      
      stopAllMotors();
      Serial.println("Movement interrupted by stop condition");
      isAutoLeveling = false; // Also cancel auto-leveling if in progress
      return; // Exit the function immediately
    }
    
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(stepDelay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(stepDelay);
  }
}

void checkPrintButton() {
  static unsigned long lastPrintButtonTime = 0;
  
  // ตรวจสอบปุ่ม Print พร้อมกับการดีบาวน์ซ
  if (digitalRead(PRINT_BTN) == LOW) {
    if ((millis() - lastPrintButtonTime) > 300) {  // Simple debounce
      if (!isPrinting && systemEnabled) {
        // เริ่มการพิมพ์
        Serial.println("Starting print sequence");
        isPrinting = true;
        currentPrintStep = 0;
        
        // ดำเนินการคำสั่งแรกทันที (G28 - Home)
        processCommand(printSequence[currentPrintStep]);
        currentPrintStep++;
      }
      lastPrintButtonTime = millis();
    }
  }
}

// Update executePrintSequence to move faster between steps
void executePrintSequence() {
  // Check if there are commands left to execute
  if (currentPrintStep < PRINT_SEQUENCE_SIZE) {
    // Execute the next command immediately
    processCommand(printSequence[currentPrintStep]);
    currentPrintStep++;
    
    // Display progress
    Serial.print("Print step ");
    Serial.print(currentPrintStep);
    Serial.print(" of ");
    Serial.println(PRINT_SEQUENCE_SIZE);
  } else {
    // Printing is complete
    Serial.println("Print sequence completed");
    isPrinting = false;
  }
}

void moveToCoordinates(String command) {
  float x = -1, y = -1, z = -1, e = -1;
  
  // แยกวิเคราะห์คำสั่ง
  int pos = 0;
  while (pos < command.length()) {
    char axis = command.charAt(pos);
    pos++;
    
    // ข้ามช่องว่าง
    while (pos < command.length() && command.charAt(pos) == ' ') pos++;
    
    // อ่านค่า
    String value = "";
    while (pos < command.length() && 
           (isDigit(command.charAt(pos)) || command.charAt(pos) == '.')) {
      value += command.charAt(pos);
      pos++;
    }
    
    // กำหนดค่าให้แกนที่เหมาะสม
    if (value.length() > 0) {
      float val = value.toFloat();
      if (axis == 'X') x = val;
      else if (axis == 'Y') y = val;
      else if (axis == 'Z') z = val;
      else if (axis == 'E') e = val;
    }
  }
  
  // เคลื่อนที่ไปยังตำแหน่งที่กำหนด
  moveToPosition(x, y, z, e);
}

// Similarly update the moveToPosition function to move all axes simultaneously
void moveToPosition(float x, float y, float z, float e) {
  // Calculate target steps for all specified axes
  int targetStepsX = (x >= 0) ? round(x * stepsPerUnitX) : currentStepsX;
  int targetStepsY = (y >= 0) ? round(y * stepsPerUnitY) : currentStepsY;
  int targetStepsZ = (z >= 0) ? round(z * stepsPerUnitZ) : currentStepsZ;
  int targetStepsE = (e >= 0) ? round(e * stepsPerUnitX) : 0; // Using X conversion for E
  
  // Calculate step differences
  int diffX = targetStepsX - currentStepsX;
  int diffY = targetStepsY - currentStepsY;
  int diffZ = targetStepsZ - currentStepsZ;
  int diffE = targetStepsE; // For extruder, we use absolute values
  
  // Set directions for all axes
  digitalWrite(DIR_X, (diffX > 0) ? HIGH : LOW);
  digitalWrite(DIR_Y, (diffY > 0) ? HIGH : LOW);
  digitalWrite(DIR_Z, (diffZ > 0) ? HIGH : LOW);
  digitalWrite(DIR_EX, (diffE > 0) ? HIGH : LOW);
  
  // Find maximum number of steps needed
  int maxSteps = max(abs(diffX), max(abs(diffY), max(abs(diffZ), abs(diffE))));
  
  if (maxSteps == 0) {
    Serial.println("No movement needed");
    return;
  }
  
  // Calculate step ratios
  float ratioX = (float)abs(diffX) / maxSteps;
  float ratioY = (float)abs(diffY) / maxSteps;
  float ratioZ = (float)abs(diffZ) / maxSteps;
  float ratioE = (float)abs(diffE) / maxSteps;
  
  // Accumulators for fractional steps
  float accX = 0, accY = 0, accZ = 0, accE = 0;
  
  // Move all axes simultaneously
  for (int i = 0; i < maxSteps; i++) {
    // Check stop conditions during movement
    if (digitalRead(STOP_BTN) == LOW || 
        (!isHoming && (digitalRead(LIMIT_SWITCH_X) == LOW || 
                      digitalRead(LIMIT_SWITCH_Y) == LOW || 
                      digitalRead(LIMIT_SWITCH_Z) == LOW))) {
      
      stopAllMotors();
      Serial.println("Movement interrupted by stop condition");
      isPrinting = false;
      return;
    }
    
    // Accumulate step fractions for each axis
    accX += ratioX;
    accY += ratioY;
    accZ += ratioZ;
    accE += ratioE;
    
    // Step X axis if needed and if movement was requested
    if (accX >= 1.0 && x >= 0) {
      digitalWrite(STEP_X, HIGH);
      accX -= 1.0;
    }
    
    // Step Y axis if needed and if movement was requested
    if (accY >= 1.0 && y >= 0) {
      digitalWrite(STEP_Y, HIGH);
      accY -= 1.0;
    }
    
    // Step Z axis if needed and if movement was requested
    if (accZ >= 1.0 && z >= 0) {
      digitalWrite(STEP_Z, HIGH);
      accZ -= 1.0;
    }
    
    // Step Extruder if needed and if movement was requested
    if (accE >= 1.0 && e >= 0) {
      digitalWrite(STEP_EX, HIGH);
      accE -= 1.0;
    }
    
    // Allow time for pulse HIGH
    delayMicroseconds(stepDelay);
    
    // Reset all step pins to LOW
    digitalWrite(STEP_X, LOW);
    digitalWrite(STEP_Y, LOW);
    digitalWrite(STEP_Z, LOW);
    digitalWrite(STEP_EX, LOW);
    
    // Allow time between steps
    delayMicroseconds(stepDelay);
  }
  
  // Update current positions
  if (x >= 0) currentStepsX = targetStepsX;
  if (y >= 0) currentStepsY = targetStepsY;
  if (z >= 0) currentStepsZ = targetStepsZ;
  
  Serial.println("Simultaneous movement to position complete");
}