const int sensorPinF = 19;
const int sensorPinR = 18;
const int ultrasonicTrigPin = 17;  // Pin de trig para el sensor ultrasónico
const int ultrasonicEchoPin = 16;  // Pin de echo para el sensor ultrasónico
const int MA = 25;
const int MB = 26;
const int MC = 32;
const int MD = 33;

void setup() {
  Serial.begin(9600);
  pinMode(sensorPinF, INPUT);
  pinMode(sensorPinR, INPUT);
  pinMode(ultrasonicTrigPin, OUTPUT);
  pinMode(ultrasonicEchoPin, INPUT);
  pinMode(MA, OUTPUT);
  pinMode(MB, OUTPUT);
  pinMode(MC, OUTPUT);
  pinMode(MD, OUTPUT);

  // Esperar 5 segundos al inicio
  delay(5000);
  
  // Encender MA y MC
  digitalWrite(MA, HIGH);
  digitalWrite(MD, HIGH);
  
  // Esperar otro cierto tiempo antes de activar MB y MD
  delay(200);
  
  // Encender MB y MD si los sensores no están activados
  if (digitalRead(sensorPinF) == HIGH && digitalRead(sensorPinR) == HIGH) {
    digitalWrite(MB, HIGH);
    digitalWrite(MD, HIGH);
  }
}

void loop() {
  int value1 = digitalRead(sensorPinF);
  int value2 = digitalRead(sensorPinR);
  if (value1 == LOW) 
  {
    digitalWrite(MA, HIGH);  // Encender el LED
  } else {
    digitalWrite(MA, LOW);  // Apagar el LED si no se detecta la zona blanca
  }
  if (value2 == LOW) 
  {
    digitalWrite(MC, HIGH);  // Encender el LED
  } else {
    digitalWrite(MC, LOW);  // Apagar el LED si no se detecta la zona blanca
  }

  // Verificar la distancia con el sensor ultrasónico
  digitalWrite(ultrasonicTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(ultrasonicTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(ultrasonicTrigPin, LOW);
  long duration = pulseIn(ultrasonicEchoPin, HIGH);
  int distance = duration * 0.034 / 2;

  // Imprimir la distancia en el monitor serie
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Activar MA y MC si la distancia es menor a cierto valor
  if (distance > 4 && value1 == HIGH && value2 == HIGH) {
    digitalWrite(MA, HIGH);
    digitalWrite(MC, HIGH);
  } else {
    digitalWrite(MA, LOW);
    digitalWrite(MC, LOW);
  }

  // Activar MB y MD si la distancia es mayor a cierto valor y los sensores no están activados
  if (distance < 6 && value1 == HIGH && value2 == HIGH) {
    digitalWrite(MB, HIGH);
    digitalWrite(MD, HIGH);
  } else {
    digitalWrite(MB, LOW);
    digitalWrite(MD, LOW);
  }

  delay(5);
}