#include <Servo.h>

Servo servo1;  // Servo 1 connected to pin 2 (horizontal movement)
Servo servo2;  // Servo 2 connected to pin 3 (vertical movement)

int servo1Pin = 2;  // Pin for Servo 1
int servo2Pin = 3;  // Pin for Servo 2

int vertPin = A0;  // VERT connected to A0 (Joystick vertical movement)
int horzPin = A1;  // HORZ connected to A1 (Joystick horizontal movement)
int selPin = A2;   // SEL connected to A2 (Joystick button for reset)

int pos1 = 90;  // Initial position for Servo 1 (90 degrees)
int pos2 = 90;  // Initial position for Servo 2 (90 degrees)

int deadzone = 10;  // Deadzone to prevent jitter in joystick movement
int step = 1;       // Servo movement step size

unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 200;  // 200 ms debounce delay for SEL button

void setup() {
  // Attach the servos to the corresponding pins
  servo1.attach(servo1Pin);  // Attach Servo 1 to pin 2
  servo2.attach(servo2Pin);  // Attach Servo 2 to pin 3
  
  // Set servos to start at 90 degrees (neutral position)
  servo1.write(pos1);
  servo2.write(pos2);
  
  // Joystick axis inputs
  pinMode(vertPin, INPUT);  // Vertical axis
  pinMode(horzPin, INPUT);  // Horizontal axis
  
  // SEL button input with internal pull-up resistor
  pinMode(selPin, INPUT_PULLUP);  // Enable internal pull-up resistor on SEL pin

  // Start serial communication for debugging
  Serial.begin(9600);  
}

void loop() {
  int vertValue = analogRead(vertPin);  // Read vertical joystick value (A0)
  int horzValue = analogRead(horzPin);  // Read horizontal joystick value (A1)

  // Check if SEL button is pressed (LOW due to pull-up resistor)
  if (digitalRead(selPin) == LOW && (millis() - lastDebounceTime) > debounceDelay) {
    // Reset both servos to 90 degrees when SEL button is pressed
    pos1 = 90;
    pos2 = 90;
    servo1.write(pos1);
    servo2.write(pos2);
    Serial.println("SEL pressed: Resetting both servos to 90 degrees");
    lastDebounceTime = millis();  // Update debounce time
  }

  // Move Servo 1 (left/right) based on horizontal joystick movement
  if (horzValue < 512 - deadzone) {
    if (pos1 < 180) {  // Move right (increase angle)
      pos1 += step;  
      servo1.write(pos1);
      Serial.print("Servo 1 moving right to: ");
      Serial.println(pos1);
    }
  } else if (horzValue > 512 + deadzone) {
    if (pos1 > 0) {  // Move left (decrease angle)
      pos1 -= step;  
      servo1.write(pos1);
      Serial.print("Servo 1 moving left to: ");
      Serial.println(pos1);
    }
  }
  
  // Move Servo 2 (up/down) based on vertical joystick movement
  if (vertValue < 512 - deadzone) {
    if (pos2 < 180) {  // Move down (increase angle)
      pos2 += step;  
      servo2.write(pos2);
      Serial.print("Servo 2 moving down to: ");
      Serial.println(pos2);
    }
  } else if (vertValue > 512 + deadzone) {
    if (pos2 > 0) {  // Move up (decrease angle)
      pos2 -= step;  
      servo2.write(pos2);
      Serial.print("Servo 2 moving up to: ");
      Serial.println(pos2);
    }
  }
  
  delay(15);  // Small delay to smooth the movement
}