#include <SoftwareSerial.h>
#include "AFMotor.h"

#define speedPin A0

SoftwareSerial BT(A4, A5); // RX, TX (same as before)
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
uint16_t speed = 0;

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

  speed = analogRead(speedPin) >> 2;
  motor1.setSpeed(speed); // Half speed (0-255)
  motor2.setSpeed(speed);
  motor3.setSpeed(speed);
  motor4.setSpeed(speed);

  motor1.run(RELEASE);  // Motor off at start
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);

  while (!Serial) {}
  Serial.println("Bluetooth motor ready.");
  Serial.println(BT.available());
}

void loop() {
  // Serial.println("Entered loop");
  uint16_t speedNew = analogRead(speedPin) >> 2;
  if (speed != speedNew) {
    speed = speedNew;
    motor1.setSpeed(speed);
    motor2.setSpeed(speed);
    motor3.setSpeed(speed);
    motor4.setSpeed(speed);
    Serial.print("Speed: ");
    Serial.println(speed);
  }

  // if (BT.available()) {
  if (Serial.available()) {
    // char cmd = toupper(BT.read());
    char cmd = toupper(Serial.read());
    Serial.println(cmd);

    if (cmd == 'F') {          // Forward
      motor1.run(FORWARD);
      motor2.run(FORWARD);
      motor3.run(FORWARD);
      motor4.run(FORWARD);
      Serial.println("You pressed F");
    }
    else if (cmd == 'B') {     // Backward
      motor1.run(BACKWARD);
      motor2.run(BACKWARD);
      motor3.run(BACKWARD);
      motor4.run(BACKWARD);
    }
    else if (cmd == 'L') {      // Left turn
      motor1.run(BACKWARD);
      motor2.run(FORWARD);
      motor3.run(BACKWARD);
      motor4.run(FORWARD);
    }
    else if (cmd == 'R') {       // Right turn
      motor1.run(FORWARD);
      motor2.run(BACKWARD);
      motor3.run(FORWARD);
      motor4.run(BACKWARD);
    }
    else if (cmd == 'S') {     // Stop
      motor1.run(RELEASE);
      motor2.run(RELEASE);
      motor3.run(RELEASE);
      motor4.run(RELEASE);
    }
    else {
      Serial.println("Didn't get anything");
    }
  }
}