#include <Keypad.h>
#include <Servo.h> // Cambiado a Servo estándar

// Pines del ESP32
const int trigPin = 2;   // Trig del sensor ultrasónico
const int echoPin = 15;  // Echo del sensor ultrasónico
const int buzzerPin = 4; // Bocina
const int servoPin = 23; // Pin del servo

// Definiciones del teclado (7 pines)
const byte ROWS = 4;
const byte COLS = 3;
char keys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};
byte rowPins[ROWS] = {12, 14, 27, 26}; // Filas conectadas a los pines del ESP32
byte colPins[COLS] = {25, 33, 32};     // Columnas conectadas a los pines del ESP32
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Definiciones del servo
Servo myServo; // Usando Servo estándar

// Variables de control
String correctCode = "1234";  // Código correcto para desactivar la alarma
String enteredCode = "";      // Código ingresado por el usuario
bool alarmActive = false;     // Indica si la alarma está activada
long duration;                // Duración del pulso del sensor ultrasónico
int distance;                 // Distancia medida por el sensor ultrasónico
bool objectDetected = false;  // Si se detecta un objeto cerca

void setup() {
  // Configuración del sensor ultrasónico
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Configuración de la bocina y el servo
  pinMode(buzzerPin, OUTPUT);
  myServo.attach(servoPin); // Asegúrate de que el pin 23 está correcto
  myServo.write(0); // Posición inicial del servo

  Serial.begin(115200);
}

void loop() {
  // Leer el sensor ultrasónico
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  // Si detecta algo cerca (menos de 5 cm), activa la alarma
  if (distance < 5 && distance > 0) {
    objectDetected = true;
    if (!alarmActive) {
      activateAlarm(); // Activar la alarma solo si no estaba ya activa
    }
  }

  // Leer el teclado
  char key = keypad.getKey();
  if (key) {
    Serial.print("Tecla presionada: ");
    Serial.println(key); // Mostrar en el monitor serial

    // Reiniciar el código si se presiona '*'
    if (key == '*') {
      enteredCode = ""; // Reiniciar el código ingresado
    } else if (key != '#') {
      // Agregar dígitos al código (máximo 4)
      if (enteredCode.length() < 4) {
        enteredCode += key;
      }

      // Cuando se hayan ingresado 4 dígitos, verifica el código
      if (enteredCode.length() == 4) {
        checkCode(); // Verifica si el código es correcto
      }
    }
  }

  // Si la alarma está activa, mover el servo sin retrasos largos
  if (alarmActive && objectDetected) {
    moveServo();  // Mover el servo continuamente mientras la alarma esté activa
  }
}

void activateAlarm() {
  alarmActive = true;
  tone(buzzerPin, 1000); // Encender la bocina
  Serial.println("Alarma activada");
}

void deactivateAlarm() {
  alarmActive = false;
  noTone(buzzerPin); // Apagar la bocina
  myServo.write(0);  // Detener el servo en la posición 0
  Serial.println("Alarma desactivada");
}

void checkCode() {
  // Verificar si el código ingresado es correcto
  if (enteredCode == correctCode) {
    deactivateAlarm(); // Desactivar la alarma si el código es correcto
    objectDetected = false; // Reiniciar la detección de objetos
  } else {
    Serial.println("Código incorrecto");
    enteredCode = ""; // Reiniciar si el código es incorrecto
  }
}

void moveServo() {
  // Mover el servo entre 0° y 180° y de vuelta
  static bool direction = true;
  static unsigned long lastMoveTime = 0;
  unsigned long currentTime = millis();

  if (currentTime - lastMoveTime > 500) {  // Cambiar la dirección del servo cada 500 ms
    if (direction) {
      myServo.write(180);  // Mover a 180°
    } else {
      myServo.write(0);    // Mover a 0°
    }
    direction = !direction; // Invertir la dirección
    lastMoveTime = currentTime; // Actualizar el tiempo del último movimiento
  }
}