#include <Arduino.h>
#include <cstdlib>

void setup() {
  pinMode(23, OUTPUT);
  pinMode(22, OUTPUT);
  pinMode(21, OUTPUT);
  pinMode(19, OUTPUT);
  pinMode(18, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(17, OUTPUT);
  pinMode(16, INPUT); 
  pinMode(4, OUTPUT); 
  pinMode(0, INPUT);
  Serial.begin(9600); 

}
float medirDistancia(int triggerPin, int echoPin) {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  long duracion = pulseIn(echoPin, HIGH);
  float distancia = (duracion * 0.034) / 2;
  return distancia;
}

void loop(){
  float tiempoTotal = 0;
  for(int conteo = 0; conteo < 10; conteo++) {
    float distancia1 = medirDistancia(17, 16);
    float distancia2 = medirDistancia(4, 0);
    int LEDRandom = rand() % 2;
    unsigned long tiempoInicio = millis();
    if(LEDRandom == 0){
      digitalWrite(23, HIGH);
      digitalWrite(22, LOW);
      while (digitalRead(18) == HIGH || distancia1 < 10  || distancia2 > 10) {
        distancia1 = medirDistancia(17, 16);
        distancia2 = medirDistancia(4, 0);
      }
      unsigned long tiempoFin = millis();
      tiempoTotal += (tiempoFin - tiempoInicio) / 1000.0;
      digitalWrite(23, LOW);
      digitalWrite(21, HIGH);
      delay(1000);
      digitalWrite(21, LOW);
    } 
    else {
      digitalWrite(23, LOW);
      digitalWrite(22, HIGH);
      unsigned long tiempoFin = millis();
      tiempoTotal += (tiempoFin - tiempoInicio) / 1000.0;
      while (digitalRead(5) == HIGH || distancia2 < 10 || distancia1 > 10) {
        distancia2 = medirDistancia(4, 0);
        distancia1 = medirDistancia(17, 16);
      }
      digitalWrite(22, LOW);
      digitalWrite(19, HIGH);
      delay(1000);
      digitalWrite(19, LOW);   
    }
  }
  float tiempoPromedio = tiempoTotal;
  Serial.print("Tiempo promedio para ponerse en verde: ");
  Serial.print(tiempoPromedio);
  Serial.println(" segundos");
  
  digitalWrite(23, LOW);
  digitalWrite(22, LOW);
  digitalWrite(21, LOW);
  digitalWrite(19, LOW);
  while (digitalRead(18) == HIGH || digitalRead(5) == HIGH) {
  } 
}