Create a Bluetooth Mesh of MATRIX Voices

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include "wishbone_bus.h"
#include "everloop.h"
#include "everloop_image.h"

namespace hal = matrix_hal;

// MATRIX LED Vars
static hal::WishboneBus wb;
static hal::Everloop everloop;
static hal::EverloopImage image;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("1feed041-ff7b-4aee-a9af-be4944e100f0");
// The characteristic of the remote service we are interested in.
static BLEUUID charUUID("1114f0e7-7f16-4660-9cdb-1557cfcdef3c");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic *remoteCharacteristic;
static BLEAdvertisedDevice *remoteDevice;
static int counter = 1;

// function to continuously report data read from remote characteristic (i.e. notify)
static void notifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify)
{
    Serial.print("Notify callback for characteristic ");
    Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
    Serial.print(" of data length ");
    Serial.println(length);
    Serial.print("data: ");
    Serial.println((char *)pData);
}

// handling connect and disconnect
class MyClientCallback : public BLEClientCallbacks
{
    void onConnect(BLEClient *clientESP32)
    {
        Serial.println("Connected");
    }

    void onDisconnect(BLEClient *clientESP32)
    {
        connected = false;
        Serial.println("Disconnected");
    }
};

bool connectToServer()
{
    Serial.print("Forming a connection to ");
    Serial.println(remoteDevice->getAddress().toString().c_str());

    //create the client and pass it to the clientESP32 variable
    BLEClient *clientESP32 = BLEDevice::createClient();
    Serial.println(" - Created client");

    clientESP32->setClientCallbacks(new MyClientCallback());

    // Connect to the remote BLE Server (i.e. the other Voice)
    clientESP32->connect(remoteDevice);
    Serial.println(" - Connected to server");

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService *remoteService = clientESP32->getService(serviceUUID);

    if (remoteService == nullptr)
    {
        Serial.print("Failed to find our service UUID: ");
        Serial.println(serviceUUID.toString().c_str());
        clientESP32->disconnect();
        return false;
    }
    Serial.println(" - Found our service");

    // Obtain a reference to the characteristic in the service of the remote BLE server.
    remoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
    if (remoteCharacteristic == nullptr)
    {
        Serial.print("Failed to find our characteristic UUID: ");
        Serial.println(charUUID.toString().c_str());
        clientESP32->disconnect();
        return false;
    }
    Serial.println(" - Found our characteristic");

    // Read the value of the characteristic.
    if (remoteCharacteristic->canRead())
    {
        std::string value = remoteCharacteristic->readValue();
        Serial.print("The characteristic value was: ");
        Serial.println(value.c_str());
    }

    if (remoteCharacteristic->canNotify())
        remoteCharacteristic->registerForNotify(notifyCallback);

    connected = true;
}

// Scan for BLE servers and find the first one that advertises the service we are looking for.
class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
{
    // Called for each advertising BLE server
    void onResult(BLEAdvertisedDevice advertisedDevice)
    {
        Serial.print("BLE Advertised Device found: ");
        Serial.println(advertisedDevice.toString().c_str());

        // once the device is found we see if it contains the service we are looking for.
        if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID))
        {
            BLEDevice::getScan()->stop();
            remoteDevice = new BLEAdvertisedDevice(advertisedDevice);
            doConnect = true;
            doScan = true;
        }
    }   
};     

void setup()
{
    //initialize the client
    Serial.begin(115200);
    Serial.println("Starting Arduino BLE Client application...");
    BLEDevice::init("Myesp32");

    // Retrieve a Scanner and set the callback we want to use to be informed when we have detected a new device.
    BLEScan *pBLEScan = BLEDevice::getScan();
    pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
    pBLEScan->setInterval(1349);
    pBLEScan->setWindow(449);
    pBLEScan->setActiveScan(true);
    pBLEScan->start(5, false);
} 

void loop()
{
    if (doConnect == true)
    {
        if (connectToServer())
        {
            Serial.println("We are now connected to the BLE Server.");
        }
        else
        {
            Serial.println("We have failed to connect to the server; there is nothin more we will do.");
        }
        doConnect = false;
    }

    if (connected)
    {
        // Set the characteristic's value to red or blue
        if (counter % 2 == 0)
        {
            remoteCharacteristic->writeValue("red", 3);
        }
        else
        {
            remoteCharacteristic->writeValue("blue", 4);
        }
    }
    else if (doScan)
    {
        BLEDevice::getScan()->start(0); // start scan after disconnect
    }
    counter++;
    delay(1000); // Delay a second between loops.
}