#include <AccelStepper.h>

#define MOTOR_INTERFACE_TYPE 1

// Motor pins
#define STEP_PIN_1 13
#define DIR_PIN_1 12
#define STEP_PIN_2 21
#define DIR_PIN_2 20

// Joystick pins
#define JOYSTICK_X_PIN 5
#define JOYSTICK_Y_PIN 4

AccelStepper stepper1(MOTOR_INTERFACE_TYPE, STEP_PIN_1, DIR_PIN_1);
AccelStepper stepper2(MOTOR_INTERFACE_TYPE, STEP_PIN_2, DIR_PIN_2);

void setup() {
  Serial.begin(115200);

  // Initialize stepper motors
  stepper1.setMaxSpeed(4000);
  stepper1.setAcceleration(1000); // Default acceleration, will be adjusted dynamically

  stepper2.setMaxSpeed(4000);
  stepper2.setAcceleration(1000); // Default acceleration, will be adjusted dynamically
}

void loop() {
  // Read joystick values
  int joystickX = analogRead(JOYSTICK_X_PIN);
  int joystickY = analogRead(JOYSTICK_Y_PIN);

  // Control motors based on joystick values
  controlMotor(stepper1, joystickX);
  controlMotor(stepper2, joystickY);

  // Run the motors
  stepper1.run();
  stepper2.run();

  delay(10); // Small delay to avoid overloading the loop
}

void controlMotor(AccelStepper &stepper, int joystickValue) {
  int midpoint = 2048; // Assuming a 12-bit ADC (values range from 0 to 4095)
  int stopZone = 50; // Define a StopZone around the midpoint

  if (joystickValue < midpoint - stopZone) {
    int speed = map(joystickValue, 0, midpoint - stopZone, -4000, 0);
    int acceleration = map(joystickValue, 0, midpoint - stopZone, 4000, 1000); // Dynamic acceleration
    stepper.setSpeed(speed);
    stepper.setAcceleration(acceleration);
  } else if (joystickValue > midpoint + stopZone) {
    int speed = map(joystickValue, midpoint + stopZone, 4095, 0, 4000);
    int acceleration = map(joystickValue, midpoint + stopZone, 4095, 1000, 4000); // Dynamic acceleration
    stepper.setSpeed(speed);
    stepper.setAcceleration(acceleration);
  } else {
    stepper.setSpeed(0); // Stop the motor within the StopZone
  }
}