#include <AccelStepper.h>

#define motorInterfaceType 1

// Stepper motor pins using CNC Shield
AccelStepper stepper1(motorInterfaceType, 2, 5); // X Axis (left/right)
AccelStepper stepper2(motorInterfaceType, 3, 6); // Y Axis (forward/backward)

// Joystick pins
const int joystickXPin = A0;
const int joystickYPin = A1;
const int joystickButtonPin = A2;

// Joystick settings
const int deadZone = 50;
const int joystickCenter = 512;
const int maxSpeed = 1000;

// Step size per button press
const int moveChunk = 200;  // Move 1 revolution per press (adjust as needed)

// Button debounce
unsigned long lastButtonPress = 0;
const unsigned long debounceDelay = 300;

// Mode
enum Mode { MANUAL, MOVING };
Mode currentMode = MANUAL;

// Flag to toggle movement direction
bool moveForward = true;

void setup() {
  Serial.begin(9600);
  pinMode(joystickButtonPin, INPUT_PULLUP); // Joystick button

  stepper1.setMaxSpeed(maxSpeed);
  stepper1.setAcceleration(500);

  stepper2.setMaxSpeed(maxSpeed);
  stepper2.setAcceleration(500);
}

void loop() {
  unsigned long now = millis();
  static bool buttonPreviouslyPressed = false;
  bool buttonPressed = (digitalRead(joystickButtonPin) == LOW);

  // Handle button press to move motors by fixed step (forward/backward)
  if (buttonPressed && !buttonPreviouslyPressed && (now - lastButtonPress > debounceDelay)) {
    lastButtonPress = now;

    // Toggle direction: move forward or backward
    if (moveForward) {
      stepper1.move(moveChunk);  // Move forward
      stepper2.move(moveChunk);  // Move forward
      Serial.println("Moving both motors forward.");
    } else {
      stepper1.move(-moveChunk);  // Move backward
      stepper2.move(-moveChunk);  // Move backward
      Serial.println("Moving both motors backward.");
    }

    // Toggle the direction for next button press
    moveForward = !moveForward;

    currentMode = MOVING;
  }
  buttonPreviouslyPressed = buttonPressed;

  // Manual joystick control
  if (currentMode == MANUAL) {
    int xVal = analogRead(joystickXPin);
    int yVal = analogRead(joystickYPin);

    int xOffset = xVal - joystickCenter;
    int yOffset = yVal - joystickCenter;

    if (abs(xOffset) > deadZone) {
      stepper1.setSpeed(map(xOffset, -512, 511, -maxSpeed, maxSpeed));
    } else {
      stepper1.setSpeed(0);
    }

    if (abs(yOffset) > deadZone) {
      stepper2.setSpeed(map(yOffset, -512, 511, maxSpeed / 5, -maxSpeed / 5));
    } else {
      stepper2.setSpeed(0);
    }

    stepper1.runSpeed();
    stepper2.runSpeed();
  }

  // Run motors toward their target position (moving forward or backward)
  if (currentMode == MOVING) {
    stepper1.run();
    stepper2.run();

    // If motors reach the target position, return to manual mode
    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
      currentMode = MANUAL;
      Serial.println("Movement complete. Returning to manual mode.");
    }
  }
}