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

Servo servo1;
const int ABERTO = 90;
const int FECHADO = 0;

#define PIN_TRIG 4
#define PIN_ECHO 5
#define PIN_TRIG2 2
#define PIN_ECHO2 3

LiquidCrystal_I2C lcd (0x26, 16, 2);
LiquidCrystal_I2C lcd2(0x27, 16, 2);

int estadoServo = FECHADO; // Variável para rastrear diretamente o estado do servo (aberto ou fechado)

void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  servo1.attach(11);
  pinMode(PIN_TRIG2, OUTPUT);
  pinMode(PIN_ECHO2, INPUT);
  servo1.write(FECHADO);

  // Inicializa o LCD
  lcd.init();
  lcd.setBacklight(1);

  lcd2.init();
  lcd2.setBacklight(1);
}

void loop() {
  // ---- Exibe mensagens padrão no início do loop ----
  if (estadoServo == FECHADO) {
    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("PERCURSO");
    lcd.setCursor(5, 1);
    lcd.print("LIVRE");
    
    lcd2.clear();
    lcd2.setCursor(4, 0);
    lcd2.print("PERCURSO");
    lcd2.setCursor(5, 1);
    lcd2.print("LIVRE");
    delay(500); // Aguarda para garantir exibição antes de checar os sensores
  }

  // ---- Leitura do primeiro sensor ----
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  int duration = pulseIn(PIN_ECHO, HIGH);
  int distancia = duration / 58;

  // ---- Leitura do segundo sensor ----
  digitalWrite(PIN_TRIG2, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG2, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG2, LOW);

  int duration2 = pulseIn(PIN_ECHO2, HIGH);
  int distancia2 = duration2 / 58;

  // Lógica para o Sensor 1
  if (distancia >= 150 && estadoServo == FECHADO) { 
    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("DIMINUA A");
    lcd.setCursor(3, 1);
    lcd.print("VELOCIDADE");
    
    lcd2.clear();
    lcd2.setCursor(4, 0);
    lcd2.print("CANCELA");
    lcd2.setCursor(2, 1);
    lcd2.print("SERA ABERTA");
    delay(2000); // Aguarda antes de abrir o servo
    
    servo1.write(ABERTO);
    lcd2.clear();
    lcd2.setCursor(4, 0);
    lcd2.print("CANCELA");
    lcd2.setCursor(4, 1);
    lcd2.print("ABERTA");

    estadoServo = ABERTO; // Atualiza o estado do servo
  }

  // Lógica para o Sensor 2
  if (distancia2 >= 150 && estadoServo == ABERTO) {
    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("PERCURSO");
    lcd.setCursor(5, 1);
    lcd.print("LIVRE");
    
    lcd2.clear();
    lcd2.setCursor(4, 0);
    lcd2.print("PERCURSO");
    lcd2.setCursor(5, 1);
    lcd2.print("LIVRE");
    servo1.write(FECHADO); // Fecha a cancela e reinicia o sistema

    estadoServo = FECHADO; // Reinicia o estado do servo
  }

  delay(100); // Pequeno atraso para evitar interferências
}