#include <SoftwareSerial.h>

// Connect HC-05 TX to Arduino pin 7 (RX)
// Connect HC-05 RX to Arduino pin 6 (TX) via a voltage divider (Arduino TX is 5V, HC-05 RX is 3.3V safe)
SoftwareSerial BTSerial(7, 6); // RX, TX

void setup() {
  // Motor control pins
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);

  // Start serial communication with Bluetooth
  BTSerial.begin(9600);
  Serial.begin(9600); // Optional: for debugging via USB
}

void loop() {
  if (BTSerial.available()) {
    char cmd = BTSerial.read();
    Serial.println(cmd); // Optional: show received command in Serial Monitor

    switch (cmd) {
      case 'F': forward(); break;
      case 'B': backward(); break;
      case 'L': left(); break;
      case 'R': right(); break;
      case 'S': stopp(); break;
      //default: stopp(); break; // safety stop on unknown command
    }
  }
   if (Serial.available()) {
    char cmd = Serial.read();
    Serial.println(cmd); // Optional: show received command in Serial Monitor

    switch (cmd) {
      case 'F': forward(); break;
      case 'B': backward(); break;
      case 'L': left(); break;
      case 'R': right(); break;
      case 'S': stopp(); break;
      default: stopp(); break; // safety stop on unknown command
    }
  }
}

void stopp() {
  Serial.println("STOPPED");
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
}

void backward() {
  Serial.println("Backward");
  digitalWrite(2, HIGH);
  digitalWrite(3, LOW);
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
  delay(200);
}

void forward() {
  Serial.println("Forward");
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH);
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
  
}

void left() {
  Serial.println("Left");
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  
}

void right() {
  Serial.println("Right");
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
  
}