#include <Servo.h>

// Pin setup for LEDs (Turn indicators)
int leftLED = 2;   // Left turn LED
int rightLED = 3;  // Right turn LED
int frontLED = 4;  // Front indicator LED

// Joystick pins
int joyX = A0;     // Joystick X-axis
int joyY = A1;     // Joystick Y-axis

// Servo Motors for steering and movement
Servo leftServo;   // Steering left
Servo rightServo;  // Steering right
Servo forwardServo; // Forward movement
Servo backwardServo; // Backward movement

int leftServoPin = 9;
int rightServoPin = 10;
int forwardServoPin = 11;
int backwardServoPin = 12;

// Neutral positions for servos
int neutralTurnAngle = 90;    // Neutral (center) position for left and right servos
int neutralMoveAngle = 0;     // Neutral (stop) position for forward and backward servos

void setup() {
  // Set LED pins as output
  pinMode(leftLED, OUTPUT);
  pinMode(rightLED, OUTPUT);
  pinMode(frontLED, OUTPUT);

  // Set Joystick pins as input
  pinMode(joyX, INPUT);
  pinMode(joyY, INPUT);

  // Servo initialization
  leftServo.attach(leftServoPin);
  rightServo.attach(rightServoPin);
  forwardServo.attach(forwardServoPin);
  backwardServo.attach(backwardServoPin);

  // Set servos to neutral positions initially
  resetServos();
}

void loop() {
  // Read joystick values
  int xValue = analogRead(joyX);
  int yValue = analogRead(joyY);

  // Left turn: left servo turns and left LED blinks
  if (xValue < 400) {
    leftServo.write(45); // Adjust to the desired left turn angle
    digitalWrite(rightLED, LOW);
    blinkLED(leftLED);
  }
  // Right turn: right servo turns and right LED blinks
  else if (xValue > 600) {
    rightServo.write(135); // Adjust to the desired right turn angle
    digitalWrite(leftLED, LOW);
    blinkLED(rightLED);
  }
  else {
    // Reset steering servos when the joystick is in the neutral X position
    leftServo.write(neutralTurnAngle);
    rightServo.write(neutralTurnAngle);
    digitalWrite(leftLED, LOW);
    digitalWrite(rightLED, LOW);
  }

  // Forward movement: forward servo turns and front LED blinks
  if (yValue > 600) {
    forwardServo.write(90);  // Adjust for forward movement
    backwardServo.write(0);  // Stop backward movement
    blinkLED(frontLED);
  }
  // Backward movement: backward servo turns and both left and right LEDs blink
  else if (yValue < 400) {
    forwardServo.write(0);   // Stop forward movement
    backwardServo.write(90); // Adjust for backward movement
    blinkBothLEDs(leftLED, rightLED);
  }
  else {
    // Reset movement servos when the joystick is in the neutral Y position
    forwardServo.write(neutralMoveAngle);
    backwardServo.write(neutralMoveAngle);
    digitalWrite(frontLED, LOW);
  }

  delay(50);  // Small delay for stability
}

// Function to blink a single LED
void blinkLED(int ledPin) {
  digitalWrite(ledPin, HIGH);
  delay(300);  // Reduced delay for faster blinking
  digitalWrite(ledPin, LOW);
  delay(150);
}

// Function to blink both LEDs together
void blinkBothLEDs(int leftPin, int rightPin) {
  digitalWrite(leftPin, HIGH);
  digitalWrite(rightPin, HIGH);
  delay(150);
  digitalWrite(leftPin, LOW);
  digitalWrite(rightPin, LOW);
  delay(150);
}

// Function to reset all servos to neutral positions
void resetServos() {
  leftServo.write(neutralTurnAngle);  // Reset left servo to neutral (center) position
  rightServo.write(neutralTurnAngle); // Reset right servo to neutral (center) position
  forwardServo.write(neutralMoveAngle); // Reset forward servo to neutral (stop) position
  backwardServo.write(neutralMoveAngle); // Reset backward servo to neutral (stop) position
}