#include <Arduino.h>
#include <AccelStepper.h>


// X Y and Z axes are normal, positive steps go in the positive direction for each axis (i.e. X 200 goes 200 steps right)
// Drivers are set up for 1/8 microstepping (1600 steps / rev)
// X Stepper
#define X_STEP 2
#define X_DIR 5
AccelStepper xStepper(AccelStepper::DRIVER, X_STEP, X_DIR);

// Y Stepper (Y1 and Y2 are connected to the same pins)
#define Y_STEP 3
#define Y_DIR 6
AccelStepper yStepper(AccelStepper::DRIVER, Y_STEP, Y_DIR);

// Z Stepper 
#define Z_STEP 4
#define Z_DIR 7
AccelStepper zStepper(AccelStepper::DRIVER, Z_STEP, Z_DIR);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Setup done");

  xStepper.setMaxSpeed(2000);
  xStepper.setAcceleration(2000);
  yStepper.setMaxSpeed(2000);
  yStepper.setAcceleration(2000);
  zStepper.setMaxSpeed(2000);
  zStepper.setAcceleration(2000);
}

void loop() {
  if(Serial.available() > 0){
    String command = Serial.readStringUntil('\n');
    parseCommand(command);
  }

  while(xStepper.distanceToGo() != 0) xStepper.run();
  while(yStepper.distanceToGo() != 0) yStepper.run();
  while(zStepper.distanceToGo() != 0) zStepper.run();
}

void simpleBackAndForth(){
  Serial.println("Moving forward 1600 steps...");
  xStepper.move(16000);
  yStepper.move(8000);
  zStepper.move(4000);
  while(xStepper.distanceToGo() != 0){
    xStepper.run();
    yStepper.run();
    zStepper.run();
  }
  delay(100);
  
  Serial.println("Moving backward 1600 steps...");
  xStepper.move(-16000);
  yStepper.move(-8000);
  zStepper.move(-4000);
  while(xStepper.distanceToGo() != 0){
    xStepper.run();
    yStepper.run();
    zStepper.run();
  }
  delay(100);   
}

void parseCommand(String command){
  // Should add error handling with try catch later (apparently the bare Arduino environment leaves this out for space reasons)

  Serial.print("Command received: ");
  Serial.println(command);
  command.trim();

  if(command.length() < 2){
    Serial.println("Invalid format");
    return;
  }

  char motor = toupper(command.charAt(0));

  String stepsString = command.substring(1);
  stepsString.trim();

  if(!isNumeric(stepsString)){
    Serial.println("Invalid steps, insert a number");
    return;
  }

  int steps = stepsString.toInt();

  Serial.print("Moving motor ");
  Serial.print(motor);
  Serial.print(", steps ");
  Serial.println(steps);

  switch(motor){
    case 'X':
      xStepper.move(steps);
      break;
    case 'Y':
      yStepper.move(steps);
      break;
    case 'Z':
      zStepper.move(steps);
      break;
    default:
      Serial.println("Invalid motor identifier! Use X, Y, or Z");
      return;
  }
}

bool isNumeric(String str) {
  for (byte i = 0; i < str.length(); i++) {
    if (!isDigit(str.charAt(i)) && !(i == 0 && str.charAt(i) == '-')) {
      return false;
    }
  }
  return true;
}