#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>  // Uporabi ESP32Servo, ne Servo.h

#define SDA_PIN 22
#define SCL_PIN 21

// Ustvari LCD objekt (naslov I2C je ponavadi 0x27 ali 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);  // LCD na naslovu 0x27, 16 stolpcev, 2 vrsti

// Nastavitve pinov
const int redLED = 32;     // Rdeča LED
const int yellowLED = 33;  // Rumena LED
const int greenLED = 25;   // Zelena LED
const int servoPin = 18;   // Servo motor

// Servo objekt
Servo zapornica;

// Časovni intervali
unsigned long rdeciCas = 5000;  // Čas za rdečo luč (5 sekund)
unsigned long rumeniCas = 2000; // Čas za rumeno luč (2 sekundi)
unsigned long zeleniCas = 5000; // Čas za zeleno luč (5 sekund)

// Spremenljivke za čas
unsigned long prejsnjiCas = 0;
int stanje = 0; // Trenutno stanje semaforja

void setup() {
  // Nastavi LED diode kot izhod
  pinMode(redLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(greenLED, OUTPUT);

  // Nastavi LCD zaslon
  lcd.begin(16, 2);  // 16 stolpcev, 2 vrsti
  lcd.backlight();
  lcd.clear();

  // Nastavi Servo motor
  zapornica.attach(servoPin);

  // Začetno stanje zapornice
  zapornica.write(0); // Zapornica je spuščena

  // Prikaži začetno sporočilo
  lcd.setCursor(0, 0);
  lcd.print("Semafor");
}

void loop() {
  unsigned long trenutniCas = millis();

  switch (stanje) {
    case 0: // Rdeča luč
      if (trenutniCas - prejsnjiCas >= rdeciCas) {
        // Premik na rumeno
        stanje = 1;
        prejsnjiCas = trenutniCas;
      } else {
        // Rdeča LED vklopljena, ostale izklopljene
        digitalWrite(redLED, HIGH);
        digitalWrite(yellowLED, LOW);
        digitalWrite(greenLED, LOW);

        // Zapornica spuščena
        zapornica.write(0);

        // Prikaži "Stoj" na LCD
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Stoj");
      }
      break;

    case 1: // Rumena luč
      if (trenutniCas - prejsnjiCas >= rumeniCas) {
        // Premik na zeleno
        stanje = 2;
        prejsnjiCas = trenutniCas;
      } else {
        // Rumena LED vklopljena, ostale izklopljene
        digitalWrite(redLED, LOW);
        digitalWrite(yellowLED, HIGH);
        digitalWrite(greenLED, LOW);

        // Zapornica spuščena (ostane spuščena)
        zapornica.write(0);

        // Prikaži "Pripravi se" na LCD
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Pripravi se");
      }
      break;

    case 2: // Zelena luč
      if (trenutniCas - prejsnjiCas >= zeleniCas) {
        // Premik nazaj na rdečo
        stanje = 0;
        prejsnjiCas = trenutniCas;
      } else {
        // Zelena LED vklopljena, ostale izklopljene
        digitalWrite(redLED, LOW);
        digitalWrite(yellowLED, LOW);
        digitalWrite(greenLED, HIGH);

        // Zapornica dvignjena
        zapornica.write(90);

        // Prikaži "Pojdi" na LCD
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Pojdi");
      }
      break;
  }
}