#include <Servo.h>

Servo servo1;
Servo servo2;

int x_pin = A1;
int y_pin = A0;
int button = A2;

int x_pos;
int y_pos;
int servo1_pin = 8;
int servo2_pin = 9;

int servo1_initialposition = 90;
int servo2_initialposition = 90;

int left_led_pin = 4;
int right_led_pin = 5;
int center_led_pin = 6;  

bool leftBlinking = false;
bool rightBlinking = false;
bool state = false;
unsigned long previousMillisLeft = 0;
unsigned long previousMillisRight = 0;
const long interval = 250;

// Define the dead zone around the center of the joystick
const int deadZoneLower = 450;
const int deadZoneUpper = 550;

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

  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);

  servo1.write(servo1_initialposition);
  servo2.write(servo2_initialposition);

  pinMode(x_pin, INPUT);
  pinMode(y_pin, INPUT);
  pinMode(button, INPUT_PULLUP);

  pinMode(left_led_pin, OUTPUT);
  pinMode(right_led_pin, OUTPUT);
  pinMode(center_led_pin, OUTPUT);
}

void loop() {
  x_pos = analogRead(x_pin);
  y_pos = analogRead(y_pin);
  bool selectPressed = (digitalRead(button) == LOW);

  if (selectPressed) {
    state = !state;
    digitalWrite(center_led_pin, HIGH);
    delay(200);
  }

  if (state) {
    // If joystick is in the center, stop blinking LEDs and turn on center LED
    if (x_pos > 300 && x_pos < 700 && y_pos > 300 && y_pos < 700) {
      leftBlinking = false;
      rightBlinking = false;
      digitalWrite(center_led_pin, HIGH);
    }
    blinkLED(left_led_pin, right_led_pin);
  } else {
    digitalWrite(left_led_pin, LOW);
    digitalWrite(right_led_pin, LOW);
    digitalWrite(center_led_pin, LOW);
  }

  // Check for joystick position and move servos
  if (x_pos <= 300 && servo1_initialposition >= 10) {
    servo1_initialposition -= 90;
    servo1.write(servo1_initialposition);
    delay(100);
    digitalWrite(center_led_pin, LOW);
    leftBlinking = true;
    rightBlinking = false;
  } else if (x_pos >= 700 && servo1_initialposition <= 170) {
    servo1_initialposition += 90;
    servo1.write(servo1_initialposition);
    delay(100);
    digitalWrite(center_led_pin, LOW);
    rightBlinking = true;
    leftBlinking = false;
  }

  if (y_pos <= 300 && servo2_initialposition >= 10) {
    servo2_initialposition -= 90;
    servo2.write(servo2_initialposition);
    delay(100);
    digitalWrite(center_led_pin, LOW);
    rightBlinking = false;
    leftBlinking = true;
  } else if (y_pos >= 700 && servo2_initialposition <= 170) {
    servo2_initialposition += 90;
    servo2.write(servo2_initialposition);
    delay(100);
    digitalWrite(center_led_pin, LOW);
    leftBlinking = false;
    rightBlinking = true;
  }

  // **Return Servos to Position 90 When Joystick is in the Dead Zone**
  if (x_pos > deadZoneLower && x_pos < deadZoneUpper) {
    servo1.write(90);  // Reset servo1 to position 90
    servo1_initialposition = 90;
  }

  if (y_pos > deadZoneLower && y_pos < deadZoneUpper) {
    servo2.write(90);  // Reset servo2 to position 90
    servo2_initialposition = 90;
  }
}

void blinkLED(int leftLED, int rightLED) {
  unsigned long currentMillis = millis();

  if (leftBlinking) {
    if (currentMillis - previousMillisLeft >= interval) {
      previousMillisLeft = currentMillis;
      digitalWrite(leftLED, !digitalRead(leftLED));
    }
  } else {
    digitalWrite(leftLED, LOW);
  }

  if (rightBlinking) {
    if (currentMillis - previousMillisRight >= interval) {
      previousMillisRight = currentMillis;
      digitalWrite(rightLED, !digitalRead(rightLED));
    }
  } else {
    digitalWrite(rightLED, LOW);
  }
}