#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h> // Librería para controlar el servomotor

// Definición de pines
#define TRIG_PIN_1 9 // Sensor ultrasónico existente
#define ECHO_PIN_1 10
#define TRIG_PIN_2 6 // Nuevo sensor ultrasónico
#define ECHO_PIN_2 5
#define GREEN_LED 8
#define RED_LED 7
#define SERVO_PIN 3 // Pin del servomotor

// Inicialización de la pantalla LCD (dirección I2C 0x27 y dimensiones 16x2)
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo; // Objeto para controlar el servomotor

void setup() {
  // Configuración de pines como entrada/salida
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  // Estado inicial de los LEDs
  digitalWrite(GREEN_LED, HIGH); // LED verde encendido
  digitalWrite(RED_LED, LOW);    // LED rojo apagado

  // Inicializar pantalla LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Sistema iniciado");

  // Inicializar servomotor
  servo.attach(SERVO_PIN);
  servo.write(0); // Tapa cerrada

  // Comunicación serial para depuración
  Serial.begin(9600);
}

void loop() {
  long duration1, distance1, duration2, distance2;

  // Sensor ultrasónico existente (distancia a la basura)
  digitalWrite(TRIG_PIN_1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_1, LOW);

  duration1 = pulseIn(ECHO_PIN_1, HIGH);
  distance1 = duration1 * 0.034 / 2;

  // Nuevo sensor ultrasónico (detección de movimiento)
  digitalWrite(TRIG_PIN_2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_2, LOW);

  duration2 = pulseIn(ECHO_PIN_2, HIGH);
  distance2 = duration2 * 0.034 / 2;

  // Condición para verificar si la papelera está llena
  if (distance1 > 0 && distance1 <= 15) {
    digitalWrite(GREEN_LED, LOW);  // Apagar LED verde
    digitalWrite(RED_LED, HIGH);  // Encender LED rojo
    lcd.setCursor(0, 0);
    lcd.print(" PAPELERA LLENA  ");
    servo.write(0); // Mantener tapa cerrada
  } else {
    digitalWrite(GREEN_LED, HIGH); // Encender LED verde
    digitalWrite(RED_LED, LOW);    // Apagar LED rojo
    lcd.setCursor(0, 0);
    lcd.print("PAPELERA NO LLENA");

    // Abrir tapa si se detecta movimiento a más de 30 cm
    if (distance2 > 30) {
      servo.write(90); // Abrir tapa
      delay(3000); // Mantener la tapa abierta por 3 segundos
      servo.write(0); // Cerrar tapa
    }
  }

  // Pequeño retraso para estabilidad
  delay(100);
}