#define TRIG_PIN 18
#define ECHO_PIN 19
#define MOTOR_VIBRATION 5
#define ZUMBADOR 4
#define SWITCH_PIN 21
#define LED_PIN 15

bool sistemaActivado = false;
bool botonAnterior = HIGH;
bool estadoAlarma = false;
unsigned long tiempoAnterior = 0;
unsigned long tiempoBoton = 0;

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(MOTOR_VIBRATION, OUTPUT);
  pinMode(ZUMBADOR, OUTPUT);
  pinMode(SWITCH_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
}

void leerBoton() {
  bool botonActual = digitalRead(SWITCH_PIN);
  unsigned long ahora = millis();

  if (botonAnterior == HIGH && botonActual == LOW) {
    if (ahora - tiempoBoton >= 300) {
      tiempoBoton = ahora;
      if (sistemaActivado == true) {
        sistemaActivado = false;
        Serial.println("Sistema APAGADO");
      } else {
        sistemaActivado = true;
        Serial.println("Sistema ENCENDIDO");
      }
    }
  }
  botonAnterior = botonActual;
}

void apagarTodo() {
  digitalWrite(MOTOR_VIBRATION, LOW);
  digitalWrite(ZUMBADOR, LOW);
  digitalWrite(LED_PIN, LOW);
  estadoAlarma = false;
  tiempoAnterior = 0;
}

void loop() {
  leerBoton();

  if (sistemaActivado == false) {
    apagarTodo();
    return;
  }

  // Enviar pulso ultrasónico
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Medir distancia
  long duracion = pulseIn(ECHO_PIN, HIGH);
  int distancia = duracion * 0.034 / 2;

  // Monitor serial
  Serial.print("Distancia: ");
  Serial.print(distancia);
  Serial.println(" cm");

  // Definir intervalo según distancia
  int intervalo = 0;

  if (distancia > 100 && distancia <= 200) {
    intervalo = 1000;
  } else if (distancia > 50 && distancia <= 100) {
    intervalo = 400;
  } else if (distancia > 0 && distancia <= 50) {
    intervalo = 100;
  } else {
    apagarTodo();
    return;
  }

  // Beep con millis sin bloquear
  unsigned long ahora = millis();

  if (ahora - tiempoAnterior >= intervalo) {
    tiempoAnterior = ahora;

    leerBoton();
    if (sistemaActivado == false) {
      apagarTodo();
      return;
    }

    if (estadoAlarma == false) {
      digitalWrite(LED_PIN, HIGH);
      digitalWrite(ZUMBADOR, HIGH);
      digitalWrite(MOTOR_VIBRATION, HIGH);
      estadoAlarma = true;
    } else {
      digitalWrite(LED_PIN, LOW);
      digitalWrite(ZUMBADOR, LOW);
      digitalWrite(MOTOR_VIBRATION, LOW);
      estadoAlarma = false;
    }
  }
}