//For DCmotor devices 
int M1A=2;
int M1B=3;
int M2A=4;
int M2B=5;
int M3A=6;
int M3B=7;
int M4A=8;
int M4B=9;

//For Bluetooth HC05
#include <SoftwareSerial.h>
int HC05_RX_PIN=11; //digital pin to receiver
int HC05_TX_PIN=10; //digital pin to transceiver
SoftwareSerial serialBT(HC05_TX_PIN, HC05_RX_PIN); //BLUETOOTH: define pins to Rx and Tx
boolean NL = true;
char str[20]={'\0'};
int counter=0;

//For Ultrasoni sensor HC-SR04
int HC_SR04_1=12;
int HC_SR04_2=16;
int HC_SR04_3=14;
int HC_SR04_4=15;

int D_front=0;
int D_right=0;
int D_back=0;
int D_left=0;
String sendstr;
boolean Running;
int Direction;//1=forward,2=back,3=lef  t,4=right

void setup() {
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  pinMode(M3A, OUTPUT);
  pinMode(M3B, OUTPUT);
  pinMode(M4A, OUTPUT);
  pinMode(M4B, OUTPUT);

  pinMode(HC_SR04_1, INPUT);
  pinMode(HC_SR04_2, INPUT);
  pinMode(HC_SR04_3, INPUT);
  pinMode(HC_SR04_4, INPUT);

  Serial.begin(9600);
  serialBT.begin(9600);
  Serial.println("Start to control Mobile robot");
  Serial.println("");
  Running=false;
}

void loop() {
  char c;
  int input;
  D_front=digitalRead(HC_SR04_1); //front
  D_left=digitalRead(HC_SR04_2);//left
  D_back=digitalRead(HC_SR04_3);//back
  D_right=digitalRead(HC_SR04_4);//right
  Serial.print(D_front);
  Serial.print(D_left);
  Serial.print(D_right);
  Serial.println(D_back);
  
 while (serialBT.available()){
  if(D_front==0 || D_back==0 || D_right==0 || D_left==0){
    c = (char)serialBT.read(); //get each char and fill string
    if(c=='a' && D_front==0){//forward
      Serial.print("forward");
      Serial.print("\n");
      //SendCharacter("a");
      Direction=1;
      Forward();
      delay(100);
      //delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
    else if(c=='b' && D_back==0){//back
      Serial.print("back");
      Serial.print("\n");
      Direction=2;
      Backward();
      //SendCharacter("b");
      delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
     else if(c=='c' && D_left==0){//left
      Serial.print("left");
      Serial.print("\n");
      Direction=3;
      Left();
      //SendCharacter("c");
      delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
     else if(c=='d' && D_right==0){//right
      Serial.print("right");
      Serial.print("\n");
      Direction=4;
      Right();
      //SendCharacter("d");
      delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
     else if(c=='e'){//rotate_right
      Serial.print("RotateR");
      Serial.print("\n");
      Rotate_R();
      //SendCharacter("e");
      delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
     else if(c=='f'){//rotate_left
      Serial.print("RotateL");
      Serial.print("\n");
      Rotate_L();
      Stop();
      //SendCharacter("f");
      delay(100);
      //Serial.print("next");
      //Serial.print("\n");
      }
      //Serial.print("running");
      //Serial.print("\n");
      delay(100);
    }
 }
//  Serial.println("Nothing");
  
  if (Direction==1 && D_front==1){Serial.println("ObstacleFront");Stop();}
  else if(Direction==2 && D_back==1){Serial.println("ObstacleBack");Stop();}
  else if(Direction==3 && D_left==1){Serial.println("ObstacleLeft");Stop();}
  else if(Direction==4 && D_right==1){Serial.println("ObstacleRight");Stop();}
  
    delay(10);   
}

void SendCharacter(String sendstr){
  for(int i=0;i < sendstr.length(); i++)
  {Serial.write(sendstr.charAt(i));
  }
  Serial.write("0");
  delay(50);
  }

void Stop(){
    digitalWrite(M1A, LOW);
    digitalWrite(M1B, LOW);
    digitalWrite(M2A, LOW);
    digitalWrite(M2B, LOW);
    digitalWrite(M3A, LOW);
    digitalWrite(M3B, LOW);
    digitalWrite(M4A, LOW);
    digitalWrite(M4B, LOW);
      Serial.print("breake");
      Serial.print("\n");
      SendCharacter("s");
  }

void Forward(){
  digitalWrite(M1A, HIGH);
  digitalWrite(M1B, LOW);
  digitalWrite(M2A, HIGH);
  digitalWrite(M2B, LOW);
  digitalWrite(M3A, HIGH);
  digitalWrite(M3B, LOW);
  digitalWrite(M4A, HIGH);
  digitalWrite(M4B, LOW);
  }

void Backward(){
  digitalWrite(M1A, LOW);
  digitalWrite(M1B, HIGH);
  digitalWrite(M2A, LOW);
  digitalWrite(M2B, HIGH);
  digitalWrite(M3A, LOW);
  digitalWrite(M3B, HIGH);
  digitalWrite(M4A, LOW);
  digitalWrite(M4B, HIGH);
  }

void Left(){
  digitalWrite(M1A, LOW);
  digitalWrite(M1B, HIGH);
  digitalWrite(M2A, HIGH);
  digitalWrite(M2B, LOW);
  digitalWrite(M3A, HIGH);
  digitalWrite(M3B, LOW);
  digitalWrite(M4A, LOW);
  digitalWrite(M4B, HIGH);
  }

void Right(){
  digitalWrite(M1A, HIGH);
  digitalWrite(M1B, LOW);
  digitalWrite(M2A, LOW);
  digitalWrite(M2B, HIGH);
  digitalWrite(M3A, LOW);
  digitalWrite(M3B, HIGH);
  digitalWrite(M4A, HIGH);
  digitalWrite(M4B, LOW);
  }

void Rotate_R(){
  digitalWrite(M1A, HIGH);
  digitalWrite(M1B, LOW);
  digitalWrite(M2A, LOW);
  digitalWrite(M2B, HIGH);
  digitalWrite(M3A, HIGH);
  digitalWrite(M3B, LOW);
  digitalWrite(M4A, LOW);
  digitalWrite(M4B, HIGH); 
  }
void Rotate_L(){
  digitalWrite(M1A, LOW);
  digitalWrite(M1B, HIGH);
  digitalWrite(M2A, HIGH);
  digitalWrite(M2B, LOW);
  digitalWrite(M3A, LOW);
  digitalWrite(M3B, HIGH);
  digitalWrite(M4A, HIGH);
  digitalWrite(M4B, LOW); 
  }