#include "BluetoothSerial.h"
BluetoothSerial TJ9_ESP32_BT;

int rxValue;
int F_LedState;
int B_LedState;
int L_LedState;
int R_LedState;
int LedState = 0;
int F_LedPin = 4;
int B_LedPin = 2;
int L_LedPin = 5;
int R_LedPin = 18;
int M_IN1 = 23;
int M_IN2 = 22;
int M_IN3 = 21;
int M_IN4 = 19;
unsigned long previousMillis = 0;
unsigned long interval = 200;

void setup() {
  Serial.begin(115200);
  TJ9_BT.begin("TJ9_ESP32_BT");
  Serial.println("Bluetooth JT9 Control");
  pinMode(F_LedPin, OUTPUT); digitalWrite(F_LedPin, LOW);
  pinMode(B_LedPin, OUTPUT); digitalWrite(B_LedPin, LOW);
  pinMode(L_LedPin, OUTPUT); digitalWrite(L_LedPin, LOW);
  pinMode(R_LedPin, OUTPUT); digitalWrite(R_LedPin, LOW);
  pinMode(M_IN1, OUTPUT); digitalWrite(M_IN1, LOW);
  pinMode(M_IN2, OUTPUT); digitalWrite(M_IN2, LOW);
  pinMode(M_IN3, OUTPUT); digitalWrite(M_IN3, LOW);
  pinMode(M_IN4, OUTPUT); digitalWrite(M_IN4, LOW);
}

void Forward() {
  digitalWrite(M_IN1, HIGH);
  digitalWrite(M_IN2,  LOW);
  digitalWrite(M_IN3, HIGH);
  digitalWrite(M_IN4,  LOW);
  F_LedState = 1;
  B_LedState = 0;
  L_LedState = 0;
  R_LedState = 0;
}

void Backward() {
  digitalWrite(M_IN1,  LOW);
  digitalWrite(M_IN2, HIGH);
  digitalWrite(M_IN3,  LOW);
  digitalWrite(M_IN4, HIGH);
  F_LedState = 0;
  B_LedState = 1;
  L_LedState = 0;
  R_LedState = 0;
}

void Turnleft() {
  digitalWrite(M_IN1, HIGH);
  digitalWrite(M_IN2,  LOW);
  digitalWrite(M_IN3,  LOW);
  digitalWrite(M_IN4, HIGH);
  F_LedState = 0;
  B_LedState = 0;
  L_LedState = 1;
  R_LedState = 0;
}

void Turnright() {
  digitalWrite(M_IN1,  LOW);
  digitalWrite(M_IN2, HIGH);
  digitalWrite(M_IN3, HIGH);
  digitalWrite(M_IN4,  LOW);
  F_LedState = 0;
  B_LedState = 0;
  L_LedState = 0;
  R_LedState = 1;
}

void Stop() {
  digitalWrite(M_IN1, LOW);
  digitalWrite(M_IN2, LOW);
  digitalWrite(M_IN3, LOW);
  digitalWrite(M_IN4, LOW);
  F_LedState = 0;
  B_LedState = 0;
  L_LedState = 0;
  R_LedState = 0;
}

void loop() {
  if (TJ9_ATOM_BT.available()) {
    rxValue = TJ9_ESP32_BT.read();
    //Serial.print("Received:"); Serial.println(rxValue);
    switch (rxValue) {
      case 'F': //Forward
        Forward();
        break;
      case 'B': //Backward
        Backward();
        break;
      case 'L': //Turnleft
        Turnleft();
        break;
      case 'R': //Turnleft
        Turnright();
        break;
      case 'S': //Stop
        Stop();
        break;
      default:
        break;
    }
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis > interval) { //blinks
    if (LedState == 0) {
      LedState = 1;
    }
    else {
      LedState = 0;
    }
    previousMillis = currentMillis;
  }

  if (F_LedState == 1) {
    if (LedState == 0) {
      digitalWrite(F_LedPin, HIGH);
    } else {
      digitalWrite(F_LedPin, LOW);
    }
  } else {
    digitalWrite(F_LedPin, LOW);
  }

  if (B_LedState == 1) {
    if (LedState == 0) {
      digitalWrite(B_LedPin, HIGH);
    } else {
      digitalWrite(B_LedPin, LOW);
    }
  } else {
    digitalWrite(B_LedPin, LOW);
  }

  if (L_LedState == 1) {
    if (LedState == 0) {
      digitalWrite(L_LedPin, HIGH);
    } else {
      digitalWrite(L_LedPin, LOW);
    }
  } else {
    digitalWrite(L_LedPin, LOW);
  }

  if (R_LedState == 1) {
    if (LedState == 0) {
      digitalWrite(R_LedPin, HIGH);
    } else {
      digitalWrite(R_LedPin, LOW);
    }
  } else {
    digitalWrite(R_LedPin, LOW);
  }
  delay(20);
}