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

const int trigPin1 = 41;
const int echoPin1 = 43;
const int redPin1 = 6;
const int yellowPin1 = 5;
const int greenPin1 = 4;
Servo servo1;

const int trigPin2 = 47;
const int echoPin2 = 49;
const int redPin2 = 25;
const int yellowPin2 = 27;
const int greenPin2 = 29;
Servo servo2;

const int trigPin3 = 51;
const int echoPin3 = 53;
const int redPin3 = 33;
const int yellowPin3 = 35;
const int greenPin3 = 37;
Servo servo3;

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);

char expectedPaymentCode[] = "555555";
bool servoSelected = false;
bool paymentCompleted = false;
Servo selectedServo;

void configureSystem();
void checkAndControlSensor(NewPing &sonar, int redPin, int yellowPin, int greenPin);
int getDistance(int trigPin, int echoPin);
bool handlePasswordInput(char *correctPassword);
void handleServoSelection();

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

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(redPin1, OUTPUT);
  pinMode(yellowPin1, OUTPUT);
  pinMode(greenPin1, OUTPUT);
  servo1.attach(13); // Attach Servo 1 to pin 13

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(redPin2, OUTPUT);
  pinMode(yellowPin2, OUTPUT);
  pinMode(greenPin2, OUTPUT);
  servo2.attach(12); // Attach Servo 2 to pin 12

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(redPin3, OUTPUT);
  pinMode(yellowPin3, OUTPUT);
  pinMode(greenPin3, OUTPUT);
  servo3.attach(11); // Attach Servo 3 to pin 11

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

  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("System Ready");
  delay(2000);
  lcd.clear();
}

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

  Serial.print("Your Distance: ");
  Serial.print(distance4);
  Serial.println(" cm");

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

  checkAndControlSensor(sonar1, redPin1, yellowPin1, greenPin1);
  checkAndControlSensor(sonar2, redPin2, yellowPin2, greenPin2);
  checkAndControlSensor(sonar3, redPin3, yellowPin3, greenPin3);

  if (!servoSelected)
  {
    handleServoSelection();
  }

  if (!paymentCompleted)
  {
    if (handlePasswordInput(expectedPaymentCode))
    {
      paymentCompleted = true;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Payment Success");
      Serial.print("Payment Success");
      delay(2000);
      lcd.clear();

      // Rotate the selected servo
      selectedServo.write(90); // You may adjust the angle as needed

      // Reset flags for continuous operation
      servoSelected = false;
      paymentCompleted = false;
    }
  }

  // You can add a delay here to control the loop speed
  delay(1000); // Adjust the delay value as needed
}

void handleServoSelection()
{
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Select Servo :");
  Serial.println("Select Servo :");

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

  int selectedServoNumber = Serial.parseInt();

  switch (selectedServoNumber)
  {
  case 1:
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Servo 1 selected");
    Serial.println("Servo 1 selected");
    selectedServo = servo1;
    selectedServo.write(90); // Debug: Rotate to 90 degrees
    delay(2000);
    break;
  case 2:
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Servo 2 selected");
    Serial.println("Servo 2 selected");
    selectedServo = servo2;
    selectedServo.write(90); // Debug: Rotate to 90 degrees
    delay(2000);
    break;
  case 3:
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Servo 3 selected");
    Serial.println("Servo 3 selected");
    selectedServo = servo3;
    selectedServo.write(90); // Debug: Rotate to 90 degrees
    delay(2000);
    break;
  default:
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Invalid Servo");
    Serial.println("Invalid Servo");
    delay(2000);
    break;
  }

  servoSelected = true;
}

bool handlePasswordInput(char *correctPassword)
{
  char password[20];
  int charCount = 0;

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

  while (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 false;
      }
    }
  }

  return strcmp(password, correctPassword) == 0;
}

void checkAndControlSensor(NewPing &sonar, int redPin, int yellowPin, int greenPin)
{
  int distance = sonar.ping_cm();

  if (distance > 5)
  {
    digitalWrite(redPin, HIGH);
    digitalWrite(yellowPin, LOW);
    digitalWrite(greenPin, LOW);
    delay(2000);
  }
  else
  {
    digitalWrite(redPin, LOW);
    digitalWrite(yellowPin, HIGH);

    // Wait for a short duration, but frequently check for other conditions
    for (int i = 0; i < 25; ++i)
    {
      delay(200);
      if (paymentCompleted || !servoSelected)
      {
        break;
      }
    }

    digitalWrite(yellowPin, LOW);
    digitalWrite(greenPin, HIGH);
    delay(2000);
  }
}

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

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

  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}