#include <AFMotor.h>

AF_DCMotor motor1(1); // Motor 1 connected to M1 on L298N
AF_DCMotor motor2(2); // Motor 2 connected to M2 on L298N

int joyX;
int joyY;
int joyXPrev = 512;
int joyYPrev = 512;

const int limitSwitchUp = A2;    // Pin for the up limit switch (analog A2)
const int limitSwitchDown = A3;  // Pin for the down limit switch (analog A3)
const int limitSwitchLeft = A4;  // Pin for the left limit switch (analog A4)
const int limitSwitchRight = A5; // Pin for the right limit switch (analog A5)

void setup() {
  pinMode(limitSwitchUp, INPUT_PULLUP);
  pinMode(limitSwitchDown, INPUT_PULLUP);
  pinMode(limitSwitchLeft, INPUT_PULLUP);
  pinMode(limitSwitchRight, INPUT_PULLUP);
}

void loop() {
  joyX = analogRead(A0); // Read the X-axis value of the joystick
  joyY = analogRead(A1); // Read the Y-axis value of the joystick

  // Map joystick values to motor speeds (adjust as needed)
  int motorSpeed1 = map(joyX, 0, 1023, -255, 255);
  int motorSpeed2 = map(joyY, 0, 1023, -255, 255);

  // Check if the joystick values have changed significantly to reduce motor jitter
  if (abs(joyX - joyXPrev) > 5) {
    if (motorSpeed1 >= 0 && !digitalRead(limitSwitchLeft)) {
      motor1.setSpeed(abs(motorSpeed1));
      motor1.run(FORWARD);
    } else if (motorSpeed1 < 0 && !digitalRead(limitSwitchRight)) {
      motor1.setSpeed(abs(motorSpeed1));
      motor1.run(BACKWARD);
    } else {
      motor1.setSpeed(0);
      motor1.run(RELEASE);
    }
    joyXPrev = joyX;
  }

  if (abs(joyY - joyYPrev) > 5) {
    if (motorSpeed2 >= 0 && !digitalRead(limitSwitchUp)) {
      motor2.setSpeed(abs(motorSpeed2));
      motor2.run(FORWARD);
    } else if (motorSpeed2 < 0 && !digitalRead(limitSwitchDown)) {
      motor2.setSpeed(abs(motorSpeed2));
      motor2.run(BACKWARD);
    } else {
      motor2.setSpeed(0);
      motor2.run(RELEASE);
    }
    joyYPrev = joyY;
  }

  delay(15); // Add a small delay to stabilize motor movement
}