#define ECHO_PIN3 7
#define TRIG_PIN3 6

#define ECHO_PIN4 4
#define TRIG_PIN4 5

#define ECHO_PIN1 2
#define TRIG_PIN1 3

#define ECHO_PIN2 8
#define TRIG_PIN2 9

void prenderBombillo();
void apagarBombillos();

int n;
int ind = 6;

void setup() {
  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);

  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);

  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);

  pinMode(TRIG_PIN4, OUTPUT);
  pinMode(ECHO_PIN4, INPUT);



  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);


  pinMode(0, INPUT);
}

float readDistanceCM1() {
  digitalWrite(TRIG_PIN1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN1, LOW);
  int duration = pulseIn(ECHO_PIN1, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM2() {
  digitalWrite(TRIG_PIN2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN2, LOW);
  int duration = pulseIn(ECHO_PIN2, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM3() {
  digitalWrite(TRIG_PIN3, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN3, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN3, LOW);
  int duration = pulseIn(ECHO_PIN3, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM4() {
  digitalWrite(TRIG_PIN4, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN4, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN4, LOW);
  int duration = pulseIn(ECHO_PIN4, HIGH);
  return duration * 0.034 / 2;
}

void loop() {

  float distance1 = readDistanceCM1();
  float distance2 = readDistanceCM2();
  float distance3 = readDistanceCM3();
  float distance4 = readDistanceCM4();

/*
  bool isNearby = distance < 100;
  digitalWrite(LED_BUILTIN, isNearby);

*/


  Serial.print("Calle1: ");
  Serial.print(distance1);

  Serial.print("        Calle2: ");
  Serial.print(distance2);

  Serial.print("        Calle3: ");
  Serial.print(distance3);

  Serial.print("        Calle4: ");
  Serial.println(distance4);


  delay(1000);

  if (digitalRead(0) == 1) {

    if (ind != 5 ) {
      ind = 5;
      apagarBombillos();
    }

  } else {

    if ( distance1 > distance2 && distance1 > distance3
      && distance1 > distance4) {
        
      if ( ind != 1){
        ind = 1;
        n = 10;
        prenderBombillo(n);
      }
        
      digitalWrite(10, HIGH);
      digitalWrite(11, LOW);
      digitalWrite(12, LOW);
      digitalWrite(13, LOW);

    } else if (distance2 > distance1 && distance2 > distance3
      && distance2 > distance4){

      if ( ind != 2){
        ind = 2;
        n = 11;
        prenderBombillo(n);
      }

      

      digitalWrite(10, LOW);
      digitalWrite(11, HIGH);
      digitalWrite(12, LOW);
      digitalWrite(13, LOW);

    } else if (distance3 > distance1 && distance3 > distance2
      && distance3 > distance4){

      if ( ind != 3){
        ind = 3;
        n = 12;
        prenderBombillo(n);
      }
      

      digitalWrite(10, LOW);
      digitalWrite(11, LOW);
      digitalWrite(12, HIGH);
      digitalWrite(13, LOW);

    } else {

      if ( ind != 4){
        ind = 4;
        n = 13;
        prenderBombillo(n);
      }

      digitalWrite(10, LOW);
      digitalWrite(11, LOW);
      digitalWrite(12, LOW);
      digitalWrite(13, HIGH);

    }
  }
}




void prenderBombillo(int n){
  for(int i=0 ; i<=3 ; i++){
    delay(180);
    digitalWrite(n, LOW);
    delay(180);
    digitalWrite(n, HIGH);
  }
}

void apagarBombillos(){
  for(int i=0 ; i<=3 ; i++){
    delay(180);
    
    digitalWrite(10, HIGH);
    digitalWrite(11, HIGH);
    digitalWrite(12, HIGH);
    digitalWrite(13, HIGH);

    delay(180);
    
    digitalWrite(10, LOW);
    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);

  }
}