#include <ESP32Servo.h> // Incluye la librería ESP32Servo para controlar el servo

// Configuración del servo
Servo servo;
const int pinServo = 2; // Pin al que está conectado el servo
const int closedPosition = 0; // Posición del servo cuando la puerta está cerrada (horizontal)
const int openPosition = 90; // Posición del servo cuando la puerta está abierta (vertical)
const int openDoorWaitTime = 5000; // Tiempo en milisegundos para mantener la puerta abierta (5 segundos)
const int distanceThreshold = 30; // Distancia en cm para abrir la puerta

// Configuración del sensor HC-SR04
const int trigPin = 4; // Pin TRIG del sensor HC-SR04
const int echoPin = 5; // Pin ECHO del sensor HC-SR04

void setup() {
  Serial.begin(115200); // Configura el puerto serial a 115200 baudios
  
  servo.attach(pinServo, 500, 2500); // Asocia el objeto servo al pin especificado y establece los valores mínimo y máximo del PWM
  servo.write(closedPosition); // Establece la posición inicial del servo (puerta cerrada)

  pinMode(trigPin, OUTPUT); // Configura el pin TRIG como salida
  pinMode(echoPin, INPUT); // Configura el pin ECHO como entrada
}

void loop() {
  long duration;
  int distance;

  // Envía un pulso para iniciar la medición
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Lee el tiempo que tarda el pulso en volver
  duration = pulseIn(echoPin, HIGH);
  // Calcula la distancia en centímetros
  distance = (duration / 2) / 29.1;
  

  Serial.print("Distancia: ");
  Serial.print(distance);
  Serial.print(" cm, ");

  if (distance <= distanceThreshold) {
    // Mueve el servo a la posición de apertura (90 grados) si la distancia es menor o igual a 30 cm
    servo.write(openPosition);
    Serial.print("Posición del servo: ");
    Serial.println(openPosition);
    Serial.println("Puerta abierta (90 grados).");

    // Espera 5 segundos con la puerta abierta
    delay(openDoorWaitTime);

    // Recalcula la distancia después de esperar 5 segundos
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distance = (duration / 2) / 29.1;

    // Si la distancia es mayor a 30 cm, cierra la puerta
    if (distance > distanceThreshold) {
      servo.write(closedPosition);
      Serial.print("Posición del servo: ");
      Serial.println(closedPosition);
      Serial.println("Puerta cerrada (0 grados).");
    }
  } else {
    // Imprime la posición del servo si no se mueve
    Serial.print("Posición del servo: ");
    Serial.println(closedPosition);
  }

  delay(500); // Espera medio segundo antes de volver a medir
}