#include <AccelStepper.h>
#include <MultiStepper.h>

String inputString = "";
bool stringComplete = false;

// Create 3 steppers (A4988 = DRIVER mode)
AccelStepper stepperX(AccelStepper::DRIVER, 2, 5);
AccelStepper stepperY(AccelStepper::DRIVER, 3, 6);
AccelStepper stepperZ(AccelStepper::DRIVER, 4, 7);
AccelStepper stepperZ2(AccelStepper::DRIVER, 11, 12);

// MultiStepper for synchronized motion
MultiStepper steppers;

void setup() {
  Serial.begin(115200);
  Serial.println("Ready");

  // Default max speed
  stepperX.setMaxSpeed(1000);
  stepperY.setMaxSpeed(1000);
  stepperZ.setMaxSpeed(1000);
  stepperZ2.setMaxSpeed(1000);

  // Add motors to multi-stepper group
  steppers.addStepper(stepperX);
  steppers.addStepper(stepperY);
  steppers.addStepper(stepperZ);
  steppers.addStepper(stepperZ2);
}

void loop() {

  // ---- READ SERIAL INPUT ----
  if (Serial.available()) {
    char c = Serial.read();
    if (c == '\n') {
      stringComplete = true;
    } else {
      inputString += c;
    }
  }

  // ---- PROCESS COMMAND ----
  if (stringComplete) {
    processCommand(inputString);
    inputString = "";
    stringComplete = false;
  }

  stepperX.run();
  stepperY.run();
  stepperZ.run();
  stepperZ2.run();
}


// ------------------------------------------------------------------
// Parse CSV "X,Y,Z,Speed"   example: 150,-50,200,1200
// ------------------------------------------------------------------
void processCommand(String csv) {

  // Find commas
  int c1 = csv.indexOf(',');
  int c2 = csv.indexOf(',', c1 + 1);
  int c3 = csv.indexOf(',', c2 + 1);
  

  if (c1 < 0 || c2 < 0 || c3 < 0) {
    Serial.println("ERR: send X,Y,Z,SPEED");
    return;
  }

  long xSteps = csv.substring(0, c1).toInt();
  long ySteps = csv.substring(c1 + 1, c2).toInt();
  long zSteps = csv.substring(c2 + 1, c3).toInt();
  long z2Steps = csv.substring(c2 + 1, c3).toInt();
  long maxSpeed = csv.substring(c3 + 1).toInt();

  if (maxSpeed < 1) maxSpeed = 1;
  if (maxSpeed > 1000) maxSpeed = 1000;


  Serial.print("Move X=");
  Serial.print(xSteps);
  Serial.print(" Y=");
  Serial.print(ySteps);
  Serial.print(" Z=");
  Serial.print(zSteps);
  Serial.print(" Speed=");
  Serial.println(maxSpeed);

  // ---- APPLY NEW SPEED ----
  stepperX.setMaxSpeed(maxSpeed);
  stepperY.setMaxSpeed(maxSpeed);
  stepperZ.setMaxSpeed(maxSpeed);
  stepperZ2.setMaxSpeed(maxSpeed);

  // ---- ABSOLUTE TARGET POSITIONS ----
  // Use ~[value] to reverse the stepper direction
  long targets[4];
  targets[0] = stepperX.currentPosition() + xSteps;
  targets[1] = stepperY.currentPosition() + ySteps;
  targets[2] = stepperZ.currentPosition() + zSteps;
  targets[3] = stepperZ2.currentPosition() + z2Steps;


  // ---- SYNCHRONIZED MOVE ----
  steppers.moveTo(targets);
  steppers.runSpeedToPosition();
}