#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int trig1 = 16; 
const int echo1 = 13; 
const int trig2 = 14; 
const int echo2 = 27; 

#define PIN_RED 2      
#define PIN_YELLOW 4   
#define PIN_BLUE 5     
#define BTN_RESET 25   
#define BTN_EMERGENCY 26 
#define SERVO_PIN 32
#define RELAY_PIN 33 

Adafruit_SSD1306 display(128, 64, &Wire, -1);
Servo cancela;

bool emergencyMode = false;
bool barrierOpen = false;
unsigned long timerClose = 0;
unsigned long lastSensorRead = 0;
const int closeDelay = 5000; 
const int distThreshold = 20; 
bool lastEmergState = HIGH;
String lastActionMsg = "Pronto";

void setup() {
  Serial.begin(115200);
  delay(1000); 
  Serial.println("--- INICIANDO BOOT ESCALONADO ---");

  // 1. Configura pinos básicos
  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_YELLOW, OUTPUT);
  pinMode(PIN_BLUE, OUTPUT);
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BTN_RESET, INPUT_PULLUP);
  pinMode(BTN_EMERGENCY, INPUT_PULLUP);
  
  // 2. Inicia os sensores com delay entre eles
  pinMode(trig1, OUTPUT); pinMode(echo1, INPUT);
  digitalWrite(trig1, LOW);
  delay(200); 
  
  pinMode(trig2, OUTPUT); pinMode(echo2, INPUT);
  digitalWrite(trig2, LOW);
  delay(200);

  // 3. Inicia o OLED (ponto crítico de crash)
  Serial.println("Ligando Display...");
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("Aviso: OLED nao responde");
  } else {
    display.clearDisplay();
    display.display();
  }
  delay(200);

  // 4. Inicia o Servo por último
  Serial.println("Ligando Servo...");
  cancela.setPeriodHertz(50);
  cancela.attach(SERVO_PIN, 500, 2400); 
  cancela.write(0); 
  
  Serial.println("--- SETUP FINALIZADO ---");
}

long getDistance(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10);
  digitalWrite(t, LOW);
  long duration = pulseIn(e, HIGH, 20000); // Timeout menor para não travar
  if (duration <= 0) return 999;
  return duration * 0.034 / 2;
}

void openBarrier(String source) {
  if (!barrierOpen) {
    digitalWrite(PIN_RED, HIGH);
    cancela.write(90); 
    barrierOpen = true;
    lastActionMsg = source;
    digitalWrite(RELAY_PIN, HIGH);
    delay(150);
    digitalWrite(PIN_RED, LOW);
  }
}

void closeBarrier(String source) {
  if (barrierOpen && !emergencyMode) {
    digitalWrite(PIN_RED, HIGH);
    cancela.write(0); 
    barrierOpen = false;
    lastActionMsg = source;
    digitalWrite(RELAY_PIN, LOW);
    delay(150);
    digitalWrite(PIN_RED, LOW);
  }
}

void loop() {
  if (digitalRead(BTN_RESET) == LOW) ESP.restart();

  bool currentEmerg = digitalRead(BTN_EMERGENCY);
  if (currentEmerg == LOW && lastEmergState == HIGH) {
    delay(50); 
    emergencyMode = !emergencyMode;
    if (emergencyMode) {
      digitalWrite(RELAY_PIN, HIGH);
      openBarrier("EMERGENCIA");
    } else {
      digitalWrite(RELAY_PIN, LOW);
      timerClose = millis();
    }
  }
  lastEmergState = currentEmerg;

  if (!emergencyMode && (millis() - lastSensorRead > 500)) { 
    long d1 = getDistance(trig1, echo1);
    long d2 = getDistance(trig2, echo2);
    
    Serial.print("S1: "); Serial.print(d1);
    Serial.print("cm | S2: "); Serial.print(d2); Serial.println("cm");

    if (d1 < distThreshold && d1 > 1 && !barrierOpen) {
      openBarrier("Entrada");
    }

    if (barrierOpen) {
      if (d2 < distThreshold && d2 > 1) {
        timerClose = millis(); 
      } else if (millis() - timerClose > closeDelay) {
        closeBarrier("Fechado");
      }
    }
    lastSensorRead = millis();
  }

  updateDisplay();
}

void updateDisplay() {
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.setTextSize(1);
  display.println("SISTEMA ATIVO");
  display.drawLine(0, 10, 128, 10, WHITE);
  
  display.setCursor(0, 20);
  display.setTextSize(2);
  if (emergencyMode) display.println("!EMERG!");
  else display.println(barrierOpen ? "ABERTA" : "FECHADA");

  display.setTextSize(1);
  display.setCursor(0, 45);
  display.print("Luz: "); display.print(digitalRead(RELAY_PIN) ? "ON" : "OFF");
  display.setCursor(0, 55);
  display.print("Dist S1: "); display.print(getDistance(trig1, echo1));
  display.display();
}