// https://forum.arduino.cc/t/my-arduino-car-moves-only-left-right-and-not-forward-backward/1430601/4

#include <SoftwareSerial.h>
SoftwareSerial BT(2, 3); // RX=pin2, TX=pin3

// Motor A (Right wheel)
int AIA = 6;
int AIB = 5;

// Motor B (Left wheel)
int BIA = 10;
int BIB = 9;

int currentSpeed = 200; // default speed

void setup() {
  Serial.begin(115200);
  BT.begin(9600);

  pinMode(AIA, OUTPUT);
  pinMode(AIB, OUTPUT);
  pinMode(BIA, OUTPUT);
  pinMode(BIB, OUTPUT);

  stop();
  Serial.println("Robot Ready!");
  Serial.println("Waiting for Bluetooth...");
}

void forward(int speed) {
  motorWrite(speed, 0, speed, 0, "Forward");
}

void backward(int speed) {
  motorWrite(0, speed, 0, speed, "Backward");
}

void turnRight(int speed) {
  motorWrite(0, speed, speed, 0, "Right");
}

void turnLeft(int speed) {
  motorWrite(speed, 0, 0, speed, "Left");
}

void stop() {
  motorWrite(0, 0, 0, 0, "Stop");
}

void motorWrite(bool aia, bool aib, bool bia, bool bib, char *text) {
  digitalWrite(AIA, aia);
  digitalWrite(AIB, aib);
  digitalWrite(BIA, bia);
  digitalWrite(BIB, bib);
  Serial.println(text);
}

void loop() {
  // ===== BT SERIAL =====
  if (BT.available()) {
    char command = BT.read();
  }

  // ===== HW SERIAL =====
  if (Serial.available()) {
    char command = Serial.read(); // https://forum.arduino.cc/t/serial-input-basics-updated/382007
    Serial.print("Command received: ");
    if (command == '\n')
      command = ' ';
    else {
      Serial.print(command);
      Serial.print(" ");
    }
    delay(50); // seems to help with discarding the CRLF
    char crlf = Serial.read(); // read and discard CRLF

    switch (command) {
      case 'f': // lower case command
      case 'F': forward(currentSpeed);   break;
      case 'b':
      case 'B': backward(currentSpeed);  break;
      case 'r':
      case 'R': turnRight(currentSpeed); break;
      case 'l':
      case 'L': turnLeft(currentSpeed);  break;
      case 's':
      case 'S': stop();                  break;

      // Speed control with number keys
      case '1': currentSpeed = 80;  Serial.println("Speed: 30%"); break;
      case '2': currentSpeed = 120; Serial.println("Speed: 47%"); break;
      case '3': currentSpeed = 160; Serial.println("Speed: 63%"); break;
      case '4': currentSpeed = 200; Serial.println("Speed: 78%"); break;
      case '5': currentSpeed = 255; Serial.println("Speed: 100%"); break;

      default: Serial.write(command); Serial.println(" Invalid"); break;
    }
  }
}