#include <AccelStepper.h>

// Define stepper motors and pins
AccelStepper stepper1(AccelStepper::DRIVER, 16, 15); // STEP pin 16, DIR pin 15
AccelStepper stepper2(AccelStepper::DRIVER, 14, 13); // STEP pin 14, DIR pin 13

// Joystick pins
const int VERT = 12;
const int HOR = 11;
const int PUSH = 10;

// Variables to store neutral (center) position of the joystick
int neutralX = 0;
int neutralY = 0;
bool calibrated = false;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Set up joystick button pin
  pinMode(PUSH, INPUT_PULLUP);

  // Set max speed and acceleration for stepper motors
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(500);

  // Initial calibration
  calibrateJoystick();
}

void loop() {
  // Check if push button is pressed for calibration
  if (digitalRead(PUSH) == LOW) {
    calibrateJoystick();
  }

  // Read joystick values
  int xVal = analogRead(HOR);
  int yVal = analogRead(VERT);

  // Adjust values based on calibration
  int adjustedX = xVal - neutralX;
  int adjustedY = yVal - neutralY;

  // Map adjusted joystick values to stepper speeds
  long speed1 = map(adjustedX, -2048, 2048, -1000, 1000);
  long speed2 = map(adjustedY, -2048, 2048, -1000, 1000);

  // Set speed for each motor only if there is a significant deviation from neutral
  if (abs(adjustedX) > 50) { // Deadzone threshold
    stepper1.setSpeed(speed1);
  } else {
    stepper1.setSpeed(0);
  }

  if (abs(adjustedY) > 50) { // Deadzone threshold
    stepper2.setSpeed(speed2);
  } else {
    stepper2.setSpeed(0);
  }

  // Move the motors
  stepper1.runSpeed();
  stepper2.runSpeed();

  // Optional: Print joystick and speed values for debugging
  Serial.print("Joystick X: ");
  Serial.print(xVal);
  Serial.print(" Joystick Y: ");
  Serial.print(yVal);
  Serial.print(" Speed1: ");
  Serial.print(speed1);
  Serial.print(" Speed2: ");
  Serial.println(speed2);

  delay(10);
}

void calibrateJoystick() {
  Serial.println("Calibrating joystick...");

  // Wait for the button to be held down for 2 seconds to start calibration
  unsigned long startTime = millis();
  while (digitalRead(PUSH) == LOW) {
    if (millis() - startTime > 2000) {
      break;
    }
  }

  // Read the current joystick values as neutral positions
  neutralX = analogRead(HOR);
  neutralY = analogRead(VERT);

  Serial.print("Neutral X: ");
  Serial.println(neutralX);
  Serial.print("Neutral Y: ");
  Serial.println(neutralY);

  calibrated = true;
}