#define OBSTACLE_SENSOR_PIN GPIO_NUM_5
#define RGB_RED_PIN   GPIO_NUM_3
#define RGB_GREEN_PIN GPIO_NUM_9
#define RGB_BLUE_PIN  GPIO_NUM_1

// Variables para el estado del sensor y lógica avanzada
int obstacleState = HIGH;
int lastObstacleState = HIGH; // Usado para detectar cambios y el estado de parpadeo (-1)
int detectionCount = 0;       // Contador de detecciones rápidas
unsigned long lastDetectionTime = 0; // Tiempo de la última detección (para resetear detectionCount)
unsigned long obstaclePresentStartTime = 0; // Tiempo en que el obstáculo se detectó por primera vez de forma continua
const long DETECTION_RESET_INTERVAL = 1000; // Si no hay detección por 1 segundo, resetear el contador de detecciones
const long LONG_PRESENCE_THRESHOLD = 3000; // 3 segundos para considerar "mucho tiempo"

void setRgbColor(bool red, bool green, bool blue) {
  digitalWrite(RGB_RED_PIN, red ? HIGH : LOW);
  digitalWrite(RGB_GREEN_PIN, green ? HIGH : LOW);
  digitalWrite(RGB_BLUE_PIN, blue ? HIGH : LOW);
}

void setup() {
  Serial.begin(115200);
  Serial.println(F("Sistema de Alerta RGB Avanzada de Obstaculos Iniciado."));
  pinMode(OBSTACLE_SENSOR_PIN, INPUT);
  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE_PIN, OUTPUT);

  setRgbColor(LOW, HIGH, LOW); // Estado inicial: VERDE (Todo Despejado)
}

void loop() {
  obstacleState = digitalRead(OBSTACLE_SENSOR_PIN);
  unsigned long currentTime = millis();

  // --- Lógica para resetear el contador de detecciones rápidas ---
  // Si ha pasado un tiempo sin detección Y el contador no está en 0
  if (currentTime - lastDetectionTime > DETECTION_RESET_INTERVAL && detectionCount > 0) {
    detectionCount = 0;
    // Si la última detección finalizó y ya pasó el intervalo de reseteo,
    // y no hay obstáculo presente, asegura que el color sea VERDE.
    if (obstacleState == HIGH && lastObstacleState != HIGH) {
      setRgbColor(LOW, HIGH, LOW); // Volver a Verde
      Serial.println("Reseteo de detecciones. Verde.");
    }
  }


  // --- Lógica principal de cambio de estado basada en la detección ---
  if (obstacleState != lastObstacleState) { // Si el estado del sensor ha cambiado
    if (obstacleState == LOW) { // ¡Obstáculo DETECTADO!
      detectionCount++;
      lastDetectionTime = currentTime; // Actualiza el tiempo de la última detección
      if (obstaclePresentStartTime == 0) { // Si es la primera detección continua, registra el inicio
        obstaclePresentStartTime = currentTime;
      }

      if (detectionCount == 1) {
        setRgbColor(HIGH, HIGH, LOW); // AMARILLO (Primera Detección)
        Serial.println("¡OBSTACULO! (1ra Deteccion) --> AMARILLO.");
      } else if (detectionCount >= 2) {
        setRgbColor(HIGH, LOW, HIGH); // MAGENTA (Detecciones Multiples)
        Serial.println("¡OBSTACULO! (Detecciones Multiples) --> MAGENTA.");
      }
    } else { // Obstáculo AUSENTE (el camino está libre)
      setRgbColor(LOW, HIGH, LOW); // VERDE (Todo Despejado)
      Serial.println("Camino LIBRE --> VERDE.");
      detectionCount = 0;          // Resetea el contador al estar libre
      obstaclePresentStartTime = 0; // Resetea el tiempo de presencia
    }
    // Solo actualiza lastObstacleState a LOW/HIGH si no es el estado de parpadeo ROJO.
    // Esto es para que el parpadeo pueda tomar control del LED sin interrumpir esta lógica.
    if (lastObstacleState != -1) {
      lastObstacleState = obstacleState;
    }
  }

  // --- Lógica para Obstáculo Presente por Mucho Tiempo (parpadeo en Rojo) ---
  // Se activa solo si hay un obstáculo y ha estado presente por más del umbral de tiempo
  if (obstacleState == LOW && obstaclePresentStartTime != 0 &&
      (currentTime - obstaclePresentStartTime > LONG_PRESENCE_THRESHOLD)) {

    // Controla el parpadeo del LED ROJO cada 250ms
    if ((currentTime % 500) < 250) { // ON por 250ms, 250ms OFF -> total 500ms ciclo
      setRgbColor(HIGH, LOW, LOW); // ROJO
    } else {
      setRgbColor(LOW, LOW, LOW);  // APAGADO
    }
    // Solo imprime el mensaje "persistente" una vez al entrar en este estado
    if (lastObstacleState != -1) {
      Serial.println("¡OBSTACULO PERSISTENTE! --> ROJO PARPADEANTE.");
      lastObstacleState = -1; // Marca un estado especial para el parpadeo
    }
  } else if (obstacleState == HIGH && lastObstacleState == -1) {
    // Si estaba en estado de "parpadeo ROJO" y el obstáculo se remueve,
    // asegúrate de volver a VERDE y resetear el estado.
    lastObstacleState = HIGH;
    setRgbColor(LOW, HIGH, LOW); // Volver a VERDE
    Serial.println("Obstaculo persistente removido --> VERDE.");
  }


  delay(10); // Pequeña pausa en el bucle principal
}