// NewPing - Version: Latest 
#include <NewPing.h>

// Adafruit Motor Shield library - Version: Latest 
#include <AFMotor.h>
#include <SoftwareSerial.h>

#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define MAX_DISTANCE 200

NewPing sonar(TRIGGER_PIN,ECHO_PIN,MAX_DISTANCE);
AF_DCMotor leftmotor(2);
AF_DCMotor rightmotor(1);

SoftwareSerial mySerial(10,9);

boolean prox;
int distance;

void setup() {
  leftmotor.setSpeed(250);
  rightmotor.setSpeed(200);
  
  pinMode(9, OUTPUT);
  pinMode(10, INPUT);
  mySerial.begin(9600);
  Serial.begin(9600);
  
  sendCommand("AT");
  sendCommand("AT+ROLE0");
  sendCommand("AT+UUID0xFFE0");
  sendCommand("AT+CHAR0xFFE1");
  sendCommand("AT+NAMEGNG2101_LN");
}

void sendCommand(const char * command){
  Serial.print("Command send:");
  Serial.println(command);
  mySerial.println(command);
  
  delay(100);
  
  char reply[50];
  int i = 0;
  while(mySerial.available()){
    reply[i] = mySerial.read();
    i += 1;
  }
  
  reply[i] = '\0';
  Serial.print(reply);
  Serial.println("Reply successful");
}

void sensor_read(){
  delay(250);
  distance = sonar.ping_cm();
  Serial.println(distance);
  if(distance > 10 or distance == 0){
    Serial.println("No obstruction");
    prox = true;
  } else{
    Serial.println("Obstruction");
    prox = false;
  }
}

void loop() {
  char reply[100];
  int i = 0;
  
  while(mySerial.available()){
    reply[i] = mySerial.read();
    i += 1;
  }
  
  sensor_read();
  
  reply[i] = '\0';
  
  if(strlen(reply) > 0){
    Serial.println(" pressed");
    Serial.println(reply);
    
    if(reply[0] == 'C'){
      if(prox == true){
        rightmotor.run(FORWARD);
        leftmotor.run(FORWARD);
        Serial.println(reply);
      } else{
        rightmotor.run(RELEASE);
        leftmotor.run(RELEASE);
      }
    }
    
    if(reply[0] == 'A'){
      rightmotor.run(BACKWARD);
      leftmotor.run(BACKWARD);
      Serial.println(reply);
    }
    
    if(reply[0] == 'D'){
      rightmotor.run(BACKWARD);
      leftmotor.run(FORWARD);
      delay(250);
      rightmotor.run(RELEASE);
      leftmotor.run(RELEASE);
      Serial.println(reply);
    }
    
    if(reply[0] == 'B'){
      rightmotor.run(FORWARD);
      leftmotor.run(BACKWARD);
      delay(250);
      rightmotor.run(RELEASE);
      leftmotor.run(RELEASE);
      Serial.println(reply);
    }
    
    if(reply[0] == 'E'){
    rightmotor.run(RELEASE);
    leftmotor.run(RELEASE);
    Serial.println(reply);
    }
    
  } else{
    Serial.println(" released");
    // rightmotor.run(RELEASE);
    // leftmotor.run(RELEASE);
  }
  
}