const int controlPin1 = 2;
const int controlPin2 = 3;
const int controlPin3 = 10;
const int controlPin4 = 12;
const int enablePin1 = 9;
const int enablePin2 = 11;

int ledRed = 7;
int ledBlue =13;

const int buzzerPin = 8;
int fre;

const int potPin = A0;
int motorSpeed = 0;

int trig = 5;
int echo = 4;

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  Serial.begin(9600);

  pinMode(controlPin1, OUTPUT);
  pinMode(controlPin2, OUTPUT);
  pinMode(enablePin1, OUTPUT);
  pinMode(controlPin3, OUTPUT);
  pinMode(controlPin4, OUTPUT);
  pinMode(enablePin2, OUTPUT);

  pinMode(ledRed, OUTPUT);
  pinMode(ledBlue, OUTPUT);

  digitalWrite(enablePin1, LOW);
  digitalWrite(enablePin2, LOW);

  pinMode(buzzerPin,OUTPUT);
  siren();
  flash();
}

void forward() {
  digitalWrite(controlPin1, HIGH);
  digitalWrite(controlPin2, LOW);
  digitalWrite(controlPin3, HIGH);
  digitalWrite(controlPin4, LOW);
}

void backward() {
  digitalWrite(controlPin1, LOW);
  digitalWrite(controlPin2, HIGH);
  digitalWrite(controlPin3, LOW);
  digitalWrite(controlPin4, HIGH);
}

void right() {
  digitalWrite(controlPin1, HIGH);
  digitalWrite(controlPin2, LOW);
  digitalWrite(controlPin3, LOW);
  digitalWrite(controlPin4, LOW);
}

void left() {
  digitalWrite(controlPin1, LOW);
  digitalWrite(controlPin2, LOW);
  digitalWrite(controlPin3, HIGH);
  digitalWrite(controlPin4, LOW);
}

void stopMotors() {
  digitalWrite(controlPin1, LOW);
  digitalWrite(controlPin2, LOW);
  digitalWrite(controlPin3, LOW);
  digitalWrite(controlPin4, LOW);
}

void motorEnable() {
  motorSpeed = analogRead(potPin) / 4;
  analogWrite(enablePin1, motorSpeed);
  analogWrite(enablePin2, motorSpeed);
  //Serial.println(motorSpeed);
}

void flash(){
  digitalWrite(ledRed, HIGH);
  digitalWrite(ledBlue, LOW);
  delay(200); 
  digitalWrite(ledRed, LOW);
  digitalWrite(ledBlue, HIGH);
  delay(200); 
}
void siren(){
  for(int i = 200;i <= 500;i++)   //frequence loop from 200 to 800
  {
    tone(8,i);   //in pin7 generate a tone,it frequence is i
    delay(7);    //wait for 5 milliseconds   
  }
  delay(500);   //wait for 4 seconds on highest frequence
  for(int i = 500;i >= 200;i--)  //frequence loop from 800 downto 200
  {
    tone(8,i);  //in pin7 generate a tone,it frequence is i
    delay(12);  //delay 10ms
  }
  noTone(8);

}
void loop() {
  long duration, inches;
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  inches = duration / 74 / 2;
  Serial.println(inches);
  motorEnable();

  if (inches > 6){
    backward();
    flash();
    //siren();
   }
    
  else if ((inches < 6) && (inches >= 4)){
    Serial.println("left");
    left();
  }
  else if ((inches < 4) && (inches >=2)){
    Serial.println("right");
    right();
  } 
}