#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEClient.h>
#include <BLEScan.h>

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

static BLEAddress *serverAddress;
static boolean doConnect = false;
static boolean connected = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;

class MyClientCallback : public BLEClientCallbacks 
{
    void onConnect(BLEClient* pclient) 
    {
        Serial.println("Connected to BLE Server!");
    }

    void onDisconnect(BLEClient* pclient) 
    {
        Serial.println("Disconnected from BLE Server!");
        connected = false;
    }
};

void scanCompleteCB(BLEScanResults scanResults) 
{
    Serial.println("Scan complete!");
}

void findServer() 
{
    BLEScan* pBLEScan = BLEDevice::getScan();
    pBLEScan->setAdvertisedDeviceCallbacks(new BLEAdvertisedDeviceCallbacks());
    pBLEScan->setActiveScan(true);
    BLEScanResults foundDevices = pBLEScan->start(5);
    
    for (int i = 0; i < foundDevices.getCount(); i++) {
        BLEAdvertisedDevice device = foundDevices.getDevice(i);
        if (device.haveServiceUUID() && device.isAdvertisingService(BLEUUID(SERVICE_UUID))) 
        {
            serverAddress = new BLEAddress(device.getAddress());
            doConnect = true;
            break;
        }
    }
}

bool connectToServer() 
{
    Serial.print("Connecting to ");
    Serial.println(serverAddress->toString().c_str());

    BLEClient* pClient = BLEDevice::createClient();
    pClient->setClientCallbacks(new MyClientCallback());

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

    Serial.println("Connected to server!");
    BLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
    if (pRemoteService == nullptr) 
    {
        Serial.println("Failed to find service!");
        pClient->disconnect();
        return false;
    }

    pRemoteCharacteristic = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID);
    if (pRemoteCharacteristic == nullptr) 
    {
        Serial.println("Failed to find characteristic!");
        pClient->disconnect();
        return false;
    }

    connected = true;
    return true;
}

void setup() 
{
    Serial.begin(115200);
    Serial.println("Starting BLE client...");

    BLEDevice::init("");
    findServer();
}

void loop() 
{
    if (doConnect && !connected) 
    {
        connected = connectToServer();
        doConnect = false;
    }

    if (connected) 
    {
        std::string value = pRemoteCharacteristic->readValue();
        Serial.print("Received: ");
        Serial.println(value.c_str());
    }

    delay(2000);
}