// ============================================================
// MineGuard — Sistema Completo (US + Colisión + Pulso + GPS + SOS)
// Optimizado para LCD 16x2 con pantalla rotativa y retención SOS
// ============================================================

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <HardwareSerial.h>
#include <TinyGPS++.h>

// ── Pines de Conexión ─────────────────────────────────────
const int PIN_TRIG        = 5;
const int PIN_ECHO        = 18;
const int PIN_BUZZER      = 23;
const int PIN_LED_ROJO    = 2;
const int PIN_LED_VERDE   = 15;
const int PIN_PULSO       = 32; 
const int PIN_COLISION    = 4;
const int PIN_BOTON_EMERG = 13; 
const int GPS_RX          = 16; 
const int GPS_TX          = 17; 

// ── Configuración de Objetos Serie y GPS ──────────────────
HardwareSerial gpsSerial(2); 
TinyGPSPlus gps;

// ── Umbrales y Tiempos ────────────────────────────────────
const int DIST_ALERTA_CM   = 40;  
const int DIST_MAX_CM      = 400; 
const unsigned long TIEMPO_RETENCION_COLISION = 2000; 
const unsigned long TIEMPO_RETENCION_SOS      = 5000; // El SOS dura 5 segundos en pantalla tras soltar el botón

const int BPM_MIN_VALID    = 40;  
const int BPM_MAX_VALID    = 200; 
const int BPM_FATIGA       = 55;  
const int BPM_ALERTA_ALTO  = 100; 

// ── LCD 16x2 con I2C ──────────────────────────────────────
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ── Estado global ─────────────────────────────────────────
bool estadoEmergencia   = false;
bool fatigaDetectada    = false;
bool alertaManualActiva = false; 

// ── Temporización (non-blocking) ──────────────────────────
unsigned long tUltrasonico = 0;
unsigned long tPulso       = 0;
unsigned long tLCD         = 0;
unsigned long tBuzzer      = 0;
unsigned long tUltimoSOS   = 0; // Temporizador para el botón SOS

const unsigned long INTERVALO_ULTRASONICO = 100;  
const unsigned long INTERVALO_PULSO       = 20;   
const unsigned long INTERVALO_LCD         = 500;  
const unsigned long DURACION_BUZZER       = 600;  

// ── Variables de Sensores ─────────────────────────────────
float distanciaCm = 999.0;
bool buzzerActivo = false;
unsigned long tBuzzerFin = 0;

int contadorCercania = 0;
const int LECTURAS_REQUERIDAS = 3;

bool colisionActiva = false;
unsigned long tUltimaColision = 0;

int muestras[20];
int idxMuestra = 0;
long sumaMuestras = 0;
unsigned long ultimoPico = 0;
unsigned long intervaloPico = 0;
int bpmActual = 0;
bool enPico = false;

// ══════════════════════════════════════════════════════════
void setup() {
  Serial.begin(115200);
  
  gpsSerial.begin(9600, SERIAL_8N1, GPS_RX, GPS_TX);

  pinMode(PIN_TRIG,        OUTPUT);
  pinMode(PIN_LED_ROJO,    OUTPUT);
  pinMode(PIN_LED_VERDE,   OUTPUT);
  pinMode(PIN_BUZZER,      OUTPUT);
  pinMode(PIN_ECHO,        INPUT);
  pinMode(PIN_COLISION,    INPUT_PULLUP);
  pinMode(PIN_BOTON_EMERG, INPUT_PULLUP); 

  digitalWrite(PIN_LED_VERDE, HIGH);
  digitalWrite(PIN_LED_ROJO,  LOW);
  digitalWrite(PIN_BUZZER,    LOW);

  Wire.begin(21, 22);
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0); lcd.print(" MineGuard V1.4 ");
  lcd.setCursor(0, 1); lcd.print(" Iniciando...   ");
  delay(1500);
  lcd.clear();

  Serial.println("[MineGuard] Sistema iniciado.");
}

// ══════════════════════════════════════════════════════════
void loop() {
  unsigned long ahora = millis();

  while (gpsSerial.available() > 0) {
    gps.encode(gpsSerial.read());
  }

  leerColision(); 
  leerBotonEmergencia(); 

  if (ahora - tUltrasonico >= INTERVALO_ULTRASONICO) {
    tUltrasonico = ahora;
    leerUltrasonico();
  }

  if (ahora - tPulso >= INTERVALO_PULSO) {
    tPulso = ahora;
    leerPulso();
  }

  if (ahora - tLCD >= INTERVALO_LCD) {
    tLCD = ahora;
    actualizarLCD();
  }

  gestionarAlertas();

  if (buzzerActivo && millis() >= tBuzzerFin) {
    noTone(PIN_BUZZER);
    buzzerActivo = false;
  }
}

// ══════════════════════════════════════════════════════════
// MÓDULOS DE LECTURA DE SENSORES
// ══════════════════════════════════════════════════════════
void leerBotonEmergencia() {
  // Si se presiona el botón, se activa la alerta y se reinicia el temporizador
  if (digitalRead(PIN_BOTON_EMERG) == LOW) {
    tUltimoSOS = millis();
    alertaManualActiva = true;
    Serial.println("[SOS] Botón de emergencia activado manualmente.");
  }
  
  // Apagar la alerta SOS si ya pasaron los 5 segundos desde que se soltó
  if (alertaManualActiva && (millis() - tUltimoSOS >= TIEMPO_RETENCION_SOS)) {
    alertaManualActiva = false;
  }
}

