#include <NimBLEDevice.h>

#define SERVICE_UUID        "12345678-1234-1234-1234-1234567890ab"
#define CHARACTERISTIC_UUID "abcd1234-5678-1234-5678-abcdef123456"

static const char* targetAddress = "58:e6:c5:02:f7:c6";

NimBLEAdvertisedDevice* targetDevice = nullptr;
NimBLEClient* pClient = nullptr;
NimBLERemoteCharacteristic* pRemoteCharacteristic = nullptr;

bool doConnect = false;
bool connected = false;
unsigned long lastRead = 0;

class ScanCallbacks : public NimBLEScanCallbacks {
  void onResult(const NimBLEAdvertisedDevice* advertisedDevice) override {
    String foundAddress = advertisedDevice->getAddress().toString().c_str();

    Serial.print("Found device: ");
    Serial.print(advertisedDevice->getName().c_str());
    Serial.print(" | Address: ");
    Serial.print(foundAddress);
    Serial.print(" | RSSI: ");
    Serial.println(advertisedDevice->getRSSI());

    if (foundAddress.equalsIgnoreCase(targetAddress)) {
      Serial.println("Target BLE address found");
      NimBLEDevice::getScan()->stop();
      targetDevice = new NimBLEAdvertisedDevice(*advertisedDevice);
      doConnect = true;
    }
  }
};

bool connectToServer() {
  if (targetDevice == nullptr) {
    Serial.println("No target device stored");
    return false;
  }

  Serial.println("Connecting to BLE server...");

  pClient = NimBLEDevice::createClient();

  if (!pClient->connect(targetDevice)) {
    Serial.println("Failed to connect");
    return false;
  }

  Serial.println("Connected to BLE server");

  NimBLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
  if (pRemoteService == nullptr) {
    Serial.println("Service not found");
    pClient->disconnect();
    return false;
  }

  pRemoteCharacteristic = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID);
  if (pRemoteCharacteristic == nullptr) {
    Serial.println("Characteristic not found");
    pClient->disconnect();
    return false;
  }

  Serial.println("Characteristic found");
  connected = true;
  return true;
}

void setup() {
  Serial.begin(115200);
  delay(3000);

  Serial.println();
  Serial.println("Booting BLE client...");

  NimBLEDevice::init("");

  NimBLEScan* pScan = NimBLEDevice::getScan();
  pScan->setScanCallbacks(new ScanCallbacks(), true);
  pScan->setInterval(45);
  pScan->setWindow(15);
  pScan->setActiveScan(true);

  Serial.println("Starting BLE scan...");
  pScan->start(0, false, true);
}

void loop() {
  if (doConnect) {
    Serial.println("doConnect = true");

    if (connectToServer()) {
      Serial.println("BLE connection successful");
    } else {
      Serial.println("BLE connection failed");
      NimBLEDevice::getScan()->start(0, false, true);
    }

    doConnect = false;
  }

  if (connected && pClient && pClient->isConnected() && pRemoteCharacteristic != nullptr) {
    if (millis() - lastRead > 2000) {
      lastRead = millis();

      Serial.println("Reading characteristic...");

      if (pRemoteCharacteristic->canRead()) {
        std::string value = pRemoteCharacteristic->readValue();
        Serial.print("Received BLE data: ");
        Serial.println(value.c_str());
      } else {
        Serial.println("Characteristic is not readable");
      }
    }
  }

  if (connected && pClient && !pClient->isConnected()) {
    Serial.println("Disconnected. Restarting scan...");
    connected = false;
    pRemoteCharacteristic = nullptr;
    NimBLEDevice::getScan()->start(0, false, true);
  }

  delay(100);
}