#include <SoftwareSerial.h>

int AO = 8;
int AT = 11;
int AS = 9;
int BO = 12;
int BT = 13;
int BS = 10;

int duration;
int distance;

const int buzzerPin = 6;

const int trigPin = 4;
const int echoPin = 5;

int spead = 254;

byte s = 0;
SoftwareSerial BLE (2, 3); //Purple TX (BLE) to pin 2
                           //Yellow RX (BLE) on pin 3

void setup() {
  Serial.begin(9600);
  BLE.begin(9600);
  pinMode(AO, OUTPUT);
  pinMode(AT, OUTPUT);
  pinMode(AS, OUTPUT);
  pinMode(BO, OUTPUT);
  pinMode(BT, OUTPUT);
  pinMode(BS, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  Serial.println(distancecm());
  if (BLE.available() > 0) {
    char c = BLE.read();
    Serial.println(c);
    switch(c) {
      case 'A':
      forward();
      break;
      case 'B':
      right();
      break;
      case 'C':
      back();
      break;
      case 'D':
      left();
      break;
      case 'a':
      forward();
      break;
      case 'b':
      right();
      break;
      case 'c':
      back();
      break;
      case 'd':
      left();
      break;
      case 'H':
      buzz();
      break;
      case 'h':
      buzz();
      break;
      default:
      finish();
    }
  }
}

int distancecm() {
  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(5); // Added this line
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  return distance;
}
void forward() {
  int cm = distancecm();
  if (30 > cm && cm > 0) {
    Serial.println(cm);
    tone(buzzerPin, 600, 350);
    finish();
    return;
  }
  analogWrite(AS, spead);
  analogWrite(BS, spead);
  digitalWrite(AO, HIGH);
  digitalWrite(AT, LOW);
  digitalWrite(BO, LOW);
  digitalWrite(BT, HIGH);
}

void back() {
  analogWrite(AS, spead);
  analogWrite(BS, spead);
  digitalWrite(AO, LOW);
  digitalWrite(AT, HIGH);
  digitalWrite(BO, HIGH);
  digitalWrite(BT, LOW);

}

void left() {
  analogWrite(AS, spead);
  analogWrite(BS, spead);
  digitalWrite(AO, HIGH);
  digitalWrite(AT, LOW);
  digitalWrite(BO, HIGH);
  digitalWrite(BT, LOW);
}

void right() {
  analogWrite(AS, spead);
  analogWrite(BS, spead);
  digitalWrite(AO, LOW);
  digitalWrite(AT, HIGH);
  digitalWrite(BO, LOW);
  digitalWrite(BT, HIGH);
}

void finish() {
  digitalWrite(AO, LOW);
  digitalWrite(AT, LOW);
  digitalWrite(BO, LOW);
  digitalWrite(BT, LOW);
}

void buzz() {
  tone(buzzerPin, 400, 500);
}