// C++ code
//
int leftSensor = 0;

int rightSensor = 0;

int FrontMiddle = 0;

int Blower = 0;

long readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}

void setup()
{
  pinMode(5, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(24, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(30, OUTPUT);
  Serial.begin(9600);
  pinMode(4, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(3, OUTPUT);
}

void loop()
{
  digitalWrite(5, HIGH);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);
  digitalWrite(2, HIGH);
  digitalWrite(24, HIGH);
  digitalWrite(27, HIGH);
  digitalWrite(30, HIGH);
  FrontMiddle = 0.01723 * readUltrasonicDistance(13, 12);
  Serial.println(FrontMiddle);
  leftSensor = 0.01723 * readUltrasonicDistance(10, 9);
  Serial.println(leftSensor);
  rightSensor = 0.01723 * readUltrasonicDistance(7, 6);
  if (leftSensor > 15 && (rightSensor > 15 && FrontMiddle > 15)) {
    digitalWrite(4, LOW);
    digitalWrite(2, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(3, HIGH);
  } else {
    if (FrontMiddle < 15) {
      if (leftSensor < rightSensor) {
        digitalWrite(4, HIGH);
        digitalWrite(2, LOW);
        digitalWrite(5, LOW);
        digitalWrite(3, HIGH);
      } else {
        digitalWrite(4, LOW);
        digitalWrite(2, HIGH);
        digitalWrite(5, HIGH);
        digitalWrite(3, LOW);
      }
    }
  }
  leftSensor = 0.01723 * readUltrasonicDistance(10, 9);
  Serial.println(leftSensor);
  if (leftSensor < 15) {
    digitalWrite(3, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
  }
  rightSensor = 0.01723 * readUltrasonicDistance(7, 6);
  Serial.println(rightSensor);
  if (rightSensor < 15) {
    digitalWrite(3, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(2, HIGH);
    digitalWrite(4, LOW);
  }
  delay(10); // Delay a little bit to improve simulation performance
}