#define left A5
#define center A4
#define right  A3

//motor one
#define ENA 5
#define IN1 2
#define IN2 7

//motor two
#define ENB 6
#define IN3 4
#define IN4 3

#define echoPin A0        // Echo Pin
#define trigPin A1        // Trigger Pin

int maximumRange = 20;    // Maximum range to start attacking

long duration, distance;  // Variables to calculate distance

int Speed = 100; // speed of this robot
int buzzer = 8;
bool leftV ;
bool centerV ;
bool rightV ;


void setup()
{
  Serial.begin(9600);
  pinMode(left, INPUT);
  pinMode(center, INPUT);
  pinMode(right, INPUT);

  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzer, OUTPUT);
}

void loop() {
  //carforward();
  //delay(1000);
  //carturnleft();
  //delay(1000);
  //carturnright();
  //delay(1000);


  leftV = digitalRead(left);
  centerV = digitalRead(center);
  rightV = digitalRead(right);

  Serial.print(leftV);
  Serial.print(" ");
  Serial.print(centerV);
  Serial.print(" ");
  Serial.println(rightV);
  // Triggering the Ultrasonic Wave :
  digitalWrite(trigPin, LOW);//     |
  delayMicroseconds(2);//           |
  digitalWrite(trigPin, HIGH);//    |
  delayMicroseconds(10);//          |
  digitalWrite(trigPin, LOW);//     |
  //---------------------------------
  //Sensing Ultrasonic Wave on echo Pin and callculating the duration
  duration = pulseIn(echoPin, HIGH);

  //Calculate the distance (in cm) based on the speed of sound.
  distance = duration / 58.2;
  // Serial.println(distance);
  // If object in range of attack start Attacking:
  // Conrolling car direction based on Infra-Red Sensors:
  //Serial.println(rightV);
  if (distance <= maximumRange) {
    carStop();
    digitalWrite(buzzer, HIGH);
  }
  else {
    LineFollowing();
    digitalWrite(buzzer, LOW);
  }

}//end of LOOP
void LineFollowing() {
  if (leftV == 0 && centerV == 1 && rightV == 0) {
    carforward();
    Serial.println("forward");
    delay(200);
  }
  else if (leftV == 1 && centerV == 1 &&  rightV == 1) {
    carStop();
    delay(5000);
  }
  else if (leftV == 1 && centerV == 0 && rightV == 0) {
    carturnleft();
    Serial.println("Left");
    delay(200);
  }
  else if (leftV == 1 && centerV == 1 &&  rightV == 0) {
    carturnleft();
    Serial.println("Left");
    delay(200);
  } else if (leftV == 0 && centerV == 0 && rightV == 0) {
    carturnright();
    delay(200);
  } else if (leftV == 0 && centerV == 1  && rightV == 1) {
    carturnright();
    Serial.println("Right");
    delay(200);
  }
  else if (leftV == 0 && centerV == 0 && rightV == 1) {
    carturnright();
    Serial.println("Right");
    delay(200);
  }
}



void carforward() {
  analogWrite(ENA, Speed);
  analogWrite(ENB, Speed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void carturnright() {
  analogWrite(ENA, Speed + 40);
  analogWrite(ENB, Speed + 40);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void carturnleft() {
  analogWrite(ENA, Speed + 40);
  analogWrite(ENB, Speed + 40);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void carStop() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void carforwardFast() {
  analogWrite(ENA, Speed + 40);
  analogWrite(ENB, Speed + 40);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}