#include <AccelStepper.h>

// Pin Definitions
#define X_STEP_PIN 2    // X-axis step pin
#define X_DIR_PIN 3     // X-axis direction pin
#define X_ENABLE_PIN 4  // X-axis enable pin
#define X_LED_PIN 10    // X-axis LED pin

#define Y_STEP_PIN 5    // Y-axis step pin
#define Y_DIR_PIN 6     // Y-axis direction pin
#define Y_ENABLE_PIN 7  // Y-axis enable pin
#define Y_LED_PIN 11    // Y-axis LED pin

#define VERT_PIN A0     // Vertical joystick pin
#define HORZ_PIN A1     // Horizontal joystick pin
#define SEL_PIN 8       // Joystick select pin
#define RESET_PIN 9     // Reset button pin
#define RES_PIN 12     // Reset button pin

// Joystick Thresholds
#define JOYSTICK_THRESHOLD 512  // Adjust this value to set the joystick threshold

// Motor Parameters
#define X_MAX_SPEED 1000  // Maximum speed for X-axis motor
#define X_ACCELERATION 1000  // Acceleration for X-axis motor
#define Y_MAX_SPEED 1000  // Maximum speed for Y-axis motor
#define Y_ACCELERATION 1000  // Acceleration for Y-axis motor

// Create stepper motor objects
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);

bool isCenterPressed = false;
bool isResetPressed = false;

void setup() {
  // Set up pins
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(RESET_PIN, INPUT_PULLUP);
  pinMode(X_LED_PIN, OUTPUT);
  pinMode(Y_LED_PIN, OUTPUT);

  // Set up motor properties
  stepperX.setMaxSpeed(X_MAX_SPEED);
  stepperX.setAcceleration(X_ACCELERATION);

  stepperY.setMaxSpeed(Y_MAX_SPEED);
  stepperY.setAcceleration(Y_ACCELERATION);
}

void loop() {
  // Read joystick and reset button values
  int vertValue = analogRead(VERT_PIN);
  int horzValue = analogRead(HORZ_PIN);
  bool selState = digitalRead(SEL_PIN);
  bool resetState = digitalRead(RESET_PIN);

  // Map joystick values to motor speed and direction
  int xSpeed = map(horzValue, 0, 1023, -X_MAX_SPEED, X_MAX_SPEED);
  int ySpeed = map(vertValue, 0, 1023, -Y_MAX_SPEED, Y_MAX_SPEED);

  // Set motor direction based on joystick position
  if (abs(xSpeed) > JOYSTICK_THRESHOLD) {
    stepperX.setSpeed(xSpeed);
    digitalWrite(X_LED_PIN, HIGH);  // Turn on X-axis LED
  } else {
    stepperX.setSpeed(0);
    digitalWrite(X_LED_PIN, LOW);  // Turn off X-axis LED
  }

  if (abs(ySpeed) > JOYSTICK_THRESHOLD) {
    stepperY.setSpeed(ySpeed);
    digitalWrite(Y_LED_PIN, HIGH);  // Turn on Y-axis LED
  } else {
    stepperY.setSpeed(0);
    digitalWrite(Y_LED_PIN, LOW);  // Turn off Y-axis LED
  }

  // Move motors
  stepperX.runSpeed();
  stepperY.runSpeed();

  // Check if the joystick center is pressed
  if (selState == LOW && !isCenterPressed) {
    isCenterPressed = true;
    // Move motors to position "0"
    stepperX.moveTo(0);
    stepperY.moveTo(0);
  } else if (selState == HIGH && isCenterPressed) {
    isCenterPressed = false;
  }

  // Check if the reset button is pressed
  if (resetState == LOW && !isResetPressed) {
    isResetPressed = true;
    // Reset the motor positions to "0"
    stepperX.setCurrentPosition(0);
    stepperY.setCurrentPosition(0);
  } else if (resetState == HIGH && isResetPressed) {
    isResetPressed = false;
  }

  // Check if the motors have reached the target position
  if (stepperX.distanceToGo() == 0 && stepperY.distanceToGo() == 0) {
    // Motors have reached the target position
    // Perform any necessary actions or stop the motors
    // Add your code here
  }

  // Corrected logic for moving the Y-axis motor with the reset button
  if (digitalRead(RESET_PIN) == LOW) {
    digitalWrite(Y_DIR_PIN, LOW);
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(1000);
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(1000);
         
  }
  if (digitalRead(RES_PIN) == LOW) {
    digitalWrite(X_DIR_PIN, LOW);
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(1000);
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(1000);
         
  }
}