#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <NewPing.h>

const int trigPin1 = 7;
const int echoPin1 = 6;
const int redPin1 = 23;
const int yellowPin1 = 25;
const int greenPin1 = 27;
Servo servo1;
int lastRotatedPin1 = -1;
bool greenLightOn1 = false;  // Flag for green light on Servo 1

const int trigPin2 = 5;
const int echoPin2 = 4;
const int redPin2 = 29;
const int yellowPin2 = 31;
const int greenPin2 = 33;
Servo servo2;
int lastRotatedPin2 = -1;
bool greenLightOn2 = false;  // Flag for green light on Servo 2

const int trigPin3 = 3;
const int echoPin3 = 2;
const int redPin3 = 35;
const int yellowPin3 = 37;
const int greenPin3 = 39;
Servo servo3;
int lastRotatedPin3 = -1;
bool greenLightOn3 = false;  // Flag for green light on Servo 3

LiquidCrystal_I2C lcd(0x27, 16, 2);

NewPing sonar1(trigPin1, echoPin1);
NewPing sonar2(trigPin2, echoPin2);
NewPing sonar3(trigPin3, echoPin3);

void configureSystem() {
  // Setup for Servo 1
  servo1.attach(13);
  servo1.write(0);

  // Setup for Servo 2
  servo2.attach(12);
  servo2.write(0);

  // Setup for Servo 3
  servo3.attach(11);
  servo3.write(0);

  // Setup for LCD
  lcd.begin(16, 2);
  lcd.print("System Ready");
  delay(2000);
  lcd.clear();
}

void controlLEDs(int redPin, int yellowPin, int greenPin, int distance, int &lastRotatedPin, Servo &servo, bool &greenLightOn) {
  if (distance > 5) {
    // Red light
    digitalWrite(redPin, HIGH);
    digitalWrite(yellowPin, LOW);
    digitalWrite(greenPin, LOW);
    delay(2000);

    // Move servo to position 0 degrees (red light)
    if (lastRotatedPin != redPin) {
      servo.write(0);
      lastRotatedPin = redPin;
      greenLightOn = false;  // Reset the flag when red light is on
    }
  } else {
    // Yellow light
    digitalWrite(redPin, LOW);
    
    if (!greenLightOn) {
      digitalWrite(yellowPin, HIGH);

      delay(5000);  // Delay for 5 seconds when yellow light will transition to green (adjust as needed)

      // Turn off yellow LED and turn on green LED
      digitalWrite(yellowPin, LOW);
      digitalWrite(greenPin, HIGH);

      // Move servo to position 90 degrees (green light)
      if (lastRotatedPin != greenPin) {
        servo.write(90);
        lastRotatedPin = greenPin;
        greenLightOn = true;  // Set the flag to indicate that the green light is on
      }
    } else {
      // Green light continues to stay on
      digitalWrite(redPin, LOW);
      digitalWrite(yellowPin, LOW);
      digitalWrite(greenPin, HIGH);
      servo.write(90);
    }
  }
}


void checkAndControlSensor(NewPing &sonar, int redPin, int yellowPin, int greenPin, int &lastRotatedPin, Servo &servo, bool &greenLightOn) {
  delay(50); // Delay untuk menghindari pembacaan yang terlalu cepat
  int distance = sonar.ping_cm();

  controlLEDs(redPin, yellowPin, greenPin, distance, lastRotatedPin, servo, greenLightOn);

  // Tambahkan logika lain yang Anda inginkan di sini
}

void setup() {
  configureSystem();
}

void loop() {
  checkAndControlSensor(sonar1, redPin1, yellowPin1, greenPin1, lastRotatedPin1, servo1, greenLightOn1);
  checkAndControlSensor(sonar2, redPin2, yellowPin2, greenPin2, lastRotatedPin2, servo2, greenLightOn2);
  checkAndControlSensor(sonar3, redPin3, yellowPin3, greenPin3, lastRotatedPin3, servo3, greenLightOn3);

  // Tambahan logika lain yang Anda inginkan di sini
}