#include <AccelStepper.h>

#define X_STEP_PIN 2
#define X_DIR_PIN 3
#define Y_STEP_PIN 4
#define Y_DIR_PIN 5
#define Z_STEP_PIN 6
#define Z_DIR_PIN 7

#define STEPS_PER_MM 80 // Adjust based on your setup

AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);

void setup() {
  Serial.begin(9600);
  stepperX.setMaxSpeed(1000.0);
  stepperX.setAcceleration(500.0);
  stepperY.setMaxSpeed(1000.0);
  stepperY.setAcceleration(500.0);
  stepperZ.setMaxSpeed(500.0);  // Adjust for Z-axis
  stepperZ.setAcceleration(200.0); // Adjust for Z-axis
}

void loop() {
  // Replace this section with your actual application logic
  moveStepper(&stepperX, 10);

  moveStepper(&stepperY, 20);
 
  moveStepper(&stepperZ, 5);  // Adjust for Z-axis
  
    // Adjust for Z-axis
 
}

void executeGCode(String gCode) {
  char command = gCode.charAt(0);
  gCode.remove(0, 1); // Remove the command character

  switch (command) {
    case 'G':
      processGCode(gCode);
      break;
    default:
      Serial.println("Unknown command");
      break;
  }
}

void processGCode(String gCode) {
  int xValueIndex = gCode.indexOf('X');
  int yValueIndex = gCode.indexOf('Y');
  int zValueIndex = gCode.indexOf('Z');

  if (xValueIndex != -1) {
    float xValue = gCode.substring(xValueIndex + 1).toFloat();
    Serial.print("Moving X-axis to: ");
    Serial.println(xValue);
    moveStepper(&stepperX, xValue);
  }

  if (yValueIndex != -1) {
    float yValue = gCode.substring(yValueIndex + 1).toFloat();
    Serial.print("Moving Y-axis to: ");
    Serial.println(yValue);
    moveStepper(&stepperY, yValue);
  }

  if (zValueIndex != -1) {
    float zValue = gCode.substring(zValueIndex + 1).toFloat();
    Serial.print("Moving Z-axis to: ");
    Serial.println(zValue);
    moveStepper(&stepperZ, zValue);
  }
}

void moveStepper(AccelStepper *stepper, float targetPosition) {
  int steps = targetPosition * STEPS_PER_MM; // Adjust STEPS_PER_MM based on your setup
  Serial.print("Moving stepper ");
  Serial.print(stepper == &stepperX ? "X" : (stepper == &stepperY ? "Y" : "Z"));
  Serial.print(" by ");
  Serial.print(targetPosition);
  Serial.println(" mm");

  stepper->move(steps);

  while (stepper->distanceToGo() != 0) {
    stepper->run();
  }
}