/*I referred BLE notify from the sketch example of ESP32 BLE Arduino.*/
#include "BLEDevice.h"

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;

//Get your UUID from Online UUID Generator.
//https://www.uuidgenerator.net/version4
#define SERVICE_UUID        "********-****-****-*****-************"
#define CHARACTERISTIC_UUID "********-****-****-*****-************"
#define DEVICE_NAME         "esp32_BLE"

static BLEUUID  serviceUUID(SERVICE_UUID);
static BLEUUID  charUUID(CHARACTERISTIC_UUID);
static BLEAddress *pServerAddress;
static boolean doConnect = false;
static boolean connected = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
}

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;
}

bool connectToServer(BLEAddress pAddress) {
  Serial.print("Forming a connection to ");
  Serial.println(pAddress.toString().c_str());
  BLEClient*  pClient  = BLEDevice::createClient();
  pClient->connect(pAddress);
  BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
  Serial.print(" - Connected to server :");
  Serial.println(serviceUUID.toString().c_str());
  if (pRemoteService == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    return false;
  }
  pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  Serial.print(" - Found our characteristic UUID: ");
  Serial.println(charUUID.toString().c_str());
  if (pRemoteCharacteristic == nullptr) {
    Serial.print("Failed to find our characteristic UUID: ");
    return false;
  }
  Serial.println(" - Found our characteristic");
  pRemoteCharacteristic->registerForNotify(notifyCallback);
}

class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());
      Serial.println(advertisedDevice.getName().c_str());

      if (advertisedDevice.getName() == DEVICE_NAME) { //
        Serial.println(advertisedDevice.getAddress().toString().c_str());
        advertisedDevice.getScan()->stop();
        pServerAddress = new BLEAddress(advertisedDevice.getAddress());
        doConnect = true;
      }
    }
};

void setup() {
  Serial.begin(115200);
  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);

  BLEDevice::init(DEVICE_NAME);
  BLEScan* pBLEScan = BLEDevice::getScan();
  Serial.println("getScan");
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  Serial.println("setAdvertisedDeviceCallbacks");
  pBLEScan->setActiveScan(true);
  pBLEScan->start(10);
  Serial.println("");
  Serial.println("End of setup");
}

void loop() {
  if (doConnect == true) {
    if (connectToServer(*pServerAddress)) {
      connected = true;
    } else {
      connected = false;
    }
    doConnect = false;
  }
  if (connected) {
    std::string value = pRemoteCharacteristic->readValue();
    String strVal = value.c_str();
    char strNum = strVal.toInt();
    Serial.println(strNum);
    switch (strNum) {
      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;
    }
  }
  else {
    Serial.println("Not connected");
    doConnect = true;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis > interval) {
    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);
  }
}