// Definición de pines según las etiquetas de tu placa
const int trigPin = A0; 
const int echoPin = A1;

const int led1 = D4; // Verde (< 80cm)
const int led2 = D5; // Amarillo (< 40cm)
const int led3 = D6; // Rojo (< 10cm)

void setup() {
  Serial.begin(9600);
  
  // IMPORTANTE: Esperar a que el puerto USB esté listo
  while (!Serial && millis() < 5000); 

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);

  Serial.println("--- Sistema Wearable Iniciado ---");
}

void loop() {
  long duracion;
  int distancia;

  // Lanzar el pulso ultrasónico
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Leer el eco (con un límite de espera de 30ms)
  duracion = pulseIn(echoPin, HIGH, 30000);
  distancia = duracion * 0.034 / 2;

  // Imprimir en el monitor serie para ver si hay vida
  if (duracion == 0) {
    Serial.println("Error: No se detecta el sensor. Revisa cables en A0 y A1.");
  } else {
    Serial.print("Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");
  }

  // --- Lógica de Semáforo ---
  digitalWrite(led1, (distancia > 0 && distancia < 80) ? HIGH : LOW);
  digitalWrite(led2, (distancia > 0 && distancia < 40) ? HIGH : LOW);
  digitalWrite(led3, (distancia > 0 && distancia < 10) ? HIGH : LOW);

  delay(100); 
}