/*
 PROTOTYPE
*/


// define digital pin number 
#define PIN1 10 // back left engine CCW
#define PIN2 11 // front left engine CW
#define PIN3 12 // front right engine CW
#define PIN4 13 // back right engine CCW

#define ECHO_PIN 19
#define TRIG_PIN 20



// main
void setup() {
  Serial.begin(115200); 

  // define digital pin mode
  pinMode(PIN1,OUTPUT); 
  pinMode(PIN2,OUTPUT); 
  pinMode(PIN3,OUTPUT); 
  pinMode(PIN4,OUTPUT); 

}

void loop() {
  int safecontrol = isHazard();
  if(safecontrol > 0){
     if (safecontrol == 2){
      Stop();
      delay(5);
    }
    
  } else if(Serial.available() > 0) { 
    Stop();    
    int value = control();

    switch(value)             //run by command
    {
    case 'W': forward(); break;
    case 'S': back(); break;
    case 'A': left(); break;
    case 'D': right(); break;
    case 'X': Stop(); break; 
    }
  } else {
    Stop();
  }

}

// control function
void forward(){
  digitalWrite(PIN2,HIGH);
  digitalWrite(PIN3,HIGH);
  delay(200);
  digitalWrite(PIN2,LOW);
  digitalWrite(PIN3,LOW); 
}

void back()
{
  digitalWrite(PIN1,HIGH);
  digitalWrite(PIN4,HIGH);
  delay(200);
  digitalWrite(PIN1,LOW);
  digitalWrite(PIN4,LOW);
}

void left()
{
  digitalWrite(PIN3,HIGH);
  delay(200);
  digitalWrite(PIN3,LOW);
}

void right()
{
  digitalWrite(PIN2,HIGH);
  delay(200);
  digitalWrite(PIN2,LOW);
} 

void Stop()
{
  digitalWrite(PIN1,LOW);
  digitalWrite(PIN2,LOW);
  digitalWrite(PIN3,LOW);
  digitalWrite(PIN4,LOW);
}


// obtain control input
int control(){
  int value = Serial.read(); // obtain the command from bluetooth
  Serial.println(value); //debug
  return value;
}

//get distance data
float getDistance(){
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

// obtain sensor data
int isHazard()
{
  float dis = getDistance();
  if ( dis < 20){
    if (dis > 10){
      return 1;
    }
    return 2;  
  }
  return 0;
}