// --- PINES SEMÁFOROS ---
const int v1 = 12, a1 = 13, r1 = 14; 
const int v2 = 27, a2 = 26, r2 = 25; 

// --- PINES SENSORES (4 SENSORES) ---
const int trig1A = 18, echo1A = 19; 
const int trig1B = 5,  echo1B = 17; 
const int trig2A = 21, echo2A = 22; 
const int trig2B = 16, echo2B = 4;  

const int umbral = 15; 
int estadoActual = 0;  
bool turnoAlternancia = false;

// Cronómetros para cualquier nivel de ocupación
unsigned long tiempoEspera1 = 0;
unsigned long tiempoEspera2 = 0;

void setup() {
  Serial.begin(115200);
  int salidas[] = {v1, a1, r1, v2, a2, r2, trig1A, trig1B, trig2A, trig2B};
  for(int p : salidas) pinMode(p, OUTPUT);
  pinMode(echo1A, INPUT); pinMode(echo1B, INPUT);
  pinMode(echo2A, INPUT); pinMode(echo2B, INPUT);
  todoRojo();
}

long leerD(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10);
  digitalWrite(t, LOW);
  long d = pulseIn(e, HIGH) * 0.034 / 2;
  return (d <= 0) ? 400 : d;
}

void todoRojo() {
  digitalWrite(v1, LOW); digitalWrite(a1, LOW); digitalWrite(r1, HIGH);
  digitalWrite(v2, LOW); digitalWrite(a2, LOW); digitalWrite(r2, HIGH);
  estadoActual = 0;
}

void abrirCarril(int carril) {
  if (estadoActual == carril) return; 
  if (estadoActual == 1) {
    digitalWrite(v1, LOW); digitalWrite(a1, HIGH); delay(2000);
    digitalWrite(a1, LOW); digitalWrite(r1, HIGH); delay(1000);
  } else if (estadoActual == 2) {
    digitalWrite(v2, LOW); digitalWrite(a2, HIGH); delay(2000);
    digitalWrite(a2, LOW); digitalWrite(r2, HIGH); delay(1000);
  }
  if (carril == 1) {
    digitalWrite(r1, LOW); digitalWrite(v1, HIGH);
    estadoActual = 1;
  } else if (carril == 2) {
    digitalWrite(r2, LOW); digitalWrite(v2, HIGH);
    estadoActual = 2;
  }
}

void loop() {
  // 1. Conteo de vehículos (Igual)
  int fila1 = 0;
  if (leerD(trig1A, echo1A) < umbral) fila1++;
  if (leerD(trig1B, echo1B) < umbral) fila1++;

  int fila2 = 0;
  if (leerD(trig2A, echo2A) < umbral) fila2++;
  if (leerD(trig2B, echo2B) < umbral) fila2++;

  // 2. Control de tiempos (MEJORADO)
  // Agregamos un pequeño margen para que el tiempo no sea 0 instantáneamente
  if (fila1 >= 1 && estadoActual != 1) {
    if (tiempoEspera1 == 0) tiempoEspera1 = millis(); 
  } else if (estadoActual == 1 || fila1 == 0) {
    tiempoEspera1 = 0; 
  }

  if (fila2 >= 1 && estadoActual != 2) {
    if (tiempoEspera2 == 0) tiempoEspera2 = millis(); 
  } else if (estadoActual == 2 || fila2 == 0) {
    tiempoEspera2 = 0; 
  }

  // 3. Monitor Serial
  Serial.print("F1: " + String(fila1) + " | F2: " + String(fila2));
  if(tiempoEspera1 > 0) Serial.print(" | Esp1: " + String((millis()-tiempoEspera1)/1000) + "s");
  if(tiempoEspera2 > 0) Serial.print(" | Esp2: " + String((millis()-tiempoEspera2)/1000) + "s");
  Serial.println("");

  // 4. Lógica de Decisión (CON MEMORIA DE ESTADO)
  if (fila1 > fila2) {
    abrirCarril(1);
  } 
  else if (fila2 > fila1) {
    abrirCarril(2);
  } 
  else if (fila1 == fila2 && fila1 > 0) {
    unsigned long e1 = (tiempoEspera1 > 0) ? (millis() - tiempoEspera1) : 0;
    unsigned long e2 = (tiempoEspera2 > 0) ? (millis() - tiempoEspera2) : 0;

    // Si el semáforo ya está abierto en uno de los carriles del empate, 
    // MANTENERLO así a menos que el otro carril lleve más de 5 segundos esperando.
    if (estadoActual == 1 && e2 < 5000) {
       // Se queda en 1
    } else if (estadoActual == 2 && e1 < 5000) {
       // Se queda en 2
    } else {
       // Solo si el otro carril esperó mucho, o si veníamos de "Todo Rojo", decidimos por tiempo
       if (e1 >= e2) abrirCarril(1);
       else abrirCarril(2);
    }
  } 
  else {
    // Alternancia si está vacío
    static unsigned long ultimaAlt = 0;
    if (millis() - ultimaAlt > 8000) {
      turnoAlternancia = !turnoAlternancia;
      abrirCarril(turnoAlternancia ? 1 : 2);
      ultimaAlt = millis();
    }
  }

  delay(500);
}