#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

#define TRIG_PIN 26
#define ECHO_PIN 27
#define RED_LED_PIN 32
#define YELLOW_LED_PIN 33
#define GREEN_LED_PIN 25
#define SERVO_PIN 18

LiquidCrystal_I2C lcd(0x27, 16, 2);  // LCD na naslovu 0x27, 16 stolpcev, 2 vrsti
Servo zapornica;

long duration;
int distance;

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.clear();
  
  zapornica.attach(SERVO_PIN);
  zapornica.write(0);  // Servo na začetni položaj (0 stopinj)
  
  lcd.setCursor(0, 0);
  lcd.print("System Start");
  delay(2000);
}

void loop() {
  // Aktiviraj ultrazvočni senzor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;  // Izračunaj razdaljo v centimetrih
  
  if (distance < 20) {  // Če je predmet preblizu (razdalja manjša od 20 cm)
    // Zelena luč prižgana, LCD izpis "POJDI", servo na 90 stopinj
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    zapornica.write(90);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("POJDI");
  } else if (distance >= 20 && distance < 30) {  // Ko je predmet v območju 20-30 cm
    // Rumena luč prižgana, LCD izpis "Pocakaj", servo na 0 stopinj
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, HIGH);
    digitalWrite(GREEN_LED_PIN, LOW);
    zapornica.write(0);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Pocakaj");
    delay(3000);  // Čakaj 3 sekunde
  } else {  // Ko je predmet dovolj oddaljen (več kot 30 cm)
    // Rdeča luč prižgana, LCD izpis "STOP", servo na 0 stopinj
    digitalWrite(RED_LED_PIN, HIGH);
    digitalWrite(YELLOW_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, LOW);
    zapornica.write(0);  // Servo na 0 stopinj
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("STOP");
  }

  delay(100);  // Počakaj nekaj časa pred naslednjim merjenjem
}