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

const int trigPin1 = 41;
const int echoPin1 = 43;
const int redPin1 = 6;
const int yellowPin1 = 5;
const int greenPin1 = 4;
Servo servo1;
int lastRotatedPin1 = -1;
bool greenLightOn1 = false;  // Flag for green light on Servo 1

const int trigPin2 = 47;
const int echoPin2 = 49;
const int redPin2 = 25;
const int yellowPin2 = 27;
const int greenPin2 = 29;
Servo servo2;
int lastRotatedPin2 = -1;
bool greenLightOn2 = false;  // Flag for green light on Servo 2

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

const int trigPin4 = 23;
const int echoPin4 = 26;

LiquidCrystal_I2C lcd(0x27, 16, 2);

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

char expectedPaymentCode[] = "555555"; 

int checkPassword(char *inputPassword, char *correctPassword) {
  return strcmp(inputPassword, correctPassword) == 0;
}

void configureSystem();
void checkAndControlSensor(NewPing &sonar, int redPin, int yellowPin, int greenPin, int &lastRotatedPin, Servo &servo, bool &greenLightOn);
int getDistance(int trigPin, int echoPin);
bool handlePasswordInput();
void controlLEDs(int redPin, int yellowPin, int greenPin, int distance, int &lastRotatedPin, Servo &servo);

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

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(redPin1, OUTPUT);
  pinMode(yellowPin1, OUTPUT);
  pinMode(greenPin1, OUTPUT);

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(redPin2, OUTPUT);
  pinMode(yellowPin2, OUTPUT);
  pinMode(greenPin2, OUTPUT);

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(redPin3, OUTPUT);
  pinMode(yellowPin3, OUTPUT);
  pinMode(greenPin3, OUTPUT);

  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);

  lastRotatedPin3 = -1;
  lcd.backlight();
}

void handleTransactionCode() {
  char password[20];
  char correctPassword[] = "555555";

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Enter Code :");

  while (Serial.available() == 0) {
    // Wait until input is available
  }

  int charCount = 0;
  while (true) {
    if (Serial.available() > 0) {
      char inputChar = Serial.read();

      if (inputChar == '\n') {
        password[charCount] = '\0';
        break;
      } else {
        password[charCount] = inputChar;
        charCount++;

        if (charCount >= sizeof(password) - 1) {
          Serial.println("Payment code length exceeded!");
          return;
        }
      }
    }
  }

  if (checkPassword(password, correctPassword)) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Access granted");
    delay(2000);
    lcd.clear();
    return;
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Access denied");
    delay(2000);
    lcd.clear();
    return;
  }
}


void loop() {
  int distance4 = getDistance(trigPin4, echoPin4);

  // Display distance from ultrasonic sensor 4 to the terminal
  Serial.print("Your Distance: ");
  Serial.print(distance4);
  Serial.println(" cm");

  Serial.println("Enter payment code:");
  
  handleTransactionCode();  // Handle the transaction code input

  int distance1 = getDistance(trigPin1, echoPin1);
  int distance2 = getDistance(trigPin2, echoPin2);
  int distance3 = getDistance(trigPin3, echoPin3);

  checkAndControlSensor(sonar1, redPin1, yellowPin1, greenPin1, lastRotatedPin1, servo1, greenLightOn1);
  checkAndControlSensor(sonar2, redPin2, yellowPin2, greenPin2, lastRotatedPin2, servo2, greenLightOn2);
  checkAndControlSensor(sonar3, redPin3, yellowPin3, greenPin3, lastRotatedPin3, servo3, greenLightOn3);
  lcd.backlight();
  }


int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;

  return distance;
}

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 configureSystem() {
  servo1.attach(13);
  servo1.write(0);

  servo2.attach(12);
  servo2.write(0);

  servo3.attach(11);
  servo3.write(0);

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