void leerColision() {
  if (digitalRead(PIN_COLISION) == LOW) {
    tUltimaColision = millis();
    colisionActiva = true;
    Serial.println("[KY031] ¡COLISION DETECTADA!");
  }
  if (colisionActiva && (millis() - tUltimaColision >= TIEMPO_RETENCION_COLISION)) {
    colisionActiva = false;
  }
}

void leerUltrasonico() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  long duracion = pulseIn(PIN_ECHO, HIGH, 25000);
  if (duracion == 0) {
    distanciaCm = DIST_MAX_CM;
  } else {
    distanciaCm = (duracion * 0.0343) / 2.0;
    if (distanciaCm > DIST_MAX_CM) distanciaCm = DIST_MAX_CM;
  }
}

void leerPulso() {
  int lectura = analogRead(PIN_PULSO);
  sumaMuestras -= muestras[idxMuestra];
  muestras[idxMuestra] = lectura;
  sumaMuestras += lectura;
  idxMuestra = (idxMuestra + 1) % 20;

  int promedio = sumaMuestras / 20;
  int umbral   = promedio + 300; 
  unsigned long ahora = millis();

  if (lectura > umbral && !enPico) {
    enPico = true;
    if (ultimoPico > 0) {
      intervaloPico = ahora - ultimoPico;
      if (intervaloPico > 300 && intervaloPico < 1500) { 
        bpmActual = 60000 / intervaloPico;
      }
    }
    ultimoPico = ahora;
  }
  if (lectura < umbral - 100) {
    enPico = false;
  }
  if (ahora - ultimoPico > 3000) {
    bpmActual = 0;
  }
}

// ══════════════════════════════════════════════════════════
// CONTROL DE ALERTAS
// ══════════════════════════════════════════════════════════
void gestionarAlertas() {
  bool hayPeligro = false;
  fatigaDetectada = false;

  if (alertaManualActiva) hayPeligro = true; 

  if (distanciaCm <= DIST_ALERTA_CM) {
    contadorCercania++;
    if (contadorCercania >= LECTURAS_REQUERIDAS) hayPeligro = true;
  } else {
    contadorCercania = 0;
  }
  
  if (colisionActiva) hayPeligro = true;

  if (bpmActual >= BPM_MIN_VALID && bpmActual <= BPM_MAX_VALID) {
    if (bpmActual < BPM_FATIGA) {
      fatigaDetectada = true;
      hayPeligro = true;
    } else if (bpmActual >= BPM_ALERTA_ALTO) {
      hayPeligro = true;
    }
  }

  if (hayPeligro) {
    activarEmergencia();
  } else if (estadoEmergencia) {
    desactivarEmergencia();
  }
}

void activarEmergencia() {
  estadoEmergencia = true;
  digitalWrite(PIN_LED_ROJO, HIGH);
  digitalWrite(PIN_LED_VERDE, LOW);
  if (!buzzerActivo) {
    tone(PIN_BUZZER, 1000); 
    tBuzzerFin  = millis() + DURACION_BUZZER;
    buzzerActivo = true;
  }
}

void desactivarEmergencia() {
  estadoEmergencia = false;
  digitalWrite(PIN_LED_ROJO, LOW);
  digitalWrite(PIN_LED_VERDE, HIGH);
}

// ══════════════════════════════════════════════════════════
// RENDERIZADO LCD 16x2 (Pantalla Dinámica)
// ══════════════════════════════════════════════════════════
void actualizarLCD() {
  lcd.clear();

  // ── Línea 0: Cabecera de Alertas (Max 16 caracteres) ──
  lcd.setCursor(0, 0);
  if (alertaManualActiva)       lcd.print("!SOS ACTIVADO!  ");
  else if (colisionActiva)      lcd.print("!ALERTA CHOQUE! ");
  else if (fatigaDetectada)     lcd.print("!ALERTA FATIGA! ");
  else if (distanciaCm <= DIST_ALERTA_CM && contadorCercania >= LECTURAS_REQUERIDAS) lcd.print("!OBJETO CERCA!  ");
  else if (bpmActual >= BPM_ALERTA_ALTO) lcd.print("!PULSO ALTO!    ");
  else                          lcd.print("MineGuard [OK]  ");

  // ── Línea 1: Pantalla Rotativa o Fija (Max 16 caracteres) ──
  lcd.setCursor(0, 1);
  
  // Si hay una emergencia SOS manual, FORZAR mostrar el GPS abajo
  if (alertaManualActiva) {
    if (gps.location.isValid()) {
      char buf[17];
      // "G:-12.08,-77.08" encaja en 15 caracteres. "GPS:" es muy largo para la pantalla.
      snprintf(buf, sizeof(buf), "G:%2.2f,%2.2f", gps.location.lat(), gps.location.lng());
      lcd.print(buf);
    } else {
      lcd.print("GPS: Buscando...");
    }
  } 
  // Si NO hay SOS manual, la pantalla sigue rotando normalmente
  else {
    int pantallaActual = (millis() / 2500) % 2; 

    if (pantallaActual == 0) {
      char buf[17];
      int d = (distanciaCm >= DIST_MAX_CM) ? 999 : (int)distanciaCm;
      int p = (bpmActual >= BPM_MIN_VALID && bpmActual <= BPM_MAX_VALID) ? bpmActual : 0;
      snprintf(buf, sizeof(buf), "D:%3dcm P:%3dbpm", d, p);
      lcd.print(buf);
    } else {
      if (gps.location.isValid()) {
        char buf[17];
        snprintf(buf, sizeof(buf), "G:%2.2f,%2.2f", gps.location.lat(), gps.location.lng());
        lcd.print(buf);
      } else {
        lcd.print("GPS: Buscando...");
      }
    }
  }
}