#include <Servo.h>

// Motor 1 (X-axis)
#define X_STEP_PIN 3
#define X_DIR_PIN 4
#define X_SWITCH_PIN 12

// Motor 2 (Y-axis)
#define Y_STEP_PIN 5
#define Y_DIR_PIN 6
#define Y_SWITCH_PIN 11

// Servo
#define SERVO_PIN 9
Servo myServo;

// Motor state
int previousTargetX = 0, previousTargetY = 0;
int targetCountX = 0, targetCountY = 0;
int pressCountX = 0, pressCountY = 0;
bool lastSwitchStateX = LOW, lastSwitchStateY = LOW;
bool runningX = false, runningY = false;

// Command queue
String queuedCommand = "";
bool commandQueued = false;
bool servoMovedTo90 = false;
bool waitingForServoReturn = false;

// Homing/Away state
bool homingX = false;
bool homingY = false;
bool awayX = false;
bool awayY = false;

void setup() {
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(X_SWITCH_PIN, INPUT);

  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  pinMode(Y_SWITCH_PIN, INPUT);

  myServo.attach(SERVO_PIN);
  myServo.write(0); // Initial position

  Serial.begin(9600);
  Serial.println("Enter command: MOVE A 1 B 3, HOMEA, HOME1, AWAYA, AWAY1");
}

void loop() {
  // Handle HOMING (reverse) and AWAY (forward) continuous moves
  if (homingX || awayX) {
    digitalWrite(X_DIR_PIN, homingX ? LOW : HIGH);
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(500);

    if (Serial.available()) {
      char c = Serial.read();
      if (c == '0') {
        homingX = false;
        awayX = false;
        Serial.println("X continuous move stopped.");
      }
    }
    return;
  }

  if (homingY || awayY) {
    digitalWrite(Y_DIR_PIN, homingY ? LOW : HIGH);
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(500);

    if (Serial.available()) {
      char c = Serial.read();
      if (c == '0') {
        homingY = false;
        awayY = false;
        Serial.println("Y continuous move stopped.");
      }
    }
    return;
  }

  // Step motors
  motorStep();

  // After first move
  if (!runningX && !runningY && commandQueued && !servoMovedTo90) {
    Serial.println("First movement done. Moving servo to 90...");
    myServo.write(90);
    delay(500);
    servoMovedTo90 = true;
    processCommand(queuedCommand);
    queuedCommand = "";
    commandQueued = false;
    waitingForServoReturn = true;
  }

  // After second move
  if (!runningX && !runningY && waitingForServoReturn) {
    Serial.println("Second movement done. Moving servo to 0...");
    myServo.write(0);
    delay(500);
    waitingForServoReturn = false;
    servoMovedTo90 = false;
  }

  // Handle input
  if (!runningX && !runningY && Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input == "HOMEA") {
      Serial.println("Starting HOMING for X axis (reverse). Send '0' to stop.");
      homingX = true;
      return;
    }

    if (input == "HOME1") {
      Serial.println("Starting HOMING for Y axis (reverse). Send '0' to stop.");
      homingY = true;
      return;
    }

    if (input == "AWAYA") {
      Serial.println("Starting AWAY for X axis (forward). Send '0' to stop.");
      awayX = true;
      return;
    }

    if (input == "AWAY1") {
      Serial.println("Starting AWAY for Y axis (forward). Send '0' to stop.");
      awayY = true;
      return;
    }

    if (input.startsWith("MOVE")) {
      input.remove(0, 4);
      input.trim();

      int idx = input.indexOf(' ');
      String first = input.substring(0, idx + 2);
      String rest = input.substring(idx + 2);
      rest.trim();

      processCommand(first);

      if (rest.length() > 0) {
        queuedCommand = rest;
        commandQueued = true;
      }
    }
  }
}

void motorStep() {
  if (runningX) {
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(500);

    bool currentSwitchStateX = digitalRead(X_SWITCH_PIN);
    if (currentSwitchStateX == HIGH && lastSwitchStateX == LOW) {
      pressCountX++;
      Serial.print("X press count: "); Serial.println(pressCountX);
    }
    lastSwitchStateX = currentSwitchStateX;

    if (pressCountX >= targetCountX) {
      runningX = false;
      Serial.println("X motor stopped.");
    }
  }

  if (runningY) {
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(500);

    bool currentSwitchStateY = digitalRead(Y_SWITCH_PIN);
    if (currentSwitchStateY == HIGH && lastSwitchStateY == LOW) {
      pressCountY++;
      Serial.print("Y press count: "); Serial.println(pressCountY);
    }
    lastSwitchStateY = currentSwitchStateY;

    if (pressCountY >= targetCountY) {
      runningY = false;
      Serial.println("Y motor stopped.");
    }
  }
}

void processCommand(String command) {
  command.trim();
  if (command.length() < 3) return;

  char inputChar = toupper(command.charAt(0));
  int inputValue = command.substring(2).toInt();

  int targetX = inputChar - 'A';  // A = 0
  int targetY = inputValue - 1;   // 1 = 0

  int deltaX = targetX - previousTargetX;
  int deltaY = targetY - previousTargetY;
  previousTargetX = targetX;
  previousTargetY = targetY;

  if (deltaX != 0 || deltaY != 0) {
    pressCountX = 0;
    pressCountY = 0;

    digitalWrite(X_DIR_PIN, (deltaX >= 0) ? HIGH : LOW);
    digitalWrite(Y_DIR_PIN, (deltaY >= 0) ? HIGH : LOW);

    targetCountX = abs(deltaX);
    targetCountY = abs(deltaY);

    runningX = targetCountX > 0;
    runningY = targetCountY > 0;

    Serial.print("X delta: "); Serial.println(deltaX);
    Serial.print("Y delta: "); Serial.println(deltaY);
  }
}