// Declare constant pin numbers
const int RIGHT_FORWARD_PIN = 6;
const int RIGHT_BACK_PIN = 7;
const int LEFT_FORWARD_PIN = 10;
const int LEFT_BACK_PIN = 11;

char cmd = 's';

void setup() {
  // put your setup code here, to run once:
  pinMode(RIGHT_FORWARD_PIN, OUTPUT);
  pinMode(RIGHT_BACK_PIN , OUTPUT);
  pinMode(LEFT_FORWARD_PIN, OUTPUT);
  pinMode(LEFT_BACK_PIN, OUTPUT);

  Serial.begin(9600);
  Serial.println("Waiting for command");
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available() > 0){
    cmd = Serial.read();
    while(Serial.available()){ Serial.read(); }; // Clear serial buffer after popping first char
  }

  Serial.println("current status: " + String(cmd));// Print current status to seiral monitor
  
  if(cmd == 'f') {
    move_forward();
  }
  if(cmd == 's') {
    move_stop();
  }
  if(cmd == 'l') {
    turn_left();
    cmd = 's';
  }
  if(cmd == 'r') {
    turn_right();
    cmd = 's';
  }
  if(cmd == 'b') {
    move_back();
  }
}

void move_forward() {
  digitalWrite(RIGHT_FORWARD_PIN, HIGH);
  digitalWrite(RIGHT_BACK_PIN, LOW);
  digitalWrite(LEFT_FORWARD_PIN, HIGH);
  digitalWrite(LEFT_BACK_PIN, LOW);
}

void move_back() {
  digitalWrite(RIGHT_FORWARD_PIN, LOW);
  digitalWrite(RIGHT_BACK_PIN, HIGH);
  digitalWrite(LEFT_FORWARD_PIN, LOW);
  digitalWrite(LEFT_BACK_PIN, HIGH);
}
void turn_left() {
  digitalWrite(RIGHT_FORWARD_PIN, HIGH);
  digitalWrite(RIGHT_BACK_PIN, LOW);
  digitalWrite(LEFT_FORWARD_PIN, LOW);
  digitalWrite(LEFT_BACK_PIN, HIGH);
  delay(300);
}
void turn_right() {
  digitalWrite(RIGHT_FORWARD_PIN, LOW);
  digitalWrite(RIGHT_BACK_PIN, HIGH);
  digitalWrite(LEFT_FORWARD_PIN, HIGH);
  digitalWrite(LEFT_BACK_PIN, LOW);
  delay(300);
}

void move_stop() {
  digitalWrite(RIGHT_FORWARD_PIN, LOW);
  digitalWrite(RIGHT_BACK_PIN, LOW);
  digitalWrite(LEFT_FORWARD_PIN, LOW);
  digitalWrite(LEFT_BACK_PIN, LOW);
}