#define TRIGGER_PIN1   2
#define ECHO_PIN1      3
#define TRIGGER_PIN2   4
#define ECHO_PIN2      5
#define TRIGGER_PIN3   8
#define ECHO_PIN3      9
#define TRIGGER_PIN4   10
#define ECHO_PIN4      11
long duration;
int distanceCm, distanceInch;
unsigned long timer1;
unsigned long timer2;
int switchPin = 7;
int LEDPin = 13;
int state = 0;
int prevState = 0; 
void setup() {
  pinMode (switchPin, INPUT);
  pinMode (LEDPin, OUTPUT);
  pinMode (TRIGGER_PIN1, OUTPUT);
  pinMode (ECHO_PIN1, INPUT);
  pinMode (TRIGGER_PIN2, OUTPUT);
  pinMode (ECHO_PIN2, INPUT);
  pinMode (TRIGGER_PIN3, OUTPUT);
  pinMode (ECHO_PIN3, INPUT);
  pinMode (TRIGGER_PIN4, OUTPUT);
  pinMode (ECHO_PIN4, INPUT);
  timer1 = millis();
  timer2 = millis();
  Serial.begin(9600);
}
void loop() {
  int buttonState = digitalRead(switchPin);
  int dist1 = distanceSensor(TRIGGER_PIN1, ECHO_PIN1);
  int dist2 = distanceSensor(TRIGGER_PIN2, ECHO_PIN2);
  int dist3 = distanceSensor(TRIGGER_PIN3, ECHO_PIN3);
  int dist4 = distanceSensor(TRIGGER_PIN4, ECHO_PIN4);
  
  int array[] = {dist1, dist2, dist3, dist4};
  int count = sizeof(array) / sizeof(array[0]);
  int index = 0;
  for (int i = 1; i < count; i++){

    if (array[i] < array[index]) {
      index = i;
    }
  }
  if (millis() - timer1 > 100){
    timer1 = millis();
    if (buttonState == 1) {
      prevState = state;
      state = 1;
    }
    else {
      prevState = state;
      state = 0;
    }
    if (prevState == 0 && state == 1) {
      timer2 = millis();
    }

    if (prevState == 1 && state == 0){
      if (millis()-timer2 < 2000){
        Serial.println(1);
      }
      else {
        Serial.println(6);
      }
    }
  }
 
  
  if (array[index] < 20){
    if (index == 0) {
      Serial.println(2);
    }
    else if (index == 1){
      Serial.println(3);
    }
    else if (index == 2){
      Serial.println(4);
    }
    else if (index == 3){
      Serial.println(5);
    }
  }


}

// Sensor Function to return distance in either CM or Inch
int distanceSensor(char triggerPin, char echoPin) {
    // Reset Signal
 digitalWrite(triggerPin, LOW);
 delayMicroseconds(2);
 // Send High signal for 10 microseconds
 digitalWrite(triggerPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(triggerPin, LOW);

 // Received Signal
 long duration = pulseIn(echoPin, HIGH);
 // convert the time into a distance
 int distanceCm = duration*0.034/2;
 int distanceInch = duration*0.0133/2;
 return distanceCm;

}