#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// DEFINICIÓN DE PINES (Según tu dibujo)
const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
const int LED_VERDE = 4;
const int LED_ROJO = 2;
const int BUZZER = 13;

// VARIABLES
Adafruit_MPU6050 mpu;
long duracion;
int distancia;
sensors_event_t a, g, temp;

void setup() {
  Serial.begin(115200);
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_VERDE, OUTPUT);
  pinMode(LED_ROJO, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  
  Serial.println("Inicializando MPU6050...");
  Wire.begin();
  
  if (!mpu.begin()) {
    Serial.println("Error: MPU6050 no encontrado.");
    while (1);
  }
  Serial.println("MPU6050 listo!");
  
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void loop() {
  // LEER SENSOR ULTRASÓNICO
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duracion = pulseIn(ECHO_PIN, HIGH);
  
  // Calcular distancia
  if (duracion > 0) {
    distancia = duracion * 0.034 / 2;
  } else {
    distancia = 999; // Si no detecta nada, poner una distancia muy alta
  }
  
  // CONTROL DE LEDS Y BUZZER (Lógica de alarma)
  if (distancia > 0 && distancia <= 30) { 
    // Si hay un objeto cerca (Peligro)
    digitalWrite(LED_ROJO, HIGH);
    digitalWrite(LED_VERDE, LOW);
    
    // Hacer sonar el Buzzer con un pitido rápido (Método seguro para Wokwi)
    digitalWrite(BUZZER, HIGH);
    delay(100);
    digitalWrite(BUZZER, LOW);
    delay(100);
    
  } else {
    // Si está lejos (Seguro)
    digitalWrite(LED_ROJO, LOW);
    digitalWrite(LED_VERDE, HIGH);
    digitalWrite(BUZZER, LOW); // Apagar Buzzer
  }
  
  // LEER MPU6050
  mpu.getEvent(&a, &g, &temp);
  
  // MOSTRAR DATOS EN MONITOR SERIE
  Serial.print("Distancia: ");
  Serial.print(distancia);
  Serial.print(" cm | Acel X: ");
  Serial.print(a.acceleration.x);
  Serial.print(" Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(" Z: ");
  Serial.println(a.acceleration.z);
  
  delay(200);
}