#include <ESP32Servo.h>

//----- Pines -----
constexpr uint8_t TRIG_PIN    = 12;
constexpr uint8_t ECHO_PIN    = 13;
constexpr uint8_t GATE_PIN    = 14;
// Usamos solo pines digitales para los 4 LEDs (sin DAC)
constexpr uint8_t LED_PINS[4] = {2, 4, 26, 18};

//----- Parámetros -----
constexpr float       DIST_THRESHOLD  = 30.0f;   // cm para abrir compuerta
constexpr unsigned long AUTO_CLOSE_MS = 2000;    // ms para cierre automático

//----- Estado -----
bool           gateOpen = false;
unsigned long  gateTS   = 0;
Servo          gateServo;

//----- Lee HC-SR04 -----
float readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long dur = pulseIn(ECHO_PIN, HIGH, 30000);
  return (dur > 0) ? (dur * 0.0343f / 2.0f) : -1.0f;
}

void setup() {
  Serial.begin(115200);
  Serial.println("Iniciando: HC-SR04 + servo + 4 LEDs (digital)");

  // Configura HC-SR04
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Configura y enciende los 4 LEDs en HIGH
  for (int i = 0; i < 4; i++) {
    pinMode(LED_PINS[i], OUTPUT);
    digitalWrite(LED_PINS[i], HIGH);
  }
  Serial.println("Todos los LEDs estan encendidos");

  // Configura servo de compuerta
  gateServo.attach(GATE_PIN);
  gateServo.write(0);
  Serial.println("Servo inicializado en 0°");
}

void loop() {
  // Lectura de distancia
  float dist = readDistance();
  if (dist > 0) {
    Serial.printf("Distance: %.1f cm\n", dist);
  } else {
    Serial.println("Distance: lectura inválida");
  }

  // Abre compuerta si el objeto está dentro del umbral
  if (dist > 0 && dist < DIST_THRESHOLD && !gateOpen) {
    gateOpen = true;
    gateTS   = millis();
    gateServo.write(90);
    Serial.println("→ Objeto detectado: abriendo compuerta (90°)");
  }

  // Cierra compuerta tras AUTO_CLOSE_MS
  if (gateOpen && (millis() - gateTS) >= AUTO_CLOSE_MS) {
    gateOpen = false;
    gateServo.write(0);
    Serial.println("→ Tiempo cumplido: cerrando compuerta (0°)");
  }

  delay(500); // evita saturar la consola
}