#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

int forwardPin1 = 13;
int backwardPin1 = 12;
int forwardPin2 = 14;
int backwardPin2 = 27;
int trig = 26;
int echo = 25;
int led = 5;
int freq = 5000;
int resolution = 8;
char command = 'S';
float duration, distance;

BLECharacteristic *pCharacteristic;

class MyCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String value = pCharacteristic->getValue().c_str();
    if (value.length() > 0) {
      command = value.charAt(0);
    }
  }
};

class ServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {}
  void onDisconnect(BLEServer* pServer) {
    BLEDevice::getAdvertising()->start();
  }
};

void setup() {
  Serial.begin(115200);
  int pins[] = {forwardPin1, backwardPin1, forwardPin2, backwardPin2, trig};
  for (int i = 0; i < 5; i++) {
    pinMode(pins[i], OUTPUT);
  }
  pinMode(echo, INPUT);
  ledcAttach(led, freq, resolution);

  BLEDevice::init("ESP32-Robot-BLE");
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());
  BLEService *pService = pServer->createService("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
  pCharacteristic = pService->createCharacteristic(
    "beb5483e-36e1-4688-b7f5-ea07361b26a8",
    BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE
  );
  pCharacteristic->setCallbacks(new MyCallbacks());
  pCharacteristic->setValue("S");
  pService->start();
  BLEDevice::getAdvertising()->start();
}

void loop() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH, 30000);
  if (duration == 0) {
    distance = 999;
  } else {
    distance = (duration * 0.0343) / 2;
  }

  if (distance <= 50 && command == 'F') {
    digitalWrite(forwardPin1, LOW);
    digitalWrite(backwardPin1, LOW);
    digitalWrite(forwardPin2, LOW);
    digitalWrite(backwardPin2, LOW);
    for (int b = 0; b <= 255; b++) {
      ledcWrite(led, b);
      delay(1);
    }
    for (int b = 255; b >= 0; b--) {
      ledcWrite(led, b);
      delay(1);
    }
  } else {
    if (command == 'F') {
      digitalWrite(forwardPin1, HIGH);
      digitalWrite(backwardPin1, LOW);
      digitalWrite(forwardPin2, HIGH);
      digitalWrite(backwardPin2, LOW);
    } else if (command == 'B') {
      digitalWrite(forwardPin1, LOW);
      digitalWrite(backwardPin1, HIGH);
      digitalWrite(forwardPin2, LOW);
      digitalWrite(backwardPin2, HIGH);
    } else if (command == 'L') {
      digitalWrite(forwardPin1, LOW);
      digitalWrite(backwardPin1, HIGH);
      digitalWrite(forwardPin2, HIGH);
      digitalWrite(backwardPin2, LOW);
    } else if (command == 'R') {
      digitalWrite(forwardPin1, HIGH);
      digitalWrite(backwardPin1, LOW);
      digitalWrite(forwardPin2, LOW);
      digitalWrite(backwardPin2, HIGH);
    } else {
      digitalWrite(forwardPin1, LOW);
      digitalWrite(backwardPin1, LOW);
      digitalWrite(forwardPin2, LOW);
      digitalWrite(backwardPin2, LOW);
    }
    ledcWrite(led, 0);
  }

  delay(100);
}