Merge commit 'b232e7f5f0e87f36afbc2f4e03a2c49c48dd47bc'

59 files changed, 2211 insertions, 1480 deletions

diff --git a/sensor/patchedBLE/examples/BLE_client/BLE_client.ino b/sensor/patchedBLE/examples/BLE_client/BLE_client.ino
index c0b6163..4c58299 100644
--- a/sensor/patchedBLE/examples/BLE_client/BLE_client.ino
+++ b/sensor/patchedBLE/examples/BLE_client/BLE_client.ino
@@ -1,19 +1,23 @@
 /**
  * A BLE client example that is rich in capabilities.
+ * There is a lot new capabilities implemented.
+ * author unknown
+ * updated by chegewara
  */
 
 #include "BLEDevice.h"
 //#include "BLEScan.h"
 
 // The remote service we wish to connect to.
-static BLEUUID serviceUUID("91bad492-b950-4226-aa2b-4ede9fa42f59");
+static BLEUUID serviceUUID("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
 // The characteristic of the remote service we are interested in.
-static BLEUUID    charUUID("0d563a58-196a-48ce-ace2-dfec78acc814");
+static BLEUUID    charUUID("beb5483e-36e1-4688-b7f5-ea07361b26a8");
 
-static BLEAddress *pServerAddress;
 static boolean doConnect = false;
 static boolean connected = false;
+static boolean doScan = false;
 static BLERemoteCharacteristic* pRemoteCharacteristic;
+static BLEAdvertisedDevice* myDevice;
 
 static void notifyCallback(
   BLERemoteCharacteristic* pBLERemoteCharacteristic,
@@ -24,17 +28,31 @@ static void notifyCallback(
     Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
     Serial.print(" of data length ");
     Serial.println(length);
+    Serial.print("data: ");
+    Serial.println((char*)pData);
 }
 
-bool connectToServer(BLEAddress pAddress) {
+class MyClientCallback : public BLEClientCallbacks {
+  void onConnect(BLEClient* pclient) {
+  }
+
+  void onDisconnect(BLEClient* pclient) {
+    connected = false;
+    Serial.println("onDisconnect");
+  }
+};
+
+bool connectToServer() {
     Serial.print("Forming a connection to ");
-    Serial.println(pAddress.toString().c_str());
+    Serial.println(myDevice->getAddress().toString().c_str());
     
     BLEClient*  pClient  = BLEDevice::createClient();
     Serial.println(" - Created client");
 
+    pClient->setClientCallbacks(new MyClientCallback());
+
     // Connect to the remove BLE Server.
-    pClient->connect(pAddress);
+    pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
     Serial.println(" - Connected to server");
 
     // Obtain a reference to the service we are after in the remote BLE server.
@@ -42,6 +60,7 @@ bool connectToServer(BLEAddress pAddress) {
     if (pRemoteService == nullptr) {
       Serial.print("Failed to find our service UUID: ");
       Serial.println(serviceUUID.toString().c_str());
+      pClient->disconnect();
       return false;
     }
     Serial.println(" - Found our service");
@@ -52,16 +71,22 @@ bool connectToServer(BLEAddress pAddress) {
     if (pRemoteCharacteristic == nullptr) {
       Serial.print("Failed to find our characteristic UUID: ");
       Serial.println(charUUID.toString().c_str());
+      pClient->disconnect();
       return false;
     }
     Serial.println(" - Found our characteristic");
 
     // Read the value of the characteristic.
-    std::string value = pRemoteCharacteristic->readValue();
-    Serial.print("The characteristic value was: ");
-    Serial.println(value.c_str());
+    if(pRemoteCharacteristic->canRead()) {
+      std::string value = pRemoteCharacteristic->readValue();
+      Serial.print("The characteristic value was: ");
+      Serial.println(value.c_str());
+    }
 
-    pRemoteCharacteristic->registerForNotify(notifyCallback);
+    if(pRemoteCharacteristic->canNotify())
+      pRemoteCharacteristic->registerForNotify(notifyCallback);
+
+    connected = true;
 }
 /**
  * Scan for BLE servers and find the first one that advertises the service we are looking for.
@@ -75,14 +100,12 @@ class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
     Serial.println(advertisedDevice.toString().c_str());
 
     // We have found a device, let us now see if it contains the service we are looking for.
-    if (advertisedDevice.haveServiceUUID() && advertisedDevice.getServiceUUID().equals(serviceUUID)) {
-
-      // 
-      Serial.print("Found our device!  address: "); 
-      advertisedDevice.getScan()->stop();
+    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {
 
-      pServerAddress = new BLEAddress(advertisedDevice.getAddress());
+      BLEDevice::getScan()->stop();
+      myDevice = new BLEAdvertisedDevice(advertisedDevice);
       doConnect = true;
+      doScan = true;
 
     } // Found our server
   } // onResult
@@ -96,11 +119,13 @@ void setup() {
 
   // Retrieve a Scanner and set the callback we want to use to be informed when we
   // have detected a new device.  Specify that we want active scanning and start the
-  // scan to run for 30 seconds.
+  // scan to run for 5 seconds.
   BLEScan* pBLEScan = BLEDevice::getScan();
   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
+  pBLEScan->setInterval(1349);
+  pBLEScan->setWindow(449);
   pBLEScan->setActiveScan(true);
-  pBLEScan->start(30);
+  pBLEScan->start(5, false);
 } // End of setup.
 
 
@@ -111,9 +136,8 @@ void loop() {
   // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
   // connected we set the connected flag to be true.
   if (doConnect == true) {
-    if (connectToServer(*pServerAddress)) {
+    if (connectToServer()) {
       Serial.println("We are now connected to the BLE Server.");
-      connected = true;
     } else {
       Serial.println("We have failed to connect to the server; there is nothin more we will do.");
     }
@@ -128,6 +152,8 @@ void loop() {
     
     // Set the characteristic's value to be the array of bytes that is actually a string.
     pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
+  }else if(doScan){
+    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
   }
   
   delay(1000); // Delay a second between loops.
diff --git a/sensor/patchedBLE/examples/BLE_iBeacon/BLE_iBeacon.ino b/sensor/patchedBLE/examples/BLE_iBeacon/BLE_iBeacon.ino
index 5f6ed00..e43174d 100644
--- a/sensor/patchedBLE/examples/BLE_iBeacon/BLE_iBeacon.ino
+++ b/sensor/patchedBLE/examples/BLE_iBeacon/BLE_iBeacon.ino
@@ -18,7 +18,6 @@
 #include "sys/time.h"
 
 #include "BLEDevice.h"
-#include "BLEServer.h"
 #include "BLEUtils.h"
 #include "BLEBeacon.h"
 #include "esp_sleep.h"
@@ -85,9 +84,9 @@ void setup() {
   BLEDevice::init("");
 
   // Create the BLE Server
-  BLEServer *pServer = BLEDevice::createServer();
+  // BLEServer *pServer = BLEDevice::createServer(); // <-- no longer required to instantiate BLEServer, less flash and ram usage
 
-  pAdvertising = pServer->getAdvertising();
+  pAdvertising = BLEDevice::getAdvertising();
   
   setBeacon();
    // Start advertising
diff --git a/sensor/patchedBLE/examples/BLE_notify/BLE_notify.ino b/sensor/patchedBLE/examples/BLE_notify/BLE_notify.ino
index 5e915be..42b9e72 100644
--- a/sensor/patchedBLE/examples/BLE_notify/BLE_notify.ino
+++ b/sensor/patchedBLE/examples/BLE_notify/BLE_notify.ino
@@ -2,6 +2,7 @@
     Video: https://www.youtube.com/watch?v=oCMOYS71NIU
     Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleNotify.cpp
     Ported to Arduino ESP32 by Evandro Copercini
+    updated by chegewara
 
    Create a BLE server that, once we receive a connection, will send periodic notifications.
    The service advertises itself as: 4fafc201-1fb5-459e-8fcc-c5c9c331914b
@@ -27,7 +28,7 @@ BLEServer* pServer = NULL;
 BLECharacteristic* pCharacteristic = NULL;
 bool deviceConnected = false;
 bool oldDeviceConnected = false;
-uint8_t value = 0;
+uint32_t value = 0;
 
 // See the following for generating UUIDs:
 // https://www.uuidgenerator.net/
@@ -52,7 +53,7 @@ void setup() {
   Serial.begin(115200);
 
   // Create the BLE Device
-  BLEDevice::init("MyESP32");
+  BLEDevice::init("ESP32");
 
   // Create the BLE Server
   pServer = BLEDevice::createServer();
@@ -78,17 +79,21 @@ void setup() {
   pService->start();
 
   // Start advertising
-  pServer->getAdvertising()->start();
+  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
+  pAdvertising->addServiceUUID(SERVICE_UUID);
+  pAdvertising->setScanResponse(false);
+  pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
+  BLEDevice::startAdvertising();
   Serial.println("Waiting a client connection to notify...");
 }
 
 void loop() {
     // notify changed value
     if (deviceConnected) {
-        pCharacteristic->setValue(&value, 1);
+        pCharacteristic->setValue((uint8_t*)&value, 4);
         pCharacteristic->notify();
         value++;
-        delay(10); // bluetooth stack will go into congestion, if too many packets are sent
+        delay(3); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
     }
     // disconnecting
     if (!deviceConnected && oldDeviceConnected) {
diff --git a/sensor/patchedBLE/examples/BLE_scan/BLE_scan.ino b/sensor/patchedBLE/examples/BLE_scan/BLE_scan.ino
index ef7d892..094f793 100644
--- a/sensor/patchedBLE/examples/BLE_scan/BLE_scan.ino
+++ b/sensor/patchedBLE/examples/BLE_scan/BLE_scan.ino
@@ -8,7 +8,8 @@
 #include <BLEScan.h>
 #include <BLEAdvertisedDevice.h>
 
-int scanTime = 30; //In seconds
+int scanTime = 5; //In seconds
+BLEScan* pBLEScan;
 
 class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
     void onResult(BLEAdvertisedDevice advertisedDevice) {
@@ -21,16 +22,19 @@ void setup() {
   Serial.println("Scanning...");
 
   BLEDevice::init("");
-  BLEScan* pBLEScan = BLEDevice::getScan(); //create new scan
+  pBLEScan = BLEDevice::getScan(); //create new scan
   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
   pBLEScan->setActiveScan(true); //active scan uses more power, but get results faster
-  BLEScanResults foundDevices = pBLEScan->start(scanTime);
-  Serial.print("Devices found: ");
-  Serial.println(foundDevices.getCount());
-  Serial.println("Scan done!");
+  pBLEScan->setInterval(100);
+  pBLEScan->setWindow(99);  // less or equal setInterval value
 }
 
 void loop() {
   // put your main code here, to run repeatedly:
+  BLEScanResults foundDevices = pBLEScan->start(scanTime, false);
+  Serial.print("Devices found: ");
+  Serial.println(foundDevices.getCount());
+  Serial.println("Scan done!");
+  pBLEScan->clearResults();   // delete results fromBLEScan buffer to release memory
   delay(2000);
 }
\ No newline at end of file
diff --git a/sensor/patchedBLE/examples/BLE_server/BLE_server.ino b/sensor/patchedBLE/examples/BLE_server/BLE_server.ino
index 38224a6..3f9176a 100644
--- a/sensor/patchedBLE/examples/BLE_server/BLE_server.ino
+++ b/sensor/patchedBLE/examples/BLE_server/BLE_server.ino
@@ -1,6 +1,7 @@
 /*
     Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleServer.cpp
     Ported to Arduino ESP32 by Evandro Copercini
+    updates by chegewara
 */
 
 #include <BLEDevice.h>
@@ -17,7 +18,7 @@ void setup() {
   Serial.begin(115200);
   Serial.println("Starting BLE work!");
 
-  BLEDevice::init("MyESP32");
+  BLEDevice::init("Long name works now");
   BLEServer *pServer = BLEDevice::createServer();
   BLEService *pService = pServer->createService(SERVICE_UUID);
   BLECharacteristic *pCharacteristic = pService->createCharacteristic(
@@ -28,8 +29,13 @@ void setup() {
 
   pCharacteristic->setValue("Hello World says Neil");
   pService->start();
-  BLEAdvertising *pAdvertising = pServer->getAdvertising();
-  pAdvertising->start();
+  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
+  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
+  pAdvertising->addServiceUUID(SERVICE_UUID);
+  pAdvertising->setScanResponse(true);
+  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
+  pAdvertising->setMinPreferred(0x12);
+  BLEDevice::startAdvertising();
   Serial.println("Characteristic defined! Now you can read it in your phone!");
 }
 
diff --git a/sensor/patchedBLE/examples/BLE_server_multiconnect/BLE_server_multiconnect.ino b/sensor/patchedBLE/examples/BLE_server_multiconnect/BLE_server_multiconnect.ino
new file mode 100644
index 0000000..90704ef
--- /dev/null
+++ b/sensor/patchedBLE/examples/BLE_server_multiconnect/BLE_server_multiconnect.ino
@@ -0,0 +1,111 @@
+/*
+    Video: https://www.youtube.com/watch?v=oCMOYS71NIU
+    Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleNotify.cpp
+    Ported to Arduino ESP32 by Evandro Copercini
+    updated by chegewara
+
+   Create a BLE server that, once we receive a connection, will send periodic notifications.
+   The service advertises itself as: 4fafc201-1fb5-459e-8fcc-c5c9c331914b
+   And has a characteristic of: beb5483e-36e1-4688-b7f5-ea07361b26a8
+
+   The design of creating the BLE server is:
+   1. Create a BLE Server
+   2. Create a BLE Service
+   3. Create a BLE Characteristic on the Service
+   4. Create a BLE Descriptor on the characteristic
+   5. Start the service.
+   6. Start advertising.
+
+   A connect hander associated with the server starts a background task that performs notification
+   every couple of seconds.
+*/
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+#include <BLE2902.h>
+
+BLEServer* pServer = NULL;
+BLECharacteristic* pCharacteristic = NULL;
+bool deviceConnected = false;
+bool oldDeviceConnected = false;
+uint32_t value = 0;
+
+// See the following for generating UUIDs:
+// https://www.uuidgenerator.net/
+
+#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
+#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
+
+
+class MyServerCallbacks: public BLEServerCallbacks {
+    void onConnect(BLEServer* pServer) {
+      deviceConnected = true;
+      BLEDevice::startAdvertising();
+    };
+
+    void onDisconnect(BLEServer* pServer) {
+      deviceConnected = false;
+    }
+};
+
+
+
+void setup() {
+  Serial.begin(115200);
+
+  // Create the BLE Device
+  BLEDevice::init("ESP32");
+
+  // Create the BLE Server
+  pServer = BLEDevice::createServer();
+  pServer->setCallbacks(new MyServerCallbacks());
+
+  // Create the BLE Service
+  BLEService *pService = pServer->createService(SERVICE_UUID);
+
+  // Create a BLE Characteristic
+  pCharacteristic = pService->createCharacteristic(
+                      CHARACTERISTIC_UUID,
+                      BLECharacteristic::PROPERTY_READ   |
+                      BLECharacteristic::PROPERTY_WRITE  |
+                      BLECharacteristic::PROPERTY_NOTIFY |
+                      BLECharacteristic::PROPERTY_INDICATE
+                    );
+
+  // https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
+  // Create a BLE Descriptor
+  pCharacteristic->addDescriptor(new BLE2902());
+
+  // Start the service
+  pService->start();
+
+  // Start advertising
+  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
+  pAdvertising->addServiceUUID(SERVICE_UUID);
+  pAdvertising->setScanResponse(false);
+  pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
+  BLEDevice::startAdvertising();
+  Serial.println("Waiting a client connection to notify...");
+}
+
+void loop() {
+    // notify changed value
+    if (deviceConnected) {
+        pCharacteristic->setValue((uint8_t*)&value, 4);
+        pCharacteristic->notify();
+        value++;
+        delay(10); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
+    }
+    // disconnecting
+    if (!deviceConnected && oldDeviceConnected) {
+        delay(500); // give the bluetooth stack the chance to get things ready
+        pServer->startAdvertising(); // restart advertising
+        Serial.println("start advertising");
+        oldDeviceConnected = deviceConnected;
+    }
+    // connecting
+    if (deviceConnected && !oldDeviceConnected) {
+        // do stuff here on connecting
+        oldDeviceConnected = deviceConnected;
+    }
+}
diff --git a/sensor/patchedBLE/library.properties b/sensor/patchedBLE/library.properties
index 23e642e..8c2a019 100644
--- a/sensor/patchedBLE/library.properties
+++ b/sensor/patchedBLE/library.properties
@@ -1,10 +1,10 @@
 name=ESP32 BLE Arduino
-version=0.4.16
+version=1.0.1
 author=Neil Kolban <kolban1@kolban.com>
-maintainer=Neil Kolban <kolban1@kolban.com>
+maintainer=Dariusz Krempa <esp32@esp32.eu.org>
 sentence=BLE functions for ESP32
 paragraph=This library provides an implementation Bluetooth Low Energy support for the ESP32 using the Arduino platform. 
 category=Communication
 url=https://github.com/nkolban/ESP32_BLE_Arduino
 architectures=esp32
-includes=BLE.h, BLEUtils.h, BLEScan.h, BLEAdvertisedDevice.h
\ No newline at end of file
+includes=BLEDevice.h, BLEUtils.h, BLEScan.h, BLEAdvertisedDevice.h
diff --git a/sensor/patchedBLE/src/BLE2902.cpp b/sensor/patchedBLE/src/BLE2902.cpp
index 8984da8..23d9c77 100644
--- a/sensor/patchedBLE/src/BLE2902.cpp
+++ b/sensor/patchedBLE/src/BLE2902.cpp
@@ -15,7 +15,7 @@
 #include "BLE2902.h"
 
 BLE2902::BLE2902() : BLEDescriptor(BLEUUID((uint16_t) 0x2902)) {
-	uint8_t data[2] = {0,0};
+	uint8_t data[2] = { 0, 0 };
 	setValue(data, 2);
 } // BLE2902
 
@@ -44,11 +44,8 @@ bool BLE2902::getIndications() {
  */
 void BLE2902::setIndications(bool flag) {
 	uint8_t *pValue = getValue();
-	if (flag) {
-		pValue[0] |= 1<<1;
-	} else {
-		pValue[0] &= ~(1<<1);
-	}
+	if (flag) pValue[0] |= 1 << 1;
+	else pValue[0] &= ~(1 << 1);
 } // setIndications
 
 
@@ -58,12 +55,8 @@ void BLE2902::setIndications(bool flag) {
  */
 void BLE2902::setNotifications(bool flag) {
 	uint8_t *pValue = getValue();
-	if (flag) {
-		pValue[0] |= 1<<0;
-	} else {
-		pValue[0] &= ~(1<<0);
-	}
+	if (flag) pValue[0] |= 1 << 0;
+	else pValue[0] &= ~(1 << 0);
 } // setNotifications
 
-
 #endif
diff --git a/sensor/patchedBLE/src/BLE2904.cpp b/sensor/patchedBLE/src/BLE2904.cpp
index 4423ed8..02252a1 100644
--- a/sensor/patchedBLE/src/BLE2904.cpp
+++ b/sensor/patchedBLE/src/BLE2904.cpp
@@ -21,7 +21,7 @@ BLE2904::BLE2904() : BLEDescriptor(BLEUUID((uint16_t) 0x2904)) {
 	m_data.m_namespace   = 1;  // 1 = Bluetooth SIG Assigned Numbers
 	m_data.m_unit        = 0;
 	m_data.m_description = 0;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 } // BLE2902
 
 
@@ -30,7 +30,7 @@ BLE2904::BLE2904() : BLEDescriptor(BLEUUID((uint16_t) 0x2904)) {
  */
 void BLE2904::setDescription(uint16_t description) {
 	m_data.m_description = description;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 }
 
 
@@ -39,7 +39,7 @@ void BLE2904::setDescription(uint16_t description) {
  */
 void BLE2904::setExponent(int8_t exponent) {
 	m_data.m_exponent = exponent;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 } // setExponent
 
 
@@ -48,7 +48,7 @@ void BLE2904::setExponent(int8_t exponent) {
  */
 void BLE2904::setFormat(uint8_t format) {
 	m_data.m_format = format;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 } // setFormat
 
 
@@ -57,18 +57,18 @@ void BLE2904::setFormat(uint8_t format) {
  */
 void BLE2904::setNamespace(uint8_t namespace_value) {
 	m_data.m_namespace = namespace_value;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 } // setNamespace
 
 
 /**
  * @brief Set the units for this value.  It should be one of the encoded values defined here:
  * https://www.bluetooth.com/specifications/assigned-numbers/units
- * @param [in] uint The type of units of this characteristic as defined by assigned numbers.
+ * @param [in] unit The type of units of this characteristic as defined by assigned numbers.
  */
 void BLE2904::setUnit(uint16_t unit) {
 	m_data.m_unit = unit;
-	setValue((uint8_t*)&m_data, sizeof(m_data));
+	setValue((uint8_t*) &m_data, sizeof(m_data));
 } // setUnit
 
 #endif
diff --git a/sensor/patchedBLE/src/BLEAddress.cpp b/sensor/patchedBLE/src/BLEAddress.cpp
index 895feda..d688334 100644
--- a/sensor/patchedBLE/src/BLEAddress.cpp
+++ b/sensor/patchedBLE/src/BLEAddress.cpp
@@ -39,17 +39,16 @@ BLEAddress::BLEAddress(esp_bd_addr_t address) {
  * @param [in] stringAddress The hex representation of the address.
  */
 BLEAddress::BLEAddress(std::string stringAddress) {
-	if (stringAddress.length() != 17) {
-		return;
-	}
+	if (stringAddress.length() != 17) return;
+
 	int data[6];
 	sscanf(stringAddress.c_str(), "%x:%x:%x:%x:%x:%x", &data[0], &data[1], &data[2], &data[3], &data[4], &data[5]);
-	m_address[0] = (uint8_t)data[0];
-	m_address[1] = (uint8_t)data[1];
-	m_address[2] = (uint8_t)data[2];
-	m_address[3] = (uint8_t)data[3];
-	m_address[4] = (uint8_t)data[4];
-	m_address[5] = (uint8_t)data[5];
+	m_address[0] = (uint8_t) data[0];
+	m_address[1] = (uint8_t) data[1];
+	m_address[2] = (uint8_t) data[2];
+	m_address[3] = (uint8_t) data[3];
+	m_address[4] = (uint8_t) data[4];
+	m_address[5] = (uint8_t) data[5];
 } // BLEAddress
 
 
@@ -85,12 +84,12 @@ esp_bd_addr_t *BLEAddress::getNative() {
  */
 std::string BLEAddress::toString() {
 	std::stringstream stream;
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[0] << ':';
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[1] << ':';
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[2] << ':';
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[3] << ':';
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[4] << ':';
-	stream << std::setfill('0') << std::setw(2) << std::hex << (int)((uint8_t *)(m_address))[5];
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[0] << ':';
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[1] << ':';
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[2] << ':';
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[3] << ':';
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[4] << ':';
+	stream << std::setfill('0') << std::setw(2) << std::hex << (int) ((uint8_t*) (m_address))[5];
 	return stream.str();
 } // toString
 #endif
diff --git a/sensor/patchedBLE/src/BLEAdvertisedDevice.cpp b/sensor/patchedBLE/src/BLEAdvertisedDevice.cpp
index 67603df..3f55e8c 100644
--- a/sensor/patchedBLE/src/BLEAdvertisedDevice.cpp
+++ b/sensor/patchedBLE/src/BLEAdvertisedDevice.cpp
@@ -13,14 +13,16 @@
  */
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-#include <esp_log.h>
 #include <sstream>
 #include "BLEAdvertisedDevice.h"
 #include "BLEUtils.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
-#endif
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
 static const char* LOG_TAG="BLEAdvertisedDevice";
+#endif
 
 BLEAdvertisedDevice::BLEAdvertisedDevice() {
 	m_adFlag           = 0;
@@ -137,9 +139,8 @@ BLEUUID BLEAdvertisedDevice::getServiceUUID() {  //TODO Remove it eventually, is
  * @return Return true if service is advertised
  */
 bool BLEAdvertisedDevice::isAdvertisingService(BLEUUID uuid){
-	for (int i = 0; i < m_serviceUUIDs.size(); ++i) {
-		if(m_serviceUUIDs[i].equals(uuid))
-			return true;
+	for (int i = 0; i < m_serviceUUIDs.size(); i++) {
+		if (m_serviceUUIDs[i].equals(uuid)) return true;
 	}
 	return false;
 }
@@ -229,12 +230,13 @@ bool BLEAdvertisedDevice::haveTXPower() {
  *
  * https://www.bluetooth.com/specifications/assigned-numbers/generic-access-profile
  */
-void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
+void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload, size_t total_len) {
 	uint8_t length;
 	uint8_t ad_type;
 	uint8_t sizeConsumed = 0;
 	bool finished = false;
-	setPayload(payload);
+	m_payload = payload;
+	m_payloadLength = total_len;
 
 	while(!finished) {
 		length = *payload;          // Retrieve the length of the record.
@@ -275,7 +277,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 				case ESP_BLE_AD_TYPE_16SRV_CMPL:
 				case ESP_BLE_AD_TYPE_16SRV_PART: {   // Adv Data Type: 0x02
 					for (int var = 0; var < length/2; ++var) {
-						setServiceUUID(BLEUUID(*reinterpret_cast<uint16_t*>(payload+var*2)));
+						setServiceUUID(BLEUUID(*reinterpret_cast<uint16_t*>(payload + var * 2)));
 					}
 					break;
 				} // ESP_BLE_AD_TYPE_16SRV_PART
@@ -283,7 +285,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 				case ESP_BLE_AD_TYPE_32SRV_CMPL:
 				case ESP_BLE_AD_TYPE_32SRV_PART: {   // Adv Data Type: 0x04
 					for (int var = 0; var < length/4; ++var) {
-						setServiceUUID(BLEUUID(*reinterpret_cast<uint32_t*>(payload+var*4)));
+						setServiceUUID(BLEUUID(*reinterpret_cast<uint32_t*>(payload + var * 4)));
 					}
 					break;
 				} // ESP_BLE_AD_TYPE_32SRV_PART
@@ -309,10 +311,10 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_SERVICE_DATA");
 						break;
 					}
-					uint16_t uuid = *(uint16_t *)payload;
+					uint16_t uuid = *(uint16_t*)payload;
 					setServiceDataUUID(BLEUUID(uuid));
 					if (length > 2) {
-						setServiceData(std::string(reinterpret_cast<char*>(payload+2), length-2));
+						setServiceData(std::string(reinterpret_cast<char*>(payload + 2), length - 2));
 					}
 					break;
 				} //ESP_BLE_AD_TYPE_SERVICE_DATA
@@ -322,10 +324,10 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 						ESP_LOGE(LOG_TAG, "Length too small for ESP_BLE_AD_TYPE_32SERVICE_DATA");
 						break;
 					}
-					uint32_t uuid = *(uint32_t *)payload;
+					uint32_t uuid = *(uint32_t*) payload;
 					setServiceDataUUID(BLEUUID(uuid));
 					if (length > 4) {
-						setServiceData(std::string(reinterpret_cast<char*>(payload+4), length-4));
+						setServiceData(std::string(reinterpret_cast<char*>(payload + 4), length - 4));
 					}
 					break;
 				} //ESP_BLE_AD_TYPE_32SERVICE_DATA
@@ -338,7 +340,7 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 
 					setServiceDataUUID(BLEUUID(payload, (size_t)16, false));
 					if (length > 16) {
-						setServiceData(std::string(reinterpret_cast<char*>(payload+16), length-16));
+						setServiceData(std::string(reinterpret_cast<char*>(payload + 16), length - 16));
 					}
 					break;
 				} //ESP_BLE_AD_TYPE_32SERVICE_DATA
@@ -352,9 +354,9 @@ void BLEAdvertisedDevice::parseAdvertisement(uint8_t* payload) {
 		} // Length <> 0
 
 
-		if (sizeConsumed >=31 || length == 0) {
+		if (sizeConsumed >= total_len)
 			finished = true;
-		}
+
 	} // !finished
 } // parseAdvertisement
 
@@ -395,7 +397,7 @@ void BLEAdvertisedDevice::setAppearance(uint16_t appearance) {
 void BLEAdvertisedDevice::setManufacturerData(std::string manufacturerData) {
 	m_manufacturerData     = manufacturerData;
 	m_haveManufacturerData = true;
-	char* pHex = BLEUtils::buildHexData(nullptr, (uint8_t*)m_manufacturerData.data(), (uint8_t)m_manufacturerData.length());
+	char* pHex = BLEUtils::buildHexData(nullptr, (uint8_t*) m_manufacturerData.data(), (uint8_t) m_manufacturerData.length());
 	ESP_LOGD(LOG_TAG, "- manufacturer data: %s", pHex);
 	free(pHex);
 } // setManufacturerData
@@ -511,10 +513,17 @@ uint8_t* BLEAdvertisedDevice::getPayload() {
 	return m_payload;
 }
 
-void BLEAdvertisedDevice::setPayload(uint8_t* payload) {
-	m_payload = payload;
+esp_ble_addr_type_t BLEAdvertisedDevice::getAddressType() {
+	return m_addressType;
+}
+
+void BLEAdvertisedDevice::setAddressType(esp_ble_addr_type_t type) {
+	m_addressType = type;
 }
 
+size_t BLEAdvertisedDevice::getPayloadLength() {
+	return m_payloadLength;
+}
 
 #endif /* CONFIG_BT_ENABLED */
 
diff --git a/sensor/patchedBLE/src/BLEAdvertisedDevice.h b/sensor/patchedBLE/src/BLEAdvertisedDevice.h
index a3b1e6e..aec8374 100644
--- a/sensor/patchedBLE/src/BLEAdvertisedDevice.h
+++ b/sensor/patchedBLE/src/BLEAdvertisedDevice.h
@@ -40,6 +40,9 @@ public:
 	BLEUUID     getServiceUUID();
 	int8_t      getTXPower();
 	uint8_t* 	getPayload();
+	size_t		getPayloadLength();
+	esp_ble_addr_type_t getAddressType();
+	void setAddressType(esp_ble_addr_type_t type);
 
 
 	bool		isAdvertisingService(BLEUUID uuid);
@@ -56,7 +59,7 @@ public:
 private:
 	friend class BLEScan;
 
-	void parseAdvertisement(uint8_t* payload);
+	void parseAdvertisement(uint8_t* payload, size_t total_len=62);
 	void setAddress(BLEAddress address);
 	void setAdFlag(uint8_t adFlag);
 	void setAdvertizementResult(uint8_t* payload);
@@ -70,8 +73,6 @@ private:
 	void setServiceUUID(const char* serviceUUID);
 	void setServiceUUID(BLEUUID serviceUUID);
 	void setTXPower(int8_t txPower);
-	void setPayload(uint8_t* payload);
-
 
 	bool m_haveAppearance;
 	bool m_haveManufacturerData;
@@ -95,6 +96,8 @@ private:
 	std::string m_serviceData;
 	BLEUUID     m_serviceDataUUID;
 	uint8_t*	m_payload;
+	size_t		m_payloadLength = 0;
+	esp_ble_addr_type_t m_addressType;
 };
 
 /**
diff --git a/sensor/patchedBLE/src/BLEAdvertising.cpp b/sensor/patchedBLE/src/BLEAdvertising.cpp
index 4b3cb8d..230d77c 100644
--- a/sensor/patchedBLE/src/BLEAdvertising.cpp
+++ b/sensor/patchedBLE/src/BLEAdvertising.cpp
@@ -19,16 +19,18 @@
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
 #include "BLEAdvertising.h"
-#include <esp_log.h>
 #include <esp_err.h>
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
 
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEAdvertising";
 #endif
 
-static const char* LOG_TAG = "BLEAdvertising";
 
 
 /**
@@ -56,6 +58,7 @@ BLEAdvertising::BLEAdvertising() {
 	m_advParams.own_addr_type     = BLE_ADDR_TYPE_PUBLIC;
 	m_advParams.channel_map       = ADV_CHNL_ALL;
 	m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
+	m_advParams.peer_addr_type    = BLE_ADDR_TYPE_PUBLIC;
 
 	m_customAdvData               = false;   // No custom advertising data
 	m_customScanResponseData      = false;   // No custom scan response data
@@ -92,22 +95,31 @@ void BLEAdvertising::setAppearance(uint16_t appearance) {
 } // setAppearance
 
 void BLEAdvertising::setMinInterval(uint16_t mininterval) {
-	m_advData.min_interval = mininterval;
 	m_advParams.adv_int_min = mininterval;
 } // setMinInterval
 
 void BLEAdvertising::setMaxInterval(uint16_t maxinterval) {
-	m_advData.max_interval = maxinterval;
 	m_advParams.adv_int_max = maxinterval;
 } // setMaxInterval
 
+void BLEAdvertising::setMinPreferred(uint16_t mininterval) {
+	m_advData.min_interval = mininterval;
+} // 
+
+void BLEAdvertising::setMaxPreferred(uint16_t maxinterval) {
+	m_advData.max_interval = maxinterval;
+} // 
+
+void BLEAdvertising::setScanResponse(bool set) {
+	m_scanResp = set;
+}
 
 /**
  * @brief Set the filtering for the scan filter.
  * @param [in] scanRequestWhitelistOnly If true, only allow scan requests from those on the white list.
  * @param [in] connectWhitelistOnly If true, only allow connections from those on the white list.
  */
-void BLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly,	bool connectWhitelistOnly) {
+void BLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly) {
 	ESP_LOGD(LOG_TAG, ">> setScanFilter: scanRequestWhitelistOnly: %d, connectWhitelistOnly: %d", scanRequestWhitelistOnly, connectWhitelistOnly);
 	if (!scanRequestWhitelistOnly && !connectWhitelistOnly) {
 		m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
@@ -173,21 +185,20 @@ void BLEAdvertising::setScanResponseData(BLEAdvertisementData& advertisementData
 void BLEAdvertising::start() {
 	ESP_LOGD(LOG_TAG, ">> start: customAdvData: %d, customScanResponseData: %d", m_customAdvData, m_customScanResponseData);
 
-
 	// We have a vector of service UUIDs that we wish to advertise.  In order to use the
 	// ESP-IDF framework, these must be supplied in a contiguous array of their 128bit (16 byte)
 	// representations.  If we have 1 or more services to advertise then we allocate enough
 	// storage to host them and then copy them in one at a time into the contiguous storage.
 	int numServices = m_serviceUUIDs.size();
 	if (numServices > 0) {
-		m_advData.service_uuid_len = 16*numServices;
-		m_advData.p_service_uuid   = new uint8_t[m_advData.service_uuid_len];
+		m_advData.service_uuid_len = 16 * numServices;
+		m_advData.p_service_uuid = new uint8_t[m_advData.service_uuid_len];
 		uint8_t* p = m_advData.p_service_uuid;
-		for (int i=0; i<numServices; i++) {
+		for (int i = 0; i < numServices; i++) {
 			ESP_LOGD(LOG_TAG, "- advertising service: %s", m_serviceUUIDs[i].toString().c_str());
 			BLEUUID serviceUUID128 = m_serviceUUIDs[i].to128();
 			memcpy(p, serviceUUID128.getNative()->uuid.uuid128, 16);
-			p+=16;
+			p += 16;
 		}
 	} else {
 		m_advData.service_uuid_len = 0;
@@ -196,9 +207,11 @@ void BLEAdvertising::start() {
 
 	esp_err_t errRc;
 
-	if (m_customAdvData == false) {
-	// Set the configuration for advertising.
+	if (!m_customAdvData) {
+		// Set the configuration for advertising.
 		m_advData.set_scan_rsp = false;
+		m_advData.include_name = !m_scanResp;
+		m_advData.include_txpower = !m_scanResp;
 		errRc = ::esp_ble_gap_config_adv_data(&m_advData);
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -206,8 +219,10 @@ void BLEAdvertising::start() {
 		}
 	}
 
-	if (m_customScanResponseData == false) {
+	if (!m_customScanResponseData && m_scanResp) {
 		m_advData.set_scan_rsp = true;
+		m_advData.include_name = m_scanResp;
+		m_advData.include_txpower = m_scanResp;
 		errRc = ::esp_ble_gap_config_adv_data(&m_advData);
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data (Scan response): rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -270,7 +285,7 @@ void BLEAdvertisementData::setAppearance(uint16_t appearance) {
 	char cdata[2];
 	cdata[0] = 3;
 	cdata[1] = ESP_BLE_AD_TYPE_APPEARANCE; // 0x19
-	addData(std::string(cdata, 2) + std::string((char *)&appearance,2));
+	addData(std::string(cdata, 2) + std::string((char*) &appearance, 2));
 } // setAppearance
 
 
@@ -280,12 +295,12 @@ void BLEAdvertisementData::setAppearance(uint16_t appearance) {
  */
 void BLEAdvertisementData::setCompleteServices(BLEUUID uuid) {
 	char cdata[2];
-	switch(uuid.bitSize()) {
+	switch (uuid.bitSize()) {
 		case 16: {
 			// [Len] [0x02] [LL] [HH]
 			cdata[0] = 3;
 			cdata[1] = ESP_BLE_AD_TYPE_16SRV_CMPL;  // 0x03
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
+			addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid16, 2));
 			break;
 		}
 
@@ -293,7 +308,7 @@ void BLEAdvertisementData::setCompleteServices(BLEUUID uuid) {
 			// [Len] [0x04] [LL] [LL] [HH] [HH]
 			cdata[0] = 5;
 			cdata[1] = ESP_BLE_AD_TYPE_32SRV_CMPL;  // 0x05
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
+			addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid32, 4));
 			break;
 		}
 
@@ -301,7 +316,7 @@ void BLEAdvertisementData::setCompleteServices(BLEUUID uuid) {
 			// [Len] [0x04] [0] [1] ... [15]
 			cdata[0] = 17;
 			cdata[1] = ESP_BLE_AD_TYPE_128SRV_CMPL;  // 0x07
-			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
+			addData(std::string(cdata, 2) + std::string((char*) uuid.getNative()->uuid.uuid128, 16));
 			break;
 		}
 
@@ -341,7 +356,7 @@ void BLEAdvertisementData::setManufacturerData(std::string data) {
 	char cdata[2];
 	cdata[0] = data.length() + 1;
 	cdata[1] = ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE;  // 0xff
-	addData(std::string(cdata, 2)  + data);
+	addData(std::string(cdata, 2) + data);
 	ESP_LOGD("BLEAdvertisementData", "<< setManufacturerData");
 } // setManufacturerData
 
@@ -355,7 +370,7 @@ void BLEAdvertisementData::setName(std::string name) {
 	char cdata[2];
 	cdata[0] = name.length() + 1;
 	cdata[1] = ESP_BLE_AD_TYPE_NAME_CMPL;  // 0x09
-	addData(std::string(cdata, 2)  + name);
+	addData(std::string(cdata, 2) + name);
 	ESP_LOGD("BLEAdvertisementData", "<< setName");
 } // setName
 
@@ -366,12 +381,12 @@ void BLEAdvertisementData::setName(std::string name) {
  */
 void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
 	char cdata[2];
-	switch(uuid.bitSize()) {
+	switch (uuid.bitSize()) {
 		case 16: {
 			// [Len] [0x02] [LL] [HH]
 			cdata[0] = 3;
 			cdata[1] = ESP_BLE_AD_TYPE_16SRV_PART;  // 0x02
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
+			addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid16, 2));
 			break;
 		}
 
@@ -379,7 +394,7 @@ void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
 			// [Len] [0x04] [LL] [LL] [HH] [HH]
 			cdata[0] = 5;
 			cdata[1] = ESP_BLE_AD_TYPE_32SRV_PART; // 0x04
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
+			addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid32, 4));
 			break;
 		}
 
@@ -387,7 +402,7 @@ void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
 			// [Len] [0x04] [0] [1] ... [15]
 			cdata[0] = 17;
 			cdata[1] = ESP_BLE_AD_TYPE_128SRV_PART;  // 0x06
-			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
+			addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid128, 16));
 			break;
 		}
 
@@ -404,12 +419,12 @@ void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
  */
 void BLEAdvertisementData::setServiceData(BLEUUID uuid, std::string data) {
 	char cdata[2];
-	switch(uuid.bitSize()) {
+	switch (uuid.bitSize()) {
 		case 16: {
 			// [Len] [0x16] [UUID16] data
 			cdata[0] = data.length() + 3;
 			cdata[1] = ESP_BLE_AD_TYPE_SERVICE_DATA;  // 0x16
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2) + data);
+			addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid16, 2) + data);
 			break;
 		}
 
@@ -417,7 +432,7 @@ void BLEAdvertisementData::setServiceData(BLEUUID uuid, std::string data) {
 			// [Len] [0x20] [UUID32] data
 			cdata[0] = data.length() + 5;
 			cdata[1] = ESP_BLE_AD_TYPE_32SERVICE_DATA; // 0x20
-			addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4) + data);
+			addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid32, 4) + data);
 			break;
 		}
 
@@ -425,7 +440,7 @@ void BLEAdvertisementData::setServiceData(BLEUUID uuid, std::string data) {
 			// [Len] [0x21] [UUID128] data
 			cdata[0] = data.length() + 17;
 			cdata[1] = ESP_BLE_AD_TYPE_128SERVICE_DATA;  // 0x21
-			addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16) + data);
+			addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid128, 16) + data);
 			break;
 		}
 
@@ -444,12 +459,11 @@ void BLEAdvertisementData::setShortName(std::string name) {
 	char cdata[2];
 	cdata[0] = name.length() + 1;
 	cdata[1] = ESP_BLE_AD_TYPE_NAME_SHORT;  // 0x08
-	addData(std::string(cdata, 2)  + name);
+	addData(std::string(cdata, 2) + name);
 	ESP_LOGD("BLEAdvertisementData", "<< setShortName");
 } // setShortName
 
 
-
 /**
  * @brief Retrieve the payload that is to be advertised.
  * @return The payload that is to be advertised.
@@ -458,5 +472,34 @@ std::string BLEAdvertisementData::getPayload() {
 	return m_payload;
 } // getPayload
 
+void BLEAdvertising::handleGAPEvent(
+		esp_gap_ble_cb_event_t  event,
+		esp_ble_gap_cb_param_t* param)  {
+
+	ESP_LOGD(LOG_TAG, "handleGAPEvent [event no: %d]", (int)event);
+
+	switch(event) {
+		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT: {
+			// m_semaphoreSetAdv.give();
+			break;
+		}
+		case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT: {
+			// m_semaphoreSetAdv.give();
+			break;
+		}
+		case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: {
+			// m_semaphoreSetAdv.give();
+			break;
+		}
+		case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT: {
+			ESP_LOGI(LOG_TAG, "STOP advertising");
+			start();
+			break;
+		}
+		default:
+			break;
+	}
+}
+
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/sensor/patchedBLE/src/BLEAdvertising.h b/sensor/patchedBLE/src/BLEAdvertising.h
index e316529..3128b50 100644
--- a/sensor/patchedBLE/src/BLEAdvertising.h
+++ b/sensor/patchedBLE/src/BLEAdvertising.h
@@ -12,6 +12,7 @@
 #include <esp_gap_ble_api.h>
 #include "BLEUUID.h"
 #include <vector>
+#include "FreeRTOS.h"
 
 /**
  * @brief Advertisement data set by the programmer to be published by the %BLE server.
@@ -56,13 +57,22 @@ public:
 	void setAdvertisementData(BLEAdvertisementData& advertisementData);
 	void setScanFilter(bool scanRequertWhitelistOnly, bool connectWhitelistOnly);
 	void setScanResponseData(BLEAdvertisementData& advertisementData);
+	void setPrivateAddress(esp_ble_addr_type_t type = BLE_ADDR_TYPE_RANDOM);
+
+	void handleGAPEvent(esp_gap_ble_cb_event_t  event, esp_ble_gap_cb_param_t* param);
+	void setMinPreferred(uint16_t);
+	void setMaxPreferred(uint16_t);
+	void setScanResponse(bool);
 
 private:
 	esp_ble_adv_data_t   m_advData;
 	esp_ble_adv_params_t m_advParams;
 	std::vector<BLEUUID> m_serviceUUIDs;
-	bool                 m_customAdvData;  // Are we using custom advertising data?
-	bool                 m_customScanResponseData;  // Are we using custom scan response data?
+	bool                 m_customAdvData = false;  // Are we using custom advertising data?
+	bool                 m_customScanResponseData = false;  // Are we using custom scan response data?
+	FreeRTOS::Semaphore  m_semaphoreSetAdv = FreeRTOS::Semaphore("startAdvert");
+	bool				m_scanResp = true;
+
 };
 #endif /* CONFIG_BT_ENABLED */
-#endif /* COMPONENTS_CPP_UTILS_BLEADVERTISING_H_ */
+#endif /* COMPONENTS_CPP_UTILS_BLEADVERTISING_H_ */
\ No newline at end of file
diff --git a/sensor/patchedBLE/src/BLEBeacon.cpp b/sensor/patchedBLE/src/BLEBeacon.cpp
index a63197c..68f8d8e 100644
--- a/sensor/patchedBLE/src/BLEBeacon.cpp
+++ b/sensor/patchedBLE/src/BLEBeacon.cpp
@@ -7,12 +7,17 @@
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
 #include <string.h>
-#include <esp_log.h>
 #include "BLEBeacon.h"
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
+#include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEBeacon";
+#endif
 
 #define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00)>>8) + (((x)&0xFF)<<8))
 
-static const char LOG_TAG[] = "BLEBeacon";
 
 BLEBeacon::BLEBeacon() {
 	m_beaconData.manufacturerId = 0x4c00;
@@ -25,7 +30,7 @@ BLEBeacon::BLEBeacon() {
 } // BLEBeacon
 
 std::string BLEBeacon::getData() {
-	return std::string((char*)&m_beaconData, sizeof(m_beaconData));
+	return std::string((char*) &m_beaconData, sizeof(m_beaconData));
 } // getData
 
 uint16_t BLEBeacon::getMajor() {
diff --git a/sensor/patchedBLE/src/BLEBeacon.h b/sensor/patchedBLE/src/BLEBeacon.h
index 0b02e2b..277bd67 100644
--- a/sensor/patchedBLE/src/BLEBeacon.h
+++ b/sensor/patchedBLE/src/BLEBeacon.h
@@ -23,7 +23,7 @@ private:
 		uint16_t major;
 		uint16_t minor;
 		int8_t   signalPower;
-	} __attribute__((packed))m_beaconData;
+	} __attribute__((packed)) m_beaconData;
 public:
 	BLEBeacon();
 	std::string getData();
diff --git a/sensor/patchedBLE/src/BLECharacteristic.cpp b/sensor/patchedBLE/src/BLECharacteristic.cpp
index 931c753..e340287 100644
--- a/sensor/patchedBLE/src/BLECharacteristic.cpp
+++ b/sensor/patchedBLE/src/BLECharacteristic.cpp
@@ -11,7 +11,6 @@
 #include <iomanip>
 #include <stdlib.h>
 #include "sdkconfig.h"
-#include <esp_log.h>
 #include <esp_err.h>
 #include "BLECharacteristic.h"
 #include "BLEService.h"
@@ -19,11 +18,13 @@
 #include "BLEUtils.h"
 #include "BLE2902.h"
 #include "GeneralUtils.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
-#endif
-
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
 static const char* LOG_TAG = "BLECharacteristic";
+#endif
 
 #define NULL_HANDLE (0xffff)
 
@@ -47,12 +48,12 @@ BLECharacteristic::BLECharacteristic(BLEUUID uuid, uint32_t properties) {
 	m_properties = (esp_gatt_char_prop_t)0;
 	m_pCallbacks = nullptr;
 
-	setBroadcastProperty((properties & PROPERTY_BROADCAST) !=0);
-	setReadProperty((properties & PROPERTY_READ) !=0);
-	setWriteProperty((properties & PROPERTY_WRITE) !=0);
-	setNotifyProperty((properties & PROPERTY_NOTIFY) !=0);
-	setIndicateProperty((properties & PROPERTY_INDICATE) !=0);
-	setWriteNoResponseProperty((properties & PROPERTY_WRITE_NR) !=0);
+	setBroadcastProperty((properties & PROPERTY_BROADCAST) != 0);
+	setReadProperty((properties & PROPERTY_READ) != 0);
+	setWriteProperty((properties & PROPERTY_WRITE) != 0);
+	setNotifyProperty((properties & PROPERTY_NOTIFY) != 0);
+	setIndicateProperty((properties & PROPERTY_INDICATE) != 0);
+	setWriteNoResponseProperty((properties & PROPERTY_WRITE_NR) != 0);
 } // BLECharacteristic
 
 /**
@@ -87,7 +88,7 @@ void BLECharacteristic::executeCreate(BLEService* pService) {
 		return;
 	}
 
-	m_pService = pService; // Save the service for to which this characteristic belongs.
+	m_pService = pService; // Save the service to which this characteristic belongs.
 
 	ESP_LOGD(LOG_TAG, "Registering characteristic (esp_ble_gatts_add_char): uuid: %s, service: %s",
 		getUUID().toString().c_str(),
@@ -97,20 +98,11 @@ void BLECharacteristic::executeCreate(BLEService* pService) {
 	control.auto_rsp = ESP_GATT_RSP_BY_APP;
 
 	m_semaphoreCreateEvt.take("executeCreate");
-
-	/*
-	esp_attr_value_t value;
-	value.attr_len     = m_value.getLength();
-	value.attr_max_len = ESP_GATT_MAX_ATTR_LEN;
-	value.attr_value   = m_value.getData();
-	*/
-
 	esp_err_t errRc = ::esp_ble_gatts_add_char(
 		m_pService->getHandle(),
 		getUUID().getNative(),
 		static_cast<esp_gatt_perm_t>(m_permissions),
 		getProperties(),
-		//&value,
 		nullptr,
 		&control); // Whether to auto respond or not.
 
@@ -118,15 +110,9 @@ void BLECharacteristic::executeCreate(BLEService* pService) {
 		ESP_LOGE(LOG_TAG, "<< esp_ble_gatts_add_char: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		return;
 	}
-
 	m_semaphoreCreateEvt.wait("executeCreate");
 
-	// Now that we have registered the characteristic, we must also register all the descriptors associated with this
-	// characteristic.  We iterate through each of those and invoke the registration call to register them with the
-	// ESP environment.
-
 	BLEDescriptor* pDescriptor = m_descriptorMap.getFirst();
-
 	while (pDescriptor != nullptr) {
 		pDescriptor->executeCreate(this);
 		pDescriptor = m_descriptorMap.getNext();
@@ -198,6 +184,14 @@ std::string BLECharacteristic::getValue() {
 	return m_value.getValue();
 } // getValue
 
+/**
+ * @brief Retrieve the current raw data of the characteristic.
+ * @return A pointer to storage containing the current characteristic data.
+ */
+uint8_t* BLECharacteristic::getData() {
+	return m_value.getData();
+} // getData
+
 
 /**
  * Handle a GATT server event.
@@ -238,7 +232,7 @@ void BLECharacteristic::handleGATTServerEvent(
 			} else {
 				m_value.cancel();
 			}
-
+// ???
 			esp_err_t errRc = ::esp_ble_gatts_send_response(
 					gatts_if,
 					param->write.conn_id,
@@ -257,9 +251,13 @@ void BLECharacteristic::handleGATTServerEvent(
 		// - uint16_t service_handle
 		// - esp_bt_uuid_t char_uuid
 		case ESP_GATTS_ADD_CHAR_EVT: {
-			if (getUUID().equals(BLEUUID(param->add_char.char_uuid)) &&
-					getHandle() == param->add_char.attr_handle &&
-					getService()->getHandle()==param->add_char.service_handle) {
+			if (getHandle() == param->add_char.attr_handle) {
+				// we have created characteristic, now we can create descriptors
+				// BLEDescriptor* pDescriptor = m_descriptorMap.getFirst();
+				// while (pDescriptor != nullptr) {
+				// 	pDescriptor->executeCreate(this);
+				// 	pDescriptor = m_descriptorMap.getNext();
+				// } // End while
 				m_semaphoreCreateEvt.give();
 			}
 			break;
@@ -360,11 +358,10 @@ void BLECharacteristic::handleGATTServerEvent(
 // The following code has deliberately not been factored to make it fewer statements because this would cloud the
 // the logic flow comprehension.
 //
-				// TODO requires some more research to confirm that 512 is max PDU like in bluetooth specs
-				uint16_t maxOffset = BLEDevice::getMTU() - 1;
-				if (BLEDevice::getMTU() > 512) {
-					maxOffset = 512;
-				}
+
+				// get mtu for peer device that we are sending read request to
+				uint16_t maxOffset =  getService()->getServer()->getPeerMTU(param->read.conn_id) - 1;
+				ESP_LOGD(LOG_TAG, "mtu value: %d", maxOffset);
 				if (param->read.need_rsp) {
 					ESP_LOGD(LOG_TAG, "Sending a response (esp_ble_gatts_send_response)");
 					esp_gatt_rsp_t rsp;
@@ -387,13 +384,9 @@ void BLECharacteristic::handleGATTServerEvent(
 						}
 					} else { // read.is_long == false
 
-						if (m_pCallbacks != nullptr) {  // If is.long is false then this is the first (or only) request to read data, so invoke the callback
-							m_pCallbacks->onRead(this);   // Invoke the read callback.
-						}
-
 						std::string value = m_value.getValue();
 
-						if (value.length()+1 > maxOffset) {
+						if (value.length() + 1 > maxOffset) {
 							// Too big for a single shot entry.
 							m_value.setReadOffset(maxOffset);
 							rsp.attr_value.len    = maxOffset;
@@ -405,6 +398,10 @@ void BLECharacteristic::handleGATTServerEvent(
 							rsp.attr_value.offset = 0;
 							memcpy(rsp.attr_value.value, value.data(), rsp.attr_value.len);
 						}
+
+						if (m_pCallbacks != nullptr) {  // If is.long is false then this is the first (or only) request to read data, so invoke the callback
+							m_pCallbacks->onRead(this);   // Invoke the read callback.
+						}
 					}
 					rsp.attr_value.handle   = param->read.handle;
 					rsp.attr_value.auth_req = ESP_GATT_AUTH_REQ_NONE;
@@ -434,12 +431,13 @@ void BLECharacteristic::handleGATTServerEvent(
 		// - uint16_t          conn_id – The connection used.
 		//
 		case ESP_GATTS_CONF_EVT: {
-			m_semaphoreConfEvt.give();
+			// ESP_LOGD(LOG_TAG, "m_handle = %d, conf->handle = %d", m_handle, param->conf.handle);
+			if(param->conf.conn_id == getService()->getServer()->getConnId()) // && param->conf.handle == m_handle) // bug in esp-idf and not implemented in arduino yet
+				m_semaphoreConfEvt.give(param->conf.status);
 			break;
 		}
 
 		case ESP_GATTS_CONNECT_EVT: {
-			m_semaphoreConfEvt.give();
 			break;
 		}
 
@@ -471,45 +469,7 @@ void BLECharacteristic::handleGATTServerEvent(
 void BLECharacteristic::indicate() {
 
 	ESP_LOGD(LOG_TAG, ">> indicate: length: %d", m_value.getValue().length());
-
-	assert(getService() != nullptr);
-	assert(getService()->getServer() != nullptr);
-
-	GeneralUtils::hexDump((uint8_t*)m_value.getValue().data(), m_value.getValue().length());
-
-	if (getService()->getServer()->getConnectedCount() == 0) {
-		ESP_LOGD(LOG_TAG, "<< indicate: No connected clients.");
-		return;
-	}
-
-	// Test to see if we have a 0x2902 descriptor.  If we do, then check to see if indications are enabled
-	// and, if not, prevent the indication.
-
-	BLE2902 *p2902 = (BLE2902*)getDescriptorByUUID((uint16_t)0x2902);
-	if (p2902 != nullptr && !p2902->getIndications()) {
-		ESP_LOGD(LOG_TAG, "<< indications disabled; ignoring");
-		return;
-	}
-
-	if (m_value.getValue().length() > (BLEDevice::getMTU() - 3)) {
-		ESP_LOGI(LOG_TAG, "- Truncating to %d bytes (maximum indicate size)", BLEDevice::getMTU() - 3);
-	}
-
-	size_t length = m_value.getValue().length();
-
-	m_semaphoreConfEvt.take("indicate");
-
-	esp_err_t errRc = ::esp_ble_gatts_send_indicate(
-			getService()->getServer()->getGattsIf(),
-			getService()->getServer()->getConnId(),
-			getHandle(), length, (uint8_t*)m_value.getValue().data(), true); // The need_confirm = true makes this an indication.
-
-	if (errRc != ESP_OK) {
-		ESP_LOGE(LOG_TAG, "<< esp_ble_gatts_send_indicate: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-		return;
-	}
-
-	m_semaphoreConfEvt.wait("indicate");
+	notify(false);
 	ESP_LOGD(LOG_TAG, "<< indicate");
 } // indicate
 
@@ -520,14 +480,12 @@ void BLECharacteristic::indicate() {
  * will not block; it is a fire and forget.
  * @return N/A.
  */
-void BLECharacteristic::notify() {
+void BLECharacteristic::notify(bool is_notification) {
 	ESP_LOGD(LOG_TAG, ">> notify: length: %d", m_value.getValue().length());
 
-
 	assert(getService() != nullptr);
 	assert(getService()->getServer() != nullptr);
 
-
 	GeneralUtils::hexDump((uint8_t*)m_value.getValue().data(), m_value.getValue().length());
 
 	if (getService()->getServer()->getConnectedCount() == 0) {
@@ -539,30 +497,39 @@ void BLECharacteristic::notify() {
 	// and, if not, prevent the notification.
 
 	BLE2902 *p2902 = (BLE2902*)getDescriptorByUUID((uint16_t)0x2902);
-	if (p2902 != nullptr && !p2902->getNotifications()) {
-		ESP_LOGD(LOG_TAG, "<< notifications disabled; ignoring");
-		return;
+	if(is_notification) {
+		if (p2902 != nullptr && !p2902->getNotifications()) {
+			ESP_LOGD(LOG_TAG, "<< notifications disabled; ignoring");
+			return;
+		}
 	}
-
-	if (m_value.getValue().length() > (BLEDevice::getMTU() - 3)) {
-		ESP_LOGI(LOG_TAG, "- Truncating to %d bytes (maximum notify size)", BLEDevice::getMTU() - 3);
+	else{
+		if (p2902 != nullptr && !p2902->getIndications()) {
+			ESP_LOGD(LOG_TAG, "<< indications disabled; ignoring");
+			return;
+		}
 	}
+	for (auto &myPair : getService()->getServer()->getPeerDevices(false)) {
+		uint16_t _mtu = (myPair.second.mtu);
+		if (m_value.getValue().length() > _mtu - 3) {
+			ESP_LOGW(LOG_TAG, "- Truncating to %d bytes (maximum notify size)", _mtu - 3);
+		}
 
-	size_t length = m_value.getValue().length();
-
-	m_semaphoreConfEvt.take("notify");
-
-	esp_err_t errRc = ::esp_ble_gatts_send_indicate(
-			getService()->getServer()->getGattsIf(),
-			getService()->getServer()->getConnId(),
-			getHandle(), length, (uint8_t*)m_value.getValue().data(), false); // The need_confirm = false makes this a notify.
-	if (errRc != ESP_OK) {
-		ESP_LOGE(LOG_TAG, "<< esp_ble_gatts_send_indicate: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-		return;
+		size_t length = m_value.getValue().length();
+		if(!is_notification)
+			m_semaphoreConfEvt.take("indicate");
+		esp_err_t errRc = ::esp_ble_gatts_send_indicate(
+				getService()->getServer()->getGattsIf(),
+				myPair.first,
+				getHandle(), length, (uint8_t*)m_value.getValue().data(), !is_notification); // The need_confirm = false makes this a notify.
+		if (errRc != ESP_OK) {
+			ESP_LOGE(LOG_TAG, "<< esp_ble_gatts_send_ %s: rc=%d %s",is_notification?"notify":"indicate", errRc, GeneralUtils::errorToString(errRc));
+			m_semaphoreConfEvt.give();
+			return;
+		}
+		if(!is_notification)
+			m_semaphoreConfEvt.wait("indicate");
 	}
-
-	m_semaphoreConfEvt.wait("notify");
-
 	ESP_LOGD(LOG_TAG, "<< notify");
 } // Notify
 
@@ -660,7 +627,7 @@ void BLECharacteristic::setReadProperty(bool value) {
  * @param [in] length The length of the data in bytes.
  */
 void BLECharacteristic::setValue(uint8_t* data, size_t length) {
-	char *pHex = BLEUtils::buildHexData(nullptr, data, length);
+	char* pHex = BLEUtils::buildHexData(nullptr, data, length);
 	ESP_LOGD(LOG_TAG, ">> setValue: length=%d, data=%s, characteristic UUID=%s", length, pHex, getUUID().toString().c_str());
 	free(pHex);
 	if (length > ESP_GATT_MAX_ATTR_LEN) {
@@ -685,38 +652,38 @@ void BLECharacteristic::setValue(std::string value) {
 
 void BLECharacteristic::setValue(uint16_t& data16) {
 	uint8_t temp[2];
-	temp[0]=data16;
-	temp[1]=data16>>8;
+	temp[0] = data16;
+	temp[1] = data16 >> 8;
 	setValue(temp, 2);
 } // setValue
 
 void BLECharacteristic::setValue(uint32_t& data32) {
 	uint8_t temp[4];
-	temp[0]=data32;
-	temp[1]=data32>>8;
-	temp[2]=data32>>16;
-	temp[3]=data32>>24;
+	temp[0] = data32;
+	temp[1] = data32 >> 8;
+	temp[2] = data32 >> 16;
+	temp[3] = data32 >> 24;
 	setValue(temp, 4);
 } // setValue
 
 void BLECharacteristic::setValue(int& data32) {
 	uint8_t temp[4];
-	temp[0]=data32;
-	temp[1]=data32>>8;
-	temp[2]=data32>>16;
-	temp[3]=data32>>24;
+	temp[0] = data32;
+	temp[1] = data32 >> 8;
+	temp[2] = data32 >> 16;
+	temp[3] = data32 >> 24;
 	setValue(temp, 4);
 } // setValue
 
 void BLECharacteristic::setValue(float& data32) {
 	uint8_t temp[4];
-	*((float *)temp) = data32;
+	*((float*)temp) = data32;
 	setValue(temp, 4);
 } // setValue
 
 void BLECharacteristic::setValue(double& data64) {
 	uint8_t temp[8];
-	*((double *)temp) = data64;
+	*((double*)temp) = data64;
 	setValue(temp, 8);
 } // setValue
 
@@ -758,12 +725,12 @@ std::string BLECharacteristic::toString() {
 	stringstream << std::hex << std::setfill('0');
 	stringstream << "UUID: " << m_bleUUID.toString() + ", handle: 0x" << std::setw(2) << m_handle;
 	stringstream << " " <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_READ)?"Read ":"") <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_WRITE)?"Write ":"") <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_WRITE_NR)?"WriteNoResponse ":"") <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_BROADCAST)?"Broadcast ":"") <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_NOTIFY)?"Notify ":"") <<
-		((m_properties & ESP_GATT_CHAR_PROP_BIT_INDICATE)?"Indicate ":"");
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_READ) ? "Read " : "") <<
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_WRITE) ? "Write " : "") <<
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_WRITE_NR) ? "WriteNoResponse " : "") <<
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_BROADCAST) ? "Broadcast " : "") <<
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_NOTIFY) ? "Notify " : "") <<
+		((m_properties & ESP_GATT_CHAR_PROP_BIT_INDICATE) ? "Indicate " : "");
 	return stringstream.str();
 } // toString
 
@@ -775,7 +742,7 @@ BLECharacteristicCallbacks::~BLECharacteristicCallbacks() {}
  * @brief Callback function to support a read request.
  * @param [in] pCharacteristic The characteristic that is the source of the event.
  */
-void BLECharacteristicCallbacks::onRead(BLECharacteristic *pCharacteristic) {
+void BLECharacteristicCallbacks::onRead(BLECharacteristic* pCharacteristic) {
 	ESP_LOGD("BLECharacteristicCallbacks", ">> onRead: default");
 	ESP_LOGD("BLECharacteristicCallbacks", "<< onRead");
 } // onRead
@@ -785,7 +752,7 @@ void BLECharacteristicCallbacks::onRead(BLECharacteristic *pCharacteristic) {
  * @brief Callback function to support a write request.
  * @param [in] pCharacteristic The characteristic that is the source of the event.
  */
-void BLECharacteristicCallbacks::onWrite(BLECharacteristic *pCharacteristic) {
+void BLECharacteristicCallbacks::onWrite(BLECharacteristic* pCharacteristic) {
 	ESP_LOGD("BLECharacteristicCallbacks", ">> onWrite: default");
 	ESP_LOGD("BLECharacteristicCallbacks", "<< onWrite");
 } // onWrite
diff --git a/sensor/patchedBLE/src/BLECharacteristic.h b/sensor/patchedBLE/src/BLECharacteristic.h
index b3f8d2e..5eb1e8d 100644
--- a/sensor/patchedBLE/src/BLECharacteristic.h
+++ b/sensor/patchedBLE/src/BLECharacteristic.h
@@ -27,30 +27,27 @@ class BLECharacteristicCallbacks;
  */
 class BLEDescriptorMap {
 public:
-	void setByUUID(const char* uuid,  BLEDescriptor *pDescriptor);
-	void setByUUID(BLEUUID uuid,      BLEDescriptor *pDescriptor);
-	void setByHandle(uint16_t handle, BLEDescriptor *pDescriptor);
+	void setByUUID(const char* uuid, BLEDescriptor* pDescriptor);
+	void setByUUID(BLEUUID uuid, BLEDescriptor* pDescriptor);
+	void setByHandle(uint16_t handle, BLEDescriptor* pDescriptor);
 	BLEDescriptor* getByUUID(const char* uuid);
 	BLEDescriptor* getByUUID(BLEUUID uuid);
 	BLEDescriptor* getByHandle(uint16_t handle);
-	std::string    toString();
-	void handleGATTServerEvent(
-			esp_gatts_cb_event_t      event,
-			esp_gatt_if_t             gatts_if,
-			esp_ble_gatts_cb_param_t* param);
+	std::string	toString();
+	void handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param);
 	BLEDescriptor* getFirst();
 	BLEDescriptor* getNext();
 private:
-	std::map<std::string, BLEDescriptor *> m_uuidMap;
-	std::map<uint16_t,    BLEDescriptor *> m_handleMap;
-	std::map<std::string, BLEDescriptor *>::iterator m_iterator;
+	std::map<BLEDescriptor*, std::string> m_uuidMap;
+	std::map<uint16_t, BLEDescriptor*> m_handleMap;
+	std::map<BLEDescriptor*, std::string>::iterator m_iterator;
 };
 
 
 /**
  * @brief The model of a %BLE Characteristic.
  *
- * A %BLE Characteristic is an identified value container that manages a value.  It is exposed by a %BLE server and
+ * A BLE Characteristic is an identified value container that manages a value. It is exposed by a BLE server and
  * can be read and written to by a %BLE client.
  */
 class BLECharacteristic {
@@ -62,12 +59,12 @@ public:
 	void           addDescriptor(BLEDescriptor* pDescriptor);
 	BLEDescriptor* getDescriptorByUUID(const char* descriptorUUID);
 	BLEDescriptor* getDescriptorByUUID(BLEUUID descriptorUUID);
-	//size_t         getLength();
 	BLEUUID        getUUID();
 	std::string    getValue();
+	uint8_t*       getData();
 
 	void indicate();
-	void notify();
+	void notify(bool is_notification = true);
 	void setBroadcastProperty(bool value);
 	void setCallbacks(BLECharacteristicCallbacks* pCallbacks);
 	void setIndicateProperty(bool value);
@@ -127,7 +124,7 @@ private:
  * @brief Callbacks that can be associated with a %BLE characteristic to inform of events.
  *
  * When a server application creates a %BLE characteristic, we may wish to be informed when there is either
- * a read or write request to the characteristic's value.  An application can register a
+ * a read or write request to the characteristic's value. An application can register a
  * sub-classed instance of this class and will be notified when such an event happens.
  */
 class BLECharacteristicCallbacks {
diff --git a/sensor/patchedBLE/src/BLECharacteristicMap.cpp b/sensor/patchedBLE/src/BLECharacteristicMap.cpp
index 86a9744..d73aae9 100644
--- a/sensor/patchedBLE/src/BLECharacteristicMap.cpp
+++ b/sensor/patchedBLE/src/BLECharacteristicMap.cpp
@@ -56,9 +56,7 @@ BLECharacteristic* BLECharacteristicMap::getByUUID(BLEUUID uuid) {
  */
 BLECharacteristic* BLECharacteristicMap::getFirst() {
 	m_iterator = m_uuidMap.begin();
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
+	if (m_iterator == m_uuidMap.end()) return nullptr;
 	BLECharacteristic* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
@@ -70,9 +68,7 @@ BLECharacteristic* BLECharacteristicMap::getFirst() {
  * @return The next characteristic in the map.
  */
 BLECharacteristic* BLECharacteristicMap::getNext() {
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
+	if (m_iterator == m_uuidMap.end()) return nullptr;
 	BLECharacteristic* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
@@ -85,12 +81,9 @@ BLECharacteristic* BLECharacteristicMap::getNext() {
  * @param [in] gatts_if
  * @param [in] param
  */
-void BLECharacteristicMap::handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t* param) {
+void BLECharacteristicMap::handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param) {
 	// Invoke the handler for every Service we have.
-	for (auto &myPair : m_uuidMap) {
+	for (auto& myPair : m_uuidMap) {
 		myPair.first->handleGATTServerEvent(event, gatts_if, param);
 	}
 } // handleGATTServerEvent
@@ -102,9 +95,8 @@ void BLECharacteristicMap::handleGATTServerEvent(
  * @param [in] characteristic The characteristic to cache.
  * @return N/A.
  */
-void BLECharacteristicMap::setByHandle(uint16_t handle,
-		BLECharacteristic *characteristic) {
-	m_handleMap.insert(std::pair<uint16_t, BLECharacteristic *>(handle, characteristic));
+void BLECharacteristicMap::setByHandle(uint16_t handle, BLECharacteristic* characteristic) {
+	m_handleMap.insert(std::pair<uint16_t, BLECharacteristic*>(handle, characteristic));
 } // setByHandle
 
 
@@ -114,10 +106,8 @@ void BLECharacteristicMap::setByHandle(uint16_t handle,
  * @param [in] characteristic The characteristic to cache.
  * @return N/A.
  */
-void BLECharacteristicMap::setByUUID(
-		BLECharacteristic *pCharacteristic,
-		BLEUUID            uuid) {
-	m_uuidMap.insert(std::pair<BLECharacteristic *, std::string>(pCharacteristic, uuid.toString()));
+void BLECharacteristicMap::setByUUID(BLECharacteristic* pCharacteristic, BLEUUID uuid) {
+	m_uuidMap.insert(std::pair<BLECharacteristic*, std::string>(pCharacteristic, uuid.toString()));
 } // setByUUID
 
 
@@ -128,7 +118,7 @@ void BLECharacteristicMap::setByUUID(
 std::string BLECharacteristicMap::toString() {
 	std::stringstream stringStream;
 	stringStream << std::hex << std::setfill('0');
-	int count=0;
+	int count = 0;
 	for (auto &myPair: m_uuidMap) {
 		if (count > 0) {
 			stringStream << "\n";
diff --git a/sensor/patchedBLE/src/BLEClient.cpp b/sensor/patchedBLE/src/BLEClient.cpp
index 141cf0f..0e552ec 100644
--- a/sensor/patchedBLE/src/BLEClient.cpp
+++ b/sensor/patchedBLE/src/BLEClient.cpp
@@ -6,7 +6,6 @@
  */
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-#include <esp_log.h>
 #include <esp_bt.h>
 #include <esp_bt_main.h>
 #include <esp_gap_ble_api.h>
@@ -18,10 +17,16 @@
 #include <string>
 #include <sstream>
 #include <unordered_set>
-#ifdef ARDUINO_ARCH_ESP32
+#include "BLEDevice.h"
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEClient";
 #endif
 
+
 /*
  * Design
  * ------
@@ -42,12 +47,11 @@
  *
  *
  */
-static const char* LOG_TAG = "BLEClient";
 
 BLEClient::BLEClient() {
 	m_pClientCallbacks = nullptr;
-	m_conn_id          = 0;
-	m_gattc_if         = 0;
+	m_conn_id          = ESP_GATT_IF_NONE;
+	m_gattc_if         = ESP_GATT_IF_NONE;
 	m_haveServices     = false;
 	m_isConnected      = false;  // Initially, we are flagged as not connected.
 } // BLEClient
@@ -81,22 +85,31 @@ void BLEClient::clearServices() {
 	ESP_LOGD(LOG_TAG, "<< clearServices");
 } // clearServices
 
+/**
+ * Add overloaded function to ease connect to peer device with not public address
+ */
+bool BLEClient::connect(BLEAdvertisedDevice* device) {
+	BLEAddress address =  device->getAddress();
+	esp_ble_addr_type_t type = device->getAddressType();
+	return connect(address, type);
+}
 
 /**
  * @brief Connect to the partner (BLE Server).
  * @param [in] address The address of the partner.
  * @return True on success.
  */
-bool BLEClient::connect(BLEAddress address) {
+bool BLEClient::connect(BLEAddress address, esp_ble_addr_type_t type) {
 	ESP_LOGD(LOG_TAG, ">> connect(%s)", address.toString().c_str());
 
 // We need the connection handle that we get from registering the application.  We register the app
 // and then block on its completion.  When the event has arrived, we will have the handle.
+	m_appId = BLEDevice::m_appId++;
+	BLEDevice::addPeerDevice(this, true, m_appId);
 	m_semaphoreRegEvt.take("connect");
 
-	clearServices(); // Delete any services that may exist.
-
-	esp_err_t errRc = ::esp_ble_gattc_app_register(0);
+	// clearServices(); // we dont need to delete services since every client is unique?
+	esp_err_t errRc = ::esp_ble_gattc_app_register(m_appId);
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gattc_app_register: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		return false;
@@ -109,10 +122,10 @@ bool BLEClient::connect(BLEAddress address) {
 	// Perform the open connection request against the target BLE Server.
 	m_semaphoreOpenEvt.take("connect");
 	errRc = ::esp_ble_gattc_open(
-		getGattcIf(),
+		m_gattc_if,
 		*getPeerAddress().getNative(), // address
-		BLE_ADDR_TYPE_PUBLIC,          // Note: This was added on 2018-04-03 when the latest ESP-IDF was detected to have changed the signature.
-		1                              // direct connection
+		type,          // Note: This was added on 2018-04-03 when the latest ESP-IDF was detected to have changed the signature.
+		1                              // direct connection <-- maybe needs to be changed in case of direct indirect connection???
 	);
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gattc_open: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -136,8 +149,6 @@ void BLEClient::disconnect() {
 		ESP_LOGE(LOG_TAG, "esp_ble_gattc_close: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		return;
 	}
-	esp_ble_gattc_app_unregister(getGattcIf());
-	m_peerAddress = BLEAddress("00:00:00:00:00:00");
 	ESP_LOGD(LOG_TAG, "<< disconnect()");
 } // disconnect
 
@@ -150,9 +161,22 @@ void BLEClient::gattClientEventHandler(
 	esp_gatt_if_t             gattc_if,
 	esp_ble_gattc_cb_param_t* evtParam) {
 
+	ESP_LOGD(LOG_TAG, "gattClientEventHandler [esp_gatt_if: %d] ... %s",
+		gattc_if, BLEUtils::gattClientEventTypeToString(event).c_str());
+
 	// Execute handler code based on the type of event received.
 	switch(event) {
 
+		case ESP_GATTC_SRVC_CHG_EVT:
+			ESP_LOGI(LOG_TAG, "SERVICE CHANGED");
+			break;
+
+		case ESP_GATTC_CLOSE_EVT: {
+				// esp_ble_gattc_app_unregister(m_appId);
+				// BLEDevice::removePeerDevice(m_gattc_if, true);
+			break;
+		}
+
 		//
 		// ESP_GATTC_DISCONNECT_EVT
 		//
@@ -163,10 +187,12 @@ void BLEClient::gattClientEventHandler(
 		case ESP_GATTC_DISCONNECT_EVT: {
 				// If we receive a disconnect event, set the class flag that indicates that we are
 				// no longer connected.
+				m_isConnected = false;
 				if (m_pClientCallbacks != nullptr) {
 					m_pClientCallbacks->onDisconnect(this);
 				}
-				m_isConnected = false;
+				BLEDevice::removePeerDevice(m_appId, true);
+				esp_ble_gattc_app_unregister(m_gattc_if);
 				m_semaphoreRssiCmplEvt.give();
 				m_semaphoreSearchCmplEvt.give(1);
 				break;
@@ -179,7 +205,6 @@ void BLEClient::gattClientEventHandler(
 		// - esp_gatt_status_t status
 		// - uint16_t          conn_id
 		// - esp_bd_addr_t     remote_bda
-		// - uint16_t          mtu
 		//
 		case ESP_GATTC_OPEN_EVT: {
 			m_conn_id = evtParam->open.conn_id;
@@ -207,6 +232,26 @@ void BLEClient::gattClientEventHandler(
 			break;
 		} // ESP_GATTC_REG_EVT
 
+		case ESP_GATTC_CFG_MTU_EVT:
+			if(evtParam->cfg_mtu.status != ESP_GATT_OK) {
+				ESP_LOGE(LOG_TAG,"Config mtu failed");
+			}
+			m_mtu = evtParam->cfg_mtu.mtu;
+			break;
+
+		case ESP_GATTC_CONNECT_EVT: {
+			BLEDevice::updatePeerDevice(this, true, m_gattc_if);
+			esp_err_t errRc = esp_ble_gattc_send_mtu_req(gattc_if, evtParam->connect.conn_id);
+			if (errRc != ESP_OK) {
+				ESP_LOGE(LOG_TAG, "esp_ble_gattc_send_mtu_req: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+			}
+#ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
+			if(BLEDevice::m_securityLevel){
+				esp_ble_set_encryption(evtParam->connect.remote_bda, BLEDevice::m_securityLevel);
+			}
+#endif	// CONFIG_BLE_SMP_ENABLE
+			break;
+		} // ESP_GATTC_CONNECT_EVT
 
 		//
 		// ESP_GATTC_SEARCH_CMPL_EVT
@@ -216,6 +261,21 @@ void BLEClient::gattClientEventHandler(
 		// - uint16_t          conn_id
 		//
 		case ESP_GATTC_SEARCH_CMPL_EVT: {
+			esp_ble_gattc_cb_param_t* p_data = (esp_ble_gattc_cb_param_t*)evtParam;
+			if (p_data->search_cmpl.status != ESP_GATT_OK){
+				ESP_LOGE(LOG_TAG, "search service failed, error status = %x", p_data->search_cmpl.status);
+				break;
+			}
+#ifndef ARDUINO_ARCH_ESP32
+// commented out just for now to keep backward compatibility			
+			// if(p_data->search_cmpl.searched_service_source == ESP_GATT_SERVICE_FROM_REMOTE_DEVICE) {
+			// 	ESP_LOGI(LOG_TAG, "Get service information from remote device");
+			// } else if (p_data->search_cmpl.searched_service_source == ESP_GATT_SERVICE_FROM_NVS_FLASH) {
+			// 	ESP_LOGI(LOG_TAG, "Get service information from flash");
+			// } else {
+			// 	ESP_LOGI(LOG_TAG, "unknown service source");
+			// }
+#endif
 			m_semaphoreSearchCmplEvt.give(0);
 			break;
 		} // ESP_GATTC_SEARCH_CMPL_EVT
@@ -238,7 +298,8 @@ void BLEClient::gattClientEventHandler(
 				evtParam->search_res.start_handle,
 				evtParam->search_res.end_handle
 			);
-			m_servicesMap.insert(std::pair<std::string, BLERemoteService *>(uuid.toString(), pRemoteService));
+			m_servicesMap.insert(std::pair<std::string, BLERemoteService*>(uuid.toString(), pRemoteService));
+			m_servicesMapByInstID.insert(std::pair<BLERemoteService *, uint16_t>(pRemoteService, evtParam->search_res.srvc_id.inst_id));
 			break;
 		} // ESP_GATTC_SEARCH_RES_EVT
 
@@ -337,7 +398,7 @@ BLERemoteService* BLEClient::getService(BLEUUID uuid) {
 		}
 	} // End of each of the services.
 	ESP_LOGD(LOG_TAG, "<< getService: not found");
-	throw new BLEUuidNotFoundException;
+	return nullptr;
 } // getService
 
 
@@ -356,14 +417,15 @@ std::map<std::string, BLERemoteService*>* BLEClient::getServices() {
  * and will culminate with an ESP_GATTC_SEARCH_CMPL_EVT when all have been received.
  */
 	ESP_LOGD(LOG_TAG, ">> getServices");
-
+// TODO implement retrieving services from cache
 	clearServices(); // Clear any services that may exist.
 
 	esp_err_t errRc = esp_ble_gattc_search_service(
 		getGattcIf(),
 		getConnId(),
-		nullptr            // Filter UUID
+		NULL            // Filter UUID
 	);
+
 	m_semaphoreSearchCmplEvt.take("getServices");
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gattc_search_service: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -400,7 +462,7 @@ void BLEClient::handleGAPEvent(
 		esp_gap_ble_cb_event_t  event,
 		esp_ble_gap_cb_param_t* param) {
 	ESP_LOGD(LOG_TAG, "BLEClient ... handling GAP event!");
-	switch(event) {
+	switch (event) {
 		//
 		// ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT
 		//
@@ -410,7 +472,7 @@ void BLEClient::handleGAPEvent(
 		// - esp_bd_addr_t remote_addr
 		//
 		case ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT: {
-			m_semaphoreRssiCmplEvt.give((uint32_t)param->read_rssi_cmpl.rssi);
+			m_semaphoreRssiCmplEvt.give((uint32_t) param->read_rssi_cmpl.rssi);
 			break;
 		} // ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT
 
@@ -451,6 +513,9 @@ void BLEClient::setValue(BLEUUID serviceUUID, BLEUUID characteristicUUID, std::s
 	ESP_LOGD(LOG_TAG, "<< setValue");
 } // setValue
 
+uint16_t BLEClient::getMTU() {
+	return m_mtu;
+}
 
 /**
  * @brief Return a string representation of this client.
diff --git a/sensor/patchedBLE/src/BLEClient.h b/sensor/patchedBLE/src/BLEClient.h
index a60ed10..1b8144d 100644
--- a/sensor/patchedBLE/src/BLEClient.h
+++ b/sensor/patchedBLE/src/BLEClient.h
@@ -19,9 +19,11 @@
 #include "BLERemoteService.h"
 #include "BLEService.h"
 #include "BLEAddress.h"
+#include "BLEAdvertisedDevice.h"
 
 class BLERemoteService;
 class BLEClientCallbacks;
+class BLEAdvertisedDevice;
 
 /**
  * @brief A model of a %BLE client.
@@ -31,7 +33,8 @@ public:
 	BLEClient();
 	~BLEClient();
 
-	bool                                       connect(BLEAddress address);   // Connect to the remote BLE Server
+	bool 									   connect(BLEAdvertisedDevice* device);
+	bool                                       connect(BLEAddress address, esp_ble_addr_type_t type = BLE_ADDR_TYPE_PUBLIC);   // Connect to the remote BLE Server
 	void                                       disconnect();                  // Disconnect from the remote BLE Server
 	BLEAddress                                 getPeerAddress();              // Get the address of the remote BLE Server
 	int                                        getRssi();                     // Get the RSSI of the remote BLE Server
@@ -51,8 +54,11 @@ public:
 	void                                       setValue(BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value);   // Set the value of a given characteristic at a given service.
 
 	std::string                                toString();                    // Return a string representation of this client.
+	uint16_t                                   getConnId();
+	esp_gatt_if_t                              getGattcIf();
+	uint16_t								   getMTU();
 
-
+uint16_t m_appId;
 private:
 	friend class BLEDevice;
 	friend class BLERemoteService;
@@ -64,14 +70,12 @@ private:
 		esp_gatt_if_t gattc_if,
 		esp_ble_gattc_cb_param_t* param);
 
-	uint16_t                                   getConnId();
-	esp_gatt_if_t                              getGattcIf();
 	BLEAddress    m_peerAddress = BLEAddress((uint8_t*)"\0\0\0\0\0\0");   // The BD address of the remote server.
 	uint16_t      m_conn_id;
 //	int           m_deviceType;
 	esp_gatt_if_t m_gattc_if;
-	bool          m_haveServices;    // Have we previously obtain the set of services from the remote server.
-	bool          m_isConnected;     // Are we currently connected.
+	bool          m_haveServices = false;    // Have we previously obtain the set of services from the remote server.
+	bool          m_isConnected = false;     // Are we currently connected.
 
 	BLEClientCallbacks* m_pClientCallbacks;
 	FreeRTOS::Semaphore m_semaphoreRegEvt        = FreeRTOS::Semaphore("RegEvt");
@@ -79,8 +83,9 @@ private:
 	FreeRTOS::Semaphore m_semaphoreSearchCmplEvt = FreeRTOS::Semaphore("SearchCmplEvt");
 	FreeRTOS::Semaphore m_semaphoreRssiCmplEvt   = FreeRTOS::Semaphore("RssiCmplEvt");
 	std::map<std::string, BLERemoteService*> m_servicesMap;
+	std::map<BLERemoteService*, uint16_t> m_servicesMapByInstID;
 	void clearServices();   // Clear any existing services.
-
+	uint16_t m_mtu = 23;
 }; // class BLEDevice
 
 
diff --git a/sensor/patchedBLE/src/BLEDescriptor.cpp b/sensor/patchedBLE/src/BLEDescriptor.cpp
index 58ff78b..ba5753d 100644
--- a/sensor/patchedBLE/src/BLEDescriptor.cpp
+++ b/sensor/patchedBLE/src/BLEDescriptor.cpp
@@ -11,16 +11,19 @@
 #include <iomanip>
 #include <stdlib.h>
 #include "sdkconfig.h"
-#include <esp_log.h>
 #include <esp_err.h>
 #include "BLEService.h"
 #include "BLEDescriptor.h"
 #include "GeneralUtils.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEDescriptor";
 #endif
 
-static const char* LOG_TAG = "BLEDescriptor";
+
 
 
 #define NULL_HANDLE (0xffff)
@@ -29,21 +32,21 @@ static const char* LOG_TAG = "BLEDescriptor";
 /**
  * @brief BLEDescriptor constructor.
  */
-BLEDescriptor::BLEDescriptor(const char* uuid) : BLEDescriptor(BLEUUID(uuid)) {
+BLEDescriptor::BLEDescriptor(const char* uuid, uint16_t len) : BLEDescriptor(BLEUUID(uuid), len) {
 }	
 
 /**
  * @brief BLEDescriptor constructor.
  */
-BLEDescriptor::BLEDescriptor(BLEUUID uuid) {
+BLEDescriptor::BLEDescriptor(BLEUUID uuid, uint16_t max_len) {
 	m_bleUUID            = uuid;
-	m_value.attr_value   = (uint8_t *)malloc(ESP_GATT_MAX_ATTR_LEN);  // Allocate storage for the value.
 	m_value.attr_len     = 0;                                         // Initial length is 0.
-	m_value.attr_max_len = ESP_GATT_MAX_ATTR_LEN;                     // Maximum length of the data.
+	m_value.attr_max_len = max_len;                     // Maximum length of the data.
 	m_handle             = NULL_HANDLE;                               // Handle is initially unknown.
 	m_pCharacteristic    = nullptr;                                   // No initial characteristic.
 	m_pCallback          = nullptr;                                   // No initial callback.
 
+	m_value.attr_value   = (uint8_t*) malloc(max_len);  // Allocate storage for the value.
 } // BLEDescriptor
 
 
@@ -70,12 +73,12 @@ void BLEDescriptor::executeCreate(BLECharacteristic* pCharacteristic) {
 	m_pCharacteristic = pCharacteristic; // Save the characteristic associated with this service.
 
 	esp_attr_control_t control;
-	control.auto_rsp = ESP_GATT_RSP_BY_APP;
+	control.auto_rsp = ESP_GATT_AUTO_RSP;
 	m_semaphoreCreateEvt.take("executeCreate");
 	esp_err_t errRc = ::esp_ble_gatts_add_char_descr(
 			pCharacteristic->getService()->getHandle(),
 			getUUID().getNative(),
-			(esp_gatt_perm_t)(ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE),
+			(esp_gatt_perm_t)m_permissions,
 			&m_value,
 			&control);
 	if (errRc != ESP_OK) {
@@ -133,8 +136,8 @@ uint8_t* BLEDescriptor::getValue() {
 void BLEDescriptor::handleGATTServerEvent(
 		esp_gatts_cb_event_t      event,
 		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t *param) {
-	switch(event) {
+		esp_ble_gatts_cb_param_t* param) {
+	switch (event) {
 		// ESP_GATTS_ADD_CHAR_DESCR_EVT
 		//
 		// add_char_descr:
@@ -143,17 +146,6 @@ void BLEDescriptor::handleGATTServerEvent(
 		// - uint16_t          service_handle
 		// - esp_bt_uuid_t     char_uuid
 		case ESP_GATTS_ADD_CHAR_DESCR_EVT: {
-			/*
-			ESP_LOGD(LOG_TAG, "DEBUG: m_pCharacteristic: %x", (uint32_t)m_pCharacteristic);
-			ESP_LOGD(LOG_TAG, "DEBUG: m_bleUUID: %s, add_char_descr.char_uuid: %s, equals: %d",
-				m_bleUUID.toString().c_str(),
-				BLEUUID(param->add_char_descr.char_uuid).toString().c_str(),
-				m_bleUUID.equals(BLEUUID(param->add_char_descr.char_uuid)));
-			ESP_LOGD(LOG_TAG, "DEBUG: service->getHandle: %x, add_char_descr.service_handle: %x",
-					m_pCharacteristic->getService()->getHandle(), param->add_char_descr.service_handle);
-			ESP_LOGD(LOG_TAG, "DEBUG: service->lastCharacteristic: %x",
-					(uint32_t)m_pCharacteristic->getService()->getLastCreatedCharacteristic());
-					*/
 			if (m_pCharacteristic != nullptr &&
 					m_bleUUID.equals(BLEUUID(param->add_char_descr.descr_uuid)) &&
 					m_pCharacteristic->getService()->getHandle() == param->add_char_descr.service_handle &&
@@ -180,23 +172,6 @@ void BLEDescriptor::handleGATTServerEvent(
 			if (param->write.handle == m_handle) {
 				setValue(param->write.value, param->write.len);   // Set the value of the descriptor.
 
-				esp_gatt_rsp_t rsp;   // Build a response.
-				rsp.attr_value.len    = getLength();
-				rsp.attr_value.handle = m_handle;
-				rsp.attr_value.offset = 0;
-				rsp.attr_value.auth_req = ESP_GATT_AUTH_REQ_NONE;
-				memcpy(rsp.attr_value.value, getValue(), rsp.attr_value.len);
-				esp_err_t errRc = ::esp_ble_gatts_send_response(
-						gatts_if,
-						param->write.conn_id,
-						param->write.trans_id,
-						ESP_GATT_OK,
-						&rsp);
-
-				if (errRc != ESP_OK) {   // Check the return code from the send of the response.
-					ESP_LOGE(LOG_TAG, "esp_ble_gatts_send_response: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-				}
-
 				if (m_pCallback != nullptr) {   // We have completed the write, if there is a user supplied callback handler, invoke it now.
 					m_pCallback->onWrite(this);   // Invoke the onWrite callback handler.
 				}
@@ -223,34 +198,13 @@ void BLEDescriptor::handleGATTServerEvent(
 					m_pCallback->onRead(this);    // Invoke the onRead callback method in the callback handler.
 				}
 
-				if (param->read.need_rsp) {   // Do we need a response
-					ESP_LOGD(LOG_TAG, "Sending a response (esp_ble_gatts_send_response)");
-					esp_gatt_rsp_t rsp;
-					rsp.attr_value.len      = getLength();
-					rsp.attr_value.handle   = param->read.handle;
-					rsp.attr_value.offset   = 0;
-					rsp.attr_value.auth_req = ESP_GATT_AUTH_REQ_NONE;
-					memcpy(rsp.attr_value.value, getValue(), rsp.attr_value.len);
-
-					esp_err_t errRc = ::esp_ble_gatts_send_response(
-							gatts_if,
-							param->read.conn_id,
-							param->read.trans_id,
-							ESP_GATT_OK,
-							&rsp);
-
-					if (errRc != ESP_OK) {   // Check the return code from the send of the response.
-						ESP_LOGE(LOG_TAG, "esp_ble_gatts_send_response: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-					}
-				} // End of need a response.
 			} // End of this is our handle
 			break;
 		} // ESP_GATTS_READ_EVT
 
-		default: {
+		default:
 			break;
-		}
-	}// switch event
+	} // switch event
 } // handleGATTServerEvent
 
 
@@ -259,7 +213,7 @@ void BLEDescriptor::handleGATTServerEvent(
  * @param [in] pCallbacks An instance of a callback structure used to define any callbacks for the descriptor.
  */
 void BLEDescriptor::setCallbacks(BLEDescriptorCallbacks* pCallback) {
-	ESP_LOGD(LOG_TAG, ">> setCallbacks: 0x%x", (uint32_t)pCallback);
+	ESP_LOGD(LOG_TAG, ">> setCallbacks: 0x%x", (uint32_t) pCallback);
 	m_pCallback = pCallback;
 	ESP_LOGD(LOG_TAG, "<< setCallbacks");
 } // setCallbacks
@@ -298,7 +252,7 @@ void BLEDescriptor::setValue(uint8_t* data, size_t length) {
  * @param [in] value The value of the descriptor in string form.
  */
 void BLEDescriptor::setValue(std::string value) {
-	setValue((uint8_t *)value.data(), value.length());
+	setValue((uint8_t*) value.data(), value.length());
 } // setValue
 
 void BLEDescriptor::setAccessPermissions(esp_gatt_perm_t perm) {
diff --git a/sensor/patchedBLE/src/BLEDescriptor.h b/sensor/patchedBLE/src/BLEDescriptor.h
index d9e0aef..03cc579 100644
--- a/sensor/patchedBLE/src/BLEDescriptor.h
+++ b/sensor/patchedBLE/src/BLEDescriptor.h
@@ -24,8 +24,8 @@ class BLEDescriptorCallbacks;
  */
 class BLEDescriptor {
 public:
-	BLEDescriptor(const char* uuid);
-	BLEDescriptor(BLEUUID uuid);
+	BLEDescriptor(const char* uuid, uint16_t max_len = 100);
+	BLEDescriptor(BLEUUID uuid, uint16_t max_len = 100);
 	virtual ~BLEDescriptor();
 
 	uint16_t getHandle();                                   // Get the handle of the descriptor.
diff --git a/sensor/patchedBLE/src/BLEDescriptorMap.cpp b/sensor/patchedBLE/src/BLEDescriptorMap.cpp
index 4e372e1..6b84583 100644
--- a/sensor/patchedBLE/src/BLEDescriptorMap.cpp
+++ b/sensor/patchedBLE/src/BLEDescriptorMap.cpp
@@ -21,7 +21,7 @@
  * @return The descriptor.  If not present, then nullptr is returned.
  */
 BLEDescriptor* BLEDescriptorMap::getByUUID(const char* uuid) {
-    return getByUUID(BLEUUID(uuid));
+	return getByUUID(BLEUUID(uuid));
 }
 
 
@@ -32,8 +32,8 @@ BLEDescriptor* BLEDescriptorMap::getByUUID(const char* uuid) {
  */
 BLEDescriptor* BLEDescriptorMap::getByUUID(BLEUUID uuid) {
 	for (auto &myPair : m_uuidMap) {
-		if (myPair.second->getUUID().equals(uuid)) {
-			return myPair.second;
+		if (myPair.first->getUUID().equals(uuid)) {
+			return myPair.first;
 		}
 	}
 	//return m_uuidMap.at(uuid.toString());
@@ -57,8 +57,8 @@ BLEDescriptor* BLEDescriptorMap::getByHandle(uint16_t handle) {
  * @param [in] characteristic The descriptor to cache.
  * @return N/A.
  */
-void BLEDescriptorMap::setByUUID(const char* uuid, BLEDescriptor *pDescriptor){
-	m_uuidMap.insert(std::pair<std::string, BLEDescriptor *>(uuid, pDescriptor));
+void BLEDescriptorMap::setByUUID(const char* uuid, BLEDescriptor* pDescriptor){
+	m_uuidMap.insert(std::pair<BLEDescriptor*, std::string>(pDescriptor, uuid));
 } // setByUUID
 
 
@@ -69,8 +69,8 @@ void BLEDescriptorMap::setByUUID(const char* uuid, BLEDescriptor *pDescriptor){
  * @param [in] characteristic The descriptor to cache.
  * @return N/A.
  */
-void BLEDescriptorMap::setByUUID(BLEUUID uuid, BLEDescriptor *pDescriptor) {
-	m_uuidMap.insert(std::pair<std::string, BLEDescriptor *>(uuid.toString(), pDescriptor));
+void BLEDescriptorMap::setByUUID(BLEUUID uuid, BLEDescriptor* pDescriptor) {
+	m_uuidMap.insert(std::pair<BLEDescriptor*, std::string>(pDescriptor, uuid.toString()));
 } // setByUUID
 
 
@@ -80,9 +80,8 @@ void BLEDescriptorMap::setByUUID(BLEUUID uuid, BLEDescriptor *pDescriptor) {
  * @param [in] descriptor The descriptor to cache.
  * @return N/A.
  */
-void BLEDescriptorMap::setByHandle(uint16_t handle,
-		BLEDescriptor *pDescriptor) {
-	m_handleMap.insert(std::pair<uint16_t, BLEDescriptor *>(handle, pDescriptor));
+void BLEDescriptorMap::setByHandle(uint16_t handle, BLEDescriptor* pDescriptor) {
+	m_handleMap.insert(std::pair<uint16_t, BLEDescriptor*>(handle, pDescriptor));
 } // setByHandle
 
 
@@ -93,13 +92,13 @@ void BLEDescriptorMap::setByHandle(uint16_t handle,
 std::string BLEDescriptorMap::toString() {
 	std::stringstream stringStream;
 	stringStream << std::hex << std::setfill('0');
-	int count=0;
-	for (auto &myPair: m_uuidMap) {
+	int count = 0;
+	for (auto &myPair : m_uuidMap) {
 		if (count > 0) {
 			stringStream << "\n";
 		}
 		count++;
-		stringStream << "handle: 0x" << std::setw(2) << myPair.second->getHandle() << ", uuid: " + myPair.second->getUUID().toString();
+		stringStream << "handle: 0x" << std::setw(2) << myPair.first->getHandle() << ", uuid: " + myPair.first->getUUID().toString();
 	}
 	return stringStream.str();
 } // toString
@@ -114,10 +113,10 @@ std::string BLEDescriptorMap::toString() {
 void BLEDescriptorMap::handleGATTServerEvent(
 		esp_gatts_cb_event_t      event,
 		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t *param) {
+		esp_ble_gatts_cb_param_t* param) {
 	// Invoke the handler for every descriptor we have.
 	for (auto &myPair : m_uuidMap) {
-		myPair.second->handleGATTServerEvent(event, gatts_if, param);
+		myPair.first->handleGATTServerEvent(event, gatts_if, param);
 	}
 } // handleGATTServerEvent
 
@@ -128,10 +127,8 @@ void BLEDescriptorMap::handleGATTServerEvent(
  */
 BLEDescriptor* BLEDescriptorMap::getFirst() {
 	m_iterator = m_uuidMap.begin();
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
-	BLEDescriptor *pRet = m_iterator->second;
+	if (m_iterator == m_uuidMap.end()) return nullptr;
+	BLEDescriptor* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
 } // getFirst
@@ -142,10 +139,8 @@ BLEDescriptor* BLEDescriptorMap::getFirst() {
  * @return The next descriptor in the map.
  */
 BLEDescriptor* BLEDescriptorMap::getNext() {
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
-	BLEDescriptor *pRet = m_iterator->second;
+	if (m_iterator == m_uuidMap.end()) return nullptr;
+	BLEDescriptor* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
 } // getNext
diff --git a/sensor/patchedBLE/src/BLEDevice.cpp b/sensor/patchedBLE/src/BLEDevice.cpp
index 3f31ace..4dbbc6e 100644
--- a/sensor/patchedBLE/src/BLEDevice.cpp
+++ b/sensor/patchedBLE/src/BLEDevice.cpp
@@ -19,7 +19,6 @@
 #include <esp_gattc_api.h>     // ESP32 BLE
 #include <esp_gatt_common_api.h>// ESP32 BLE
 #include <esp_err.h>           // ESP32 ESP-IDF
-#include <esp_log.h>           // ESP32 ESP-IDF
 #include <map>                 // Part of C++ Standard library
 #include <sstream>             // Part of C++ Standard library
 #include <iomanip>             // Part of C++ Standard library
@@ -28,12 +27,19 @@
 #include "BLEClient.h"
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
-#ifdef ARDUINO_ARCH_ESP32
-#include "esp32-hal-log.h"
+
+#if defined(ARDUINO_ARCH_ESP32)
 #include "esp32-hal-bt.h"
 #endif
 
+#if defined(CONFIG_ARDUHAL_ESP_LOG)
+#include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
 static const char* LOG_TAG = "BLEDevice";
+#endif
+
 
 /**
  * Singletons for the BLEDevice.
@@ -41,10 +47,16 @@ static const char* LOG_TAG = "BLEDevice";
 BLEServer* BLEDevice::m_pServer = nullptr;
 BLEScan*   BLEDevice::m_pScan   = nullptr;
 BLEClient* BLEDevice::m_pClient = nullptr;
-bool       initialized          = false;   // Have we been initialized?
-esp_ble_sec_act_t 	BLEDevice::m_securityLevel = (esp_ble_sec_act_t)0;
+bool       initialized          = false;  
+esp_ble_sec_act_t BLEDevice::m_securityLevel = (esp_ble_sec_act_t)0;
 BLESecurityCallbacks* BLEDevice::m_securityCallbacks = nullptr;
-uint16_t   BLEDevice::m_localMTU = 23;
+uint16_t   BLEDevice::m_localMTU = 23;  // not sure if this variable is useful
+BLEAdvertising* BLEDevice::m_bleAdvertising = nullptr;
+uint16_t BLEDevice::m_appId = 0;
+std::map<uint16_t, conn_status_t> BLEDevice::m_connectedClientsMap;
+gap_event_handler BLEDevice::m_customGapHandler = nullptr;
+gattc_event_handler BLEDevice::m_customGattcHandler = nullptr;
+gatts_event_handler BLEDevice::m_customGattsHandler = nullptr;
 
 /**
  * @brief Create a new instance of a client.
@@ -73,7 +85,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 	abort();
 #endif // CONFIG_GATTS_ENABLE
 	m_pServer = new BLEServer();
-	m_pServer->createApp(0);
+	m_pServer->createApp(m_appId++);
 	ESP_LOGD(LOG_TAG, "<< createServer");
 	return m_pServer;
 } // createServer
@@ -97,9 +109,8 @@ uint16_t   BLEDevice::m_localMTU = 23;
 
 	BLEUtils::dumpGattServerEvent(event, gatts_if, param);
 
-	switch(event) {
+	switch (event) {
 		case ESP_GATTS_CONNECT_EVT: {
-			BLEDevice::m_localMTU = 23;
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
 			if(BLEDevice::m_securityLevel){
 				esp_ble_set_encryption(param->connect.remote_bda, BLEDevice::m_securityLevel);
@@ -108,11 +119,6 @@ uint16_t   BLEDevice::m_localMTU = 23;
 			break;
 		} // ESP_GATTS_CONNECT_EVT
 
-		case ESP_GATTS_MTU_EVT: {
-			BLEDevice::m_localMTU = param->mtu.mtu;
-	        ESP_LOGI(LOG_TAG, "ESP_GATTS_MTU_EVT, MTU %d", BLEDevice::m_localMTU);
-	        break;
-		}
 		default: {
 			break;
 		}
@@ -122,6 +128,11 @@ uint16_t   BLEDevice::m_localMTU = 23;
 	if (BLEDevice::m_pServer != nullptr) {
 		BLEDevice::m_pServer->handleGATTServerEvent(event, gatts_if, param);
 	}
+
+	if(m_customGattsHandler != nullptr) {
+		m_customGattsHandler(event, gatts_if, param);
+	}
+
 } // gattServerEventHandler
 
 
@@ -145,31 +156,29 @@ uint16_t   BLEDevice::m_localMTU = 23;
 
 	switch(event) {
 		case ESP_GATTC_CONNECT_EVT: {
-			if(BLEDevice::getMTU() != 23){
-				esp_err_t errRc = esp_ble_gattc_send_mtu_req(gattc_if, param->connect.conn_id);
-				if (errRc != ESP_OK) {
-					ESP_LOGE(LOG_TAG, "esp_ble_gattc_send_mtu_req: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-				}
-			}
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
 			if(BLEDevice::m_securityLevel){
 				esp_ble_set_encryption(param->connect.remote_bda, BLEDevice::m_securityLevel);
 			}
 #endif	// CONFIG_BLE_SMP_ENABLE
 			break;
-		} // ESP_GATTC_CONNECT_EVT
+		} // ESP_GATTS_CONNECT_EVT
 
-		default: {
+		default:
 			break;
-		}
 	} // switch
+	for(auto &myPair : BLEDevice::getPeerDevices(true)) {
+		conn_status_t conn_status = (conn_status_t)myPair.second;
+		if(((BLEClient*)conn_status.peer_device)->getGattcIf() == gattc_if || ((BLEClient*)conn_status.peer_device)->getGattcIf() == ESP_GATT_IF_NONE || gattc_if == ESP_GATT_IF_NONE){
+			((BLEClient*)conn_status.peer_device)->gattClientEventHandler(event, gattc_if, param);
+		}
+	}
 
-
-	// If we have a client registered, call it.
-	if (BLEDevice::m_pClient != nullptr) {
-		BLEDevice::m_pClient->gattClientEventHandler(event, gattc_if, param);
+	if(m_customGattcHandler != nullptr) {
+		m_customGattcHandler(event, gattc_if, param);
 	}
 
+
 } // gattClientEventHandler
 
 
@@ -193,10 +202,10 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		 case ESP_GAP_BLE_LOCAL_ER_EVT:                               /* BLE local ER event */
 			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_LOCAL_ER_EVT");
 			 break;
-		 case ESP_GAP_BLE_NC_REQ_EVT:
+		 case ESP_GAP_BLE_NC_REQ_EVT:								/*  NUMERIC CONFIRMATION  */
 			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_NC_REQ_EVT");
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
-			if(BLEDevice::m_securityCallbacks!=nullptr){
+			if(BLEDevice::m_securityCallbacks != nullptr){
 			 	 esp_ble_confirm_reply(param->ble_security.ble_req.bd_addr, BLEDevice::m_securityCallbacks->onConfirmPIN(param->ble_security.key_notif.passkey));
 			}
 #endif	// CONFIG_BLE_SMP_ENABLE
@@ -205,7 +214,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 			ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_PASSKEY_REQ_EVT: ");
 			// esp_log_buffer_hex(LOG_TAG, m_remote_bda, sizeof(m_remote_bda));
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
-			if(BLEDevice::m_securityCallbacks!=nullptr){
+			if(BLEDevice::m_securityCallbacks != nullptr){
 				esp_ble_passkey_reply(param->ble_security.ble_req.bd_addr, true, BLEDevice::m_securityCallbacks->onPassKeyRequest());
 			}
 #endif	// CONFIG_BLE_SMP_ENABLE
@@ -229,19 +238,19 @@ uint16_t   BLEDevice::m_localMTU = 23;
 			 /*
 			  *
 			  */
-		 case ESP_GAP_BLE_PASSKEY_NOTIF_EVT:  ///the app will receive this evt when the IO  has Output capability and the peer device IO has Input capability.
-			 ///show the passkey number to the user to input it in the peer deivce.
+		 case ESP_GAP_BLE_PASSKEY_NOTIF_EVT:  //the app will receive this evt when the IO  has Output capability and the peer device IO has Input capability.
+			 //display the passkey number to the user to input it in the peer deivce within 30 seconds
 			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_PASSKEY_NOTIF_EVT");
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
+			ESP_LOGI(LOG_TAG, "passKey = %d", param->ble_security.key_notif.passkey);
 			if(BLEDevice::m_securityCallbacks!=nullptr){
-				ESP_LOGI(LOG_TAG, "passKey = %d", param->ble_security.key_notif.passkey);
 				BLEDevice::m_securityCallbacks->onPassKeyNotify(param->ble_security.key_notif.passkey);
 			}
 #endif	// CONFIG_BLE_SMP_ENABLE
 			 break;
 		 case ESP_GAP_BLE_KEY_EVT:
 			 //shows the ble key type info share with peer device to the user.
-			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_KEY_EVT");
+			 ESP_LOGD(LOG_TAG, "ESP_GAP_BLE_KEY_EVT");
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
 			 ESP_LOGI(LOG_TAG, "key type = %s", BLESecurity::esp_key_type_to_str(param->ble_security.ble_key.key_type));
 #endif	// CONFIG_BLE_SMP_ENABLE
@@ -249,7 +258,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		 case ESP_GAP_BLE_AUTH_CMPL_EVT:
 			 ESP_LOGI(LOG_TAG, "ESP_GAP_BLE_AUTH_CMPL_EVT");
 #ifdef CONFIG_BLE_SMP_ENABLE   // Check that BLE SMP (security) is configured in make menuconfig
-			 if(BLEDevice::m_securityCallbacks!=nullptr){
+			 if(BLEDevice::m_securityCallbacks != nullptr){
 				 BLEDevice::m_securityCallbacks->onAuthenticationComplete(param->ble_security.auth_cmpl);
 			 }
 #endif	// CONFIG_BLE_SMP_ENABLE
@@ -259,10 +268,6 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		}
 	} // switch
 
-	if (BLEDevice::m_pServer != nullptr) {
-		BLEDevice::m_pServer->handleGAPEvent(event, param);
-	}
-
 	if (BLEDevice::m_pClient != nullptr) {
 		BLEDevice::m_pClient->handleGAPEvent(event, param);
 	}
@@ -271,10 +276,13 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		BLEDevice::getScan()->handleGAPEvent(event, param);
 	}
 
-	/*
-	 * Security events:
-	 */
+	if(m_bleAdvertising != nullptr) {
+		BLEDevice::getAdvertising()->handleGAPEvent(event, param);
+	}
 
+	if(m_customGapHandler != nullptr) {
+		BLEDevice::m_customGapHandler(event, param);
+	}
 
 } // gapEventHandler
 
@@ -315,7 +323,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
  */
 /* STATIC */ std::string BLEDevice::getValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID) {
 	ESP_LOGD(LOG_TAG, ">> getValue: bdAddress: %s, serviceUUID: %s, characteristicUUID: %s", bdAddress.toString().c_str(), serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
-	BLEClient *pClient = createClient();
+	BLEClient* pClient = createClient();
 	pClient->connect(bdAddress);
 	std::string ret = pClient->getValue(serviceUUID, characteristicUUID);
 	pClient->disconnect();
@@ -330,7 +338,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
  */
 /* STATIC */ void BLEDevice::init(std::string deviceName) {
 	if(!initialized){
-		initialized = true;   // Set the initialization flag to ensure we are only initialized once.
+		initialized = true; // Set the initialization flag to ensure we are only initialized once.
 
 		esp_err_t errRc = ESP_OK;
 #ifdef ARDUINO_ARCH_ESP32
@@ -345,6 +353,9 @@ uint16_t   BLEDevice::m_localMTU = 23;
 			return;
 		}
 
+#ifndef CLASSIC_BT_ENABLED
+		esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);  
+#endif
 		esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
 		errRc = esp_bt_controller_init(&bt_cfg);
 		if (errRc != ESP_OK) {
@@ -353,9 +364,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		}
 
 #ifndef CLASSIC_BT_ENABLED
-	//	esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);  //FIXME waiting for response from esp-idf issue
 		errRc = esp_bt_controller_enable(ESP_BT_MODE_BLE);
-		//errRc = esp_bt_controller_enable(ESP_BT_MODE_BTDM);
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_bt_controller_enable: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 			return;
@@ -370,7 +379,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 #endif
 
 		esp_bluedroid_status_t bt_state = esp_bluedroid_get_status();
-		if (bt_state == ESP_BLUEDROID_STATUS_UNINITIALIZED){
+		if (bt_state == ESP_BLUEDROID_STATUS_UNINITIALIZED) {
 			errRc = esp_bluedroid_init();
 			if (errRc != ESP_OK) {
 				ESP_LOGE(LOG_TAG, "esp_bluedroid_init: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -378,7 +387,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 			}
 		}
 
-		if (bt_state != ESP_BLUEDROID_STATUS_ENABLED){
+		if (bt_state != ESP_BLUEDROID_STATUS_ENABLED) {
 			errRc = esp_bluedroid_enable();
 			if (errRc != ESP_OK) {
 				ESP_LOGE(LOG_TAG, "esp_bluedroid_enable: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -423,7 +432,7 @@ uint16_t   BLEDevice::m_localMTU = 23;
 		};
 #endif // CONFIG_BLE_SMP_ENABLE
 	}
-	vTaskDelay(200/portTICK_PERIOD_MS); // Delay for 200 msecs as a workaround to an apparent Arduino environment issue.
+	vTaskDelay(200 / portTICK_PERIOD_MS); // Delay for 200 msecs as a workaround to an apparent Arduino environment issue.
 } // init
 
 void BLEDevice::uninit() {
@@ -466,7 +475,7 @@ void BLEDevice::uninit() {
  */
 /* STATIC */ void BLEDevice::setValue(BLEAddress bdAddress, BLEUUID serviceUUID, BLEUUID characteristicUUID, std::string value) {
 	ESP_LOGD(LOG_TAG, ">> setValue: bdAddress: %s, serviceUUID: %s, characteristicUUID: %s", bdAddress.toString().c_str(), serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
-	BLEClient *pClient = createClient();
+	BLEClient* pClient = createClient();
 	pClient->connect(bdAddress);
 	pClient->setValue(serviceUUID, characteristicUUID, value);
 	pClient->disconnect();
@@ -534,7 +543,7 @@ void BLEDevice::setSecurityCallbacks(BLESecurityCallbacks* callbacks) {
 esp_err_t BLEDevice::setMTU(uint16_t mtu) {
 	ESP_LOGD(LOG_TAG, ">> setLocalMTU: %d", mtu);
 	esp_err_t err = esp_ble_gatt_set_local_mtu(mtu);
-	if(err == ESP_OK){
+	if (err == ESP_OK) {
 		m_localMTU = mtu;
 	} else {
 		ESP_LOGE(LOG_TAG, "can't set local mtu value: %d", mtu);
@@ -553,4 +562,97 @@ uint16_t BLEDevice::getMTU() {
 bool BLEDevice::getInitialized() {
 	return initialized;
 }
+
+BLEAdvertising* BLEDevice::getAdvertising() {
+	if(m_bleAdvertising == nullptr) {
+		m_bleAdvertising = new BLEAdvertising();
+		ESP_LOGI(LOG_TAG, "create advertising");
+	}
+	ESP_LOGD(LOG_TAG, "get advertising");
+	return m_bleAdvertising; 
+}
+
+void BLEDevice::startAdvertising() {
+	ESP_LOGD(LOG_TAG, ">> startAdvertising");
+	getAdvertising()->start();
+	ESP_LOGD(LOG_TAG, "<< startAdvertising");
+} // startAdvertising
+
+/* multi connect support */
+/* requires a little more work */
+std::map<uint16_t, conn_status_t> BLEDevice::getPeerDevices(bool _client) {
+	return m_connectedClientsMap;
+}
+
+BLEClient* BLEDevice::getClientByGattIf(uint16_t conn_id) {
+	return (BLEClient*)m_connectedClientsMap.find(conn_id)->second.peer_device;
+}
+
+void BLEDevice::updatePeerDevice(void* peer, bool _client, uint16_t conn_id) {
+	ESP_LOGD(LOG_TAG, "update conn_id: %d, GATT role: %s", conn_id, _client? "client":"server");
+	std::map<uint16_t, conn_status_t>::iterator it = m_connectedClientsMap.find(ESP_GATT_IF_NONE);
+	if (it != m_connectedClientsMap.end()) {
+		std::swap(m_connectedClientsMap[conn_id], it->second);
+		m_connectedClientsMap.erase(it);
+	}else{
+		it = m_connectedClientsMap.find(conn_id);
+		if (it != m_connectedClientsMap.end()) {
+			conn_status_t _st = it->second;
+			_st.peer_device = peer;
+			std::swap(m_connectedClientsMap[conn_id], _st);
+		}
+	}
+}
+
+void BLEDevice::addPeerDevice(void* peer, bool _client, uint16_t conn_id) {
+	ESP_LOGI(LOG_TAG, "add conn_id: %d, GATT role: %s", conn_id, _client? "client":"server");
+	conn_status_t status = {
+		.peer_device = peer,
+		.connected = true,
+		.mtu = 23
+	};
+
+	m_connectedClientsMap.insert(std::pair<uint16_t, conn_status_t>(conn_id, status));
+}
+
+void BLEDevice::removePeerDevice(uint16_t conn_id, bool _client) {
+	ESP_LOGI(LOG_TAG, "remove: %d, GATT role %s", conn_id, _client?"client":"server");
+	if(m_connectedClientsMap.find(conn_id) != m_connectedClientsMap.end())
+		m_connectedClientsMap.erase(conn_id);
+}
+
+/* multi connect support */
+
+/**
+ * @brief de-Initialize the %BLE environment.
+ * @param release_memory release the internal BT stack memory
+ */
+/* STATIC */ void BLEDevice::deinit(bool release_memory) {
+    if (!initialized) return;
+
+    esp_bluedroid_disable();
+    esp_bluedroid_deinit();
+    esp_bt_controller_disable();
+    esp_bt_controller_deinit();
+#ifndef ARDUINO_ARCH_ESP32
+    if (release_memory) {
+        esp_bt_controller_mem_release(ESP_BT_MODE_BTDM);  // <-- require tests because we released classic BT memory and this can cause crash (most likely not, esp-idf takes care of it)
+    } else {
+        initialized = false;   
+    } 
+#endif
+}
+
+void BLEDevice::setCustomGapHandler(gap_event_handler handler) {
+	m_customGapHandler = handler;
+}
+
+void BLEDevice::setCustomGattcHandler(gattc_event_handler handler) {
+	m_customGattcHandler = handler;
+}
+
+void BLEDevice::setCustomGattsHandler(gatts_event_handler handler) {
+	m_customGattsHandler = handler;
+}
+
 #endif // CONFIG_BT_ENABLED
diff --git a/sensor/patchedBLE/src/BLEDevice.h b/sensor/patchedBLE/src/BLEDevice.h
index 16c323c..cb4fc17 100644
--- a/sensor/patchedBLE/src/BLEDevice.h
+++ b/sensor/patchedBLE/src/BLEDevice.h
@@ -22,8 +22,12 @@
 #include "BLEAddress.h"
 
 /**
- * @brief %BLE functions.
+ * @brief BLE functions.
  */
+typedef void (*gap_event_handler)(esp_gap_ble_cb_event_t  event, esp_ble_gap_cb_param_t* param);
+typedef void (*gattc_event_handler)(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t* param);
+typedef void (*gatts_event_handler)(esp_gatts_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gatts_cb_param_t* param);
+
 class BLEDevice {
 public:
 
@@ -44,16 +48,31 @@ public:
 	static esp_err_t   setMTU(uint16_t mtu);
 	static uint16_t	   getMTU();
 	static bool        getInitialized(); // Returns the state of the device, is it initialized or not?
+	/* move advertising to BLEDevice for saving ram and flash in beacons */
+	static BLEAdvertising* 	getAdvertising();
+	static void		   		startAdvertising();
+	static uint16_t 	m_appId;
+	/* multi connect */
+	static std::map<uint16_t, conn_status_t> getPeerDevices(bool client);
+	static void addPeerDevice(void* peer, bool is_client, uint16_t conn_id);
+	static void updatePeerDevice(void* peer, bool _client, uint16_t conn_id);
+	static void removePeerDevice(uint16_t conn_id, bool client);
+	static BLEClient* getClientByGattIf(uint16_t conn_id);
+	static void setCustomGapHandler(gap_event_handler handler);
+	static void setCustomGattcHandler(gattc_event_handler handler);
+	static void setCustomGattsHandler(gatts_event_handler handler);
+	static void deinit(bool release_memory = false);
+	static uint16_t		m_localMTU;
+	static esp_ble_sec_act_t 	m_securityLevel;
 
 private:
-	static BLEServer *m_pServer;
-	static BLEScan   *m_pScan;
-	static BLEClient *m_pClient;
-	static esp_ble_sec_act_t 	m_securityLevel;
+	static BLEServer*	m_pServer;
+	static BLEScan*		m_pScan;
+	static BLEClient*	m_pClient;
 	static BLESecurityCallbacks* m_securityCallbacks;
-	static uint16_t		m_localMTU;
-
+	static BLEAdvertising* m_bleAdvertising;
 	static esp_gatt_if_t getGattcIF();
+	static std::map<uint16_t, conn_status_t> m_connectedClientsMap;	
 
 	static void gattClientEventHandler(
 		esp_gattc_cb_event_t      event,
@@ -69,6 +88,12 @@ private:
 		esp_gap_ble_cb_event_t  event,
 		esp_ble_gap_cb_param_t* param);
 
+public:
+/* custom gap and gatt handlers for flexibility */
+	static gap_event_handler m_customGapHandler;
+	static gattc_event_handler m_customGattcHandler;
+	static gatts_event_handler m_customGattsHandler;
+
 }; // class BLE
 
 #endif // CONFIG_BT_ENABLED
diff --git a/sensor/patchedBLE/src/BLEEddystoneTLM.cpp b/sensor/patchedBLE/src/BLEEddystoneTLM.cpp
new file mode 100644
index 0000000..a92bcdb
--- /dev/null
+++ b/sensor/patchedBLE/src/BLEEddystoneTLM.cpp
@@ -0,0 +1,150 @@
+/*
+ * BLEEddystoneTLM.cpp
+ *
+ *  Created on: Mar 12, 2018
+ *      Author: pcbreflux
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+#include <string.h>
+#include <sstream>
+#include <esp_log.h>
+#include "BLEEddystoneTLM.h"
+
+static const char LOG_TAG[] = "BLEEddystoneTLM";
+#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00)>>8) + (((x)&0xFF)<<8))
+#define ENDIAN_CHANGE_U32(x) ((((x)&0xFF000000)>>24) + (((x)&0x00FF0000)>>8)) + ((((x)&0xFF00)<<8) + (((x)&0xFF)<<24))
+
+BLEEddystoneTLM::BLEEddystoneTLM() {
+	beaconUUID = 0xFEAA;
+	m_eddystoneData.frameType = EDDYSTONE_TLM_FRAME_TYPE;
+	m_eddystoneData.version = 0;
+	m_eddystoneData.volt = 3300; // 3300mV = 3.3V
+	m_eddystoneData.temp = (uint16_t) ((float) 23.00);
+	m_eddystoneData.advCount = 0;
+	m_eddystoneData.tmil = 0;
+} // BLEEddystoneTLM
+
+std::string BLEEddystoneTLM::getData() {
+	return std::string((char*) &m_eddystoneData, sizeof(m_eddystoneData));
+} // getData
+
+BLEUUID BLEEddystoneTLM::getUUID() {
+	return BLEUUID(beaconUUID);
+} // getUUID
+
+uint8_t BLEEddystoneTLM::getVersion() {
+	return m_eddystoneData.version;
+} // getVersion
+
+uint16_t BLEEddystoneTLM::getVolt() {
+	return m_eddystoneData.volt;
+} // getVolt
+
+float BLEEddystoneTLM::getTemp() {
+	return (float)m_eddystoneData.temp;
+} // getTemp
+
+uint32_t BLEEddystoneTLM::getCount() {
+	return m_eddystoneData.advCount;
+} // getCount
+
+uint32_t BLEEddystoneTLM::getTime() {
+	return m_eddystoneData.tmil;
+} // getTime
+
+std::string BLEEddystoneTLM::toString() {
+	std::stringstream ss;
+	std::string out = "";
+  uint32_t rawsec;
+  ss << "Version ";
+  ss << std::dec << m_eddystoneData.version;
+  ss << "\n";
+
+  ss << "Battery Voltage ";
+  ss << std::dec << ENDIAN_CHANGE_U16(m_eddystoneData.volt);
+  ss << " mV\n";
+
+  ss << "Temperature ";
+  ss << (float) m_eddystoneData.temp;
+  ss << " °C\n";
+
+  ss << "Adv. Count ";
+  ss << std::dec << ENDIAN_CHANGE_U32(m_eddystoneData.advCount);
+
+  ss << "\n";
+
+  ss << "Time ";
+
+  rawsec = ENDIAN_CHANGE_U32(m_eddystoneData.tmil);
+  std::stringstream buffstream;
+  buffstream << "0000";
+  buffstream << std::dec << rawsec / 864000;
+  std::string buff = buffstream.str();
+
+  ss << buff.substr(buff.length() - 4, buff.length());
+  ss << ".";
+
+  buffstream.str("");
+  buffstream.clear();
+  buffstream << "00";
+  buffstream << std::dec << (rawsec / 36000) % 24;
+  buff = buffstream.str();
+  ss << buff.substr(buff.length()-2, buff.length());
+  ss << ":";
+
+  buffstream.str("");
+  buffstream.clear();
+  buffstream << "00";
+  buffstream << std::dec << (rawsec / 600) % 60;
+  buff = buffstream.str();
+  ss << buff.substr(buff.length() - 2, buff.length());
+  ss << ":";
+
+  buffstream.str("");
+  buffstream.clear();
+  buffstream << "00";
+  buffstream << std::dec << (rawsec / 10) % 60;
+  buff = buffstream.str();
+  ss << buff.substr(buff.length() - 2, buff.length());
+  ss << "\n";
+
+  return ss.str();
+} // toString
+
+/**
+ * Set the raw data for the beacon record.
+ */
+void BLEEddystoneTLM::setData(std::string data) {
+	if (data.length() != sizeof(m_eddystoneData)) {
+		ESP_LOGE(LOG_TAG, "Unable to set the data ... length passed in was %d and expected %d", data.length(), sizeof(m_eddystoneData));
+		return;
+	}
+  memcpy(&m_eddystoneData, data.data(), data.length());
+} // setData
+
+void BLEEddystoneTLM::setUUID(BLEUUID l_uuid) {
+	beaconUUID = l_uuid.getNative()->uuid.uuid16;
+} // setUUID
+
+void BLEEddystoneTLM::setVersion(uint8_t version) {
+	m_eddystoneData.version = version;
+} // setVersion
+
+void BLEEddystoneTLM::setVolt(uint16_t volt) {
+	m_eddystoneData.volt = volt;
+} // setVolt
+
+void BLEEddystoneTLM::setTemp(float temp) {
+	m_eddystoneData.temp = (uint16_t)temp;
+} // setTemp
+
+void BLEEddystoneTLM::setCount(uint32_t advCount) {
+	m_eddystoneData.advCount = advCount;
+} // setCount
+
+void BLEEddystoneTLM::setTime(uint32_t tmil) {
+	m_eddystoneData.tmil = tmil;
+} // setTime
+
+#endif
diff --git a/sensor/patchedBLE/src/BLEEddystoneTLM.h b/sensor/patchedBLE/src/BLEEddystoneTLM.h
new file mode 100644
index 0000000..a93e224
--- /dev/null
+++ b/sensor/patchedBLE/src/BLEEddystoneTLM.h
@@ -0,0 +1,51 @@
+/*
+ * BLEEddystoneTLM.cpp
+ *
+ *  Created on: Mar 12, 2018
+ *      Author: pcbreflux
+ */
+
+#ifndef _BLEEddystoneTLM_H_
+#define _BLEEddystoneTLM_H_
+#include "BLEUUID.h"
+
+#define EDDYSTONE_TLM_FRAME_TYPE 0x20
+
+/**
+ * @brief Representation of a beacon.
+ * See:
+ * * https://github.com/google/eddystone
+ */
+class BLEEddystoneTLM {
+public:
+	BLEEddystoneTLM();
+	std::string getData();
+	BLEUUID	 getUUID();
+	uint8_t	 getVersion();
+	uint16_t	getVolt();
+	float	   getTemp();
+	uint32_t	getCount();
+	uint32_t	getTime();
+	std::string toString();
+	void		setData(std::string data);
+	void		setUUID(BLEUUID l_uuid);
+	void		setVersion(uint8_t version);
+	void		setVolt(uint16_t volt);
+	void		setTemp(float temp);
+	void		setCount(uint32_t advCount);
+	void		setTime(uint32_t tmil);
+
+private:
+	uint16_t beaconUUID;
+	struct {
+		uint8_t frameType;
+		uint8_t version;
+		uint16_t volt;
+		uint16_t temp;
+		uint32_t advCount;
+		uint32_t tmil;
+	} __attribute__((packed)) m_eddystoneData;
+
+}; // BLEEddystoneTLM
+
+#endif /* _BLEEddystoneTLM_H_ */
diff --git a/sensor/patchedBLE/src/BLEEddystoneURL.cpp b/sensor/patchedBLE/src/BLEEddystoneURL.cpp
new file mode 100644
index 0000000..af3b674
--- /dev/null
+++ b/sensor/patchedBLE/src/BLEEddystoneURL.cpp
@@ -0,0 +1,148 @@
+/*
+ * BLEEddystoneURL.cpp
+ *
+ *  Created on: Mar 12, 2018
+ *      Author: pcbreflux
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+#include <string.h>
+#include <esp_log.h>
+#include "BLEEddystoneURL.h"
+
+static const char LOG_TAG[] = "BLEEddystoneURL";
+
+BLEEddystoneURL::BLEEddystoneURL() {
+	beaconUUID = 0xFEAA;
+	lengthURL = 0;
+	m_eddystoneData.frameType = EDDYSTONE_URL_FRAME_TYPE;
+	m_eddystoneData.advertisedTxPower = 0;
+	memset(m_eddystoneData.url, 0, sizeof(m_eddystoneData.url));
+} // BLEEddystoneURL
+
+std::string BLEEddystoneURL::getData() {
+	return std::string((char*) &m_eddystoneData, sizeof(m_eddystoneData));
+} // getData
+
+BLEUUID BLEEddystoneURL::getUUID() {
+	return BLEUUID(beaconUUID);
+} // getUUID
+
+int8_t BLEEddystoneURL::getPower() {
+	return m_eddystoneData.advertisedTxPower;
+} // getPower
+
+std::string BLEEddystoneURL::getURL() {
+	return std::string((char*) &m_eddystoneData.url, sizeof(m_eddystoneData.url));
+} // getURL
+
+std::string BLEEddystoneURL::getDecodedURL() {
+	std::string decodedURL = "";
+
+	switch (m_eddystoneData.url[0]) {
+		case 0x00:
+			decodedURL += "http://www.";
+			break;
+		case 0x01:
+			decodedURL += "https://www.";
+			break;
+		case 0x02:
+			decodedURL += "http://";
+			break;
+		case 0x03:
+			decodedURL += "https://";
+			break;
+		default:
+			decodedURL += m_eddystoneData.url[0];
+	}
+
+	for (int i = 1; i < lengthURL; i++) {
+		if (m_eddystoneData.url[i] > 33 && m_eddystoneData.url[i] < 127) {
+			decodedURL += m_eddystoneData.url[i];
+		} else {
+			switch (m_eddystoneData.url[i]) {
+				case 0x00:
+					decodedURL += ".com/";
+					break;
+				case 0x01:
+					decodedURL += ".org/";
+					break;
+				case 0x02:
+					decodedURL += ".edu/";
+					break;
+				case 0x03:
+					decodedURL += ".net/";
+					break;
+				case 0x04:
+					decodedURL += ".info/";
+					break;
+				case 0x05:
+					decodedURL += ".biz/";
+					break;
+				case 0x06:
+					decodedURL += ".gov/";
+					break;
+				case 0x07:
+					decodedURL += ".com";
+					break;
+				case 0x08:
+					decodedURL += ".org";
+					break;
+				case 0x09:
+					decodedURL += ".edu";
+					break;
+				case 0x0A:
+					decodedURL += ".net";
+					break;
+				case 0x0B:
+					decodedURL += ".info";
+					break;
+				case 0x0C:
+					decodedURL += ".biz";
+					break;
+				case 0x0D:
+					decodedURL += ".gov";
+					break;
+				default:
+					break;
+			}
+		}
+	}
+	return decodedURL;
+} // getDecodedURL
+
+
+
+/**
+ * Set the raw data for the beacon record.
+ */
+void BLEEddystoneURL::setData(std::string data) {
+	if (data.length() > sizeof(m_eddystoneData)) {
+		ESP_LOGE(LOG_TAG, "Unable to set the data ... length passed in was %d and max expected %d", data.length(), sizeof(m_eddystoneData));
+		return;
+	}
+	memset(&m_eddystoneData, 0, sizeof(m_eddystoneData));
+	memcpy(&m_eddystoneData, data.data(), data.length());
+	lengthURL = data.length() - (sizeof(m_eddystoneData) - sizeof(m_eddystoneData.url));
+} // setData
+
+void BLEEddystoneURL::setUUID(BLEUUID l_uuid) {
+	beaconUUID = l_uuid.getNative()->uuid.uuid16;
+} // setUUID
+
+void BLEEddystoneURL::setPower(int8_t advertisedTxPower) {
+	m_eddystoneData.advertisedTxPower = advertisedTxPower;
+} // setPower
+
+void BLEEddystoneURL::setURL(std::string url) {
+  if (url.length() > sizeof(m_eddystoneData.url)) {
+	ESP_LOGE(LOG_TAG, "Unable to set the url ... length passed in was %d and max expected %d", url.length(), sizeof(m_eddystoneData.url));
+	return;
+  }
+  memset(m_eddystoneData.url, 0, sizeof(m_eddystoneData.url));
+  memcpy(m_eddystoneData.url, url.data(), url.length());
+  lengthURL = url.length();
+} // setURL
+
+
+#endif
diff --git a/sensor/patchedBLE/src/BLEEddystoneURL.h b/sensor/patchedBLE/src/BLEEddystoneURL.h
new file mode 100644
index 0000000..0b538c0
--- /dev/null
+++ b/sensor/patchedBLE/src/BLEEddystoneURL.h
@@ -0,0 +1,43 @@
+/*
+ * BLEEddystoneURL.cpp
+ *
+ *  Created on: Mar 12, 2018
+ *      Author: pcbreflux
+ */
+
+#ifndef _BLEEddystoneURL_H_
+#define _BLEEddystoneURL_H_
+#include "BLEUUID.h"
+
+#define EDDYSTONE_URL_FRAME_TYPE 0x10
+
+/**
+ * @brief Representation of a beacon.
+ * See:
+ * * https://github.com/google/eddystone
+ */
+class BLEEddystoneURL {
+public:
+	BLEEddystoneURL();
+	std::string getData();
+	BLEUUID	 getUUID();
+	int8_t	  getPower();
+	std::string getURL();
+	std::string getDecodedURL();
+	void		setData(std::string data);
+	void		setUUID(BLEUUID l_uuid);
+	void		setPower(int8_t advertisedTxPower);
+	void		setURL(std::string url);
+
+private:
+	uint16_t beaconUUID;
+	uint8_t  lengthURL;
+	struct {
+		uint8_t frameType;
+		int8_t  advertisedTxPower;
+		uint8_t url[16];
+	} __attribute__((packed)) m_eddystoneData;
+
+}; // BLEEddystoneURL
+
+#endif /* _BLEEddystoneURL_H_ */
diff --git a/sensor/patchedBLE/src/BLEExceptions.h b/sensor/patchedBLE/src/BLEExceptions.h
index 369fcaf..ea9db85 100644
--- a/sensor/patchedBLE/src/BLEExceptions.h
+++ b/sensor/patchedBLE/src/BLEExceptions.h
@@ -17,13 +17,13 @@
 
 
 class BLEDisconnectedException : public std::exception {
-	const char *what() const throw () {
+	const char* what() const throw () {
 		return "BLE Disconnected";
 	}
 };
 
 class BLEUuidNotFoundException : public std::exception {
-	const char *what() const throw () {
+	const char* what() const throw () {
 		return "No such UUID";
 	}
 };
diff --git a/sensor/patchedBLE/src/BLEHIDDevice.cpp b/sensor/patchedBLE/src/BLEHIDDevice.cpp
index 29376f3..69e18be 100644
--- a/sensor/patchedBLE/src/BLEHIDDevice.cpp
+++ b/sensor/patchedBLE/src/BLEHIDDevice.cpp
@@ -22,15 +22,15 @@ BLEHIDDevice::BLEHIDDevice(BLEServer* server) {
 	/*
 	 * Mandatory characteristic for device info service
 	 */
-	m_pnpCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t)0x2a50, BLECharacteristic::PROPERTY_READ);
+	m_pnpCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t) 0x2a50, BLECharacteristic::PROPERTY_READ);
 
 	/*
 	 * Mandatory characteristics for HID service
 	 */
-	m_hidInfoCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4a, BLECharacteristic::PROPERTY_READ);
-	m_reportMapCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4b, BLECharacteristic::PROPERTY_READ);
-	m_hidControlCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4c, BLECharacteristic::PROPERTY_WRITE_NR);
-	m_protocolModeCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4e, BLECharacteristic::PROPERTY_WRITE_NR | BLECharacteristic::PROPERTY_READ);
+	m_hidInfoCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4a, BLECharacteristic::PROPERTY_READ);
+	m_reportMapCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4b, BLECharacteristic::PROPERTY_READ);
+	m_hidControlCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4c, BLECharacteristic::PROPERTY_WRITE_NR);
+	m_protocolModeCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4e, BLECharacteristic::PROPERTY_WRITE_NR | BLECharacteristic::PROPERTY_READ);
 
 	/*
 	 * Mandatory battery level characteristic with notification and presence descriptor
@@ -40,7 +40,7 @@ BLEHIDDevice::BLEHIDDevice(BLEServer* server) {
 	batteryLevelDescriptor->setNamespace(1);
 	batteryLevelDescriptor->setUnit(0x27ad);
 
-	m_batteryLevelCharacteristic = m_batteryService->createCharacteristic((uint16_t)0x2a19, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+	m_batteryLevelCharacteristic = m_batteryService->createCharacteristic((uint16_t) 0x2a19, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
 	m_batteryLevelCharacteristic->addDescriptor(batteryLevelDescriptor);
 	m_batteryLevelCharacteristic->addDescriptor(new BLE2902());
 
@@ -48,8 +48,8 @@ BLEHIDDevice::BLEHIDDevice(BLEServer* server) {
 	 * This value is setup here because its default value in most usage cases, its very rare to use boot mode
 	 * and we want to simplify library using as much as possible
 	 */
-	const uint8_t pMode[] = {0x01};
-	protocolMode()->setValue((uint8_t*)pMode, 1);
+	const uint8_t pMode[] = { 0x01 };
+	protocolMode()->setValue((uint8_t*) pMode, 1);
 }
 
 BLEHIDDevice::~BLEHIDDevice() {
@@ -74,8 +74,8 @@ void BLEHIDDevice::startServices() {
 /*
  * @brief Create manufacturer characteristic (this characteristic is optional)
  */
-BLECharacteristic* 	BLEHIDDevice::manufacturer() {
-	m_manufacturerCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t)0x2a29, BLECharacteristic::PROPERTY_READ);
+BLECharacteristic* BLEHIDDevice::manufacturer() {
+	m_manufacturerCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t) 0x2a29, BLECharacteristic::PROPERTY_READ);
 	return m_manufacturerCharacteristic;
 }
 
@@ -91,7 +91,7 @@ void BLEHIDDevice::manufacturer(std::string name) {
  * @brief
  */
 void BLEHIDDevice::pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version) {
-	uint8_t pnp[] = {sig, (uint8_t)(vid>>8), (uint8_t)vid, (uint8_t)(pid>>8), (uint8_t)pid, (uint8_t)(version>>8), (uint8_t)version};
+	uint8_t pnp[] = { sig, (uint8_t) (vid >> 8), (uint8_t) vid, (uint8_t) (pid >> 8), (uint8_t) pid, (uint8_t) (version >> 8), (uint8_t) version };
 	m_pnpCharacteristic->setValue(pnp, sizeof(pnp));
 }
 
@@ -99,8 +99,8 @@ void BLEHIDDevice::pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version
  * @brief
  */
 void BLEHIDDevice::hidInfo(uint8_t country, uint8_t flags) {
-	uint8_t info[] = {0x11,0x1, country, flags};
-	m_hidInfoCharacteristic->setValue(info, sizeof(info));;
+	uint8_t info[] = { 0x11, 0x1, country, flags };
+	m_hidInfoCharacteristic->setValue(info, sizeof(info));
 }
 
 /*
@@ -109,12 +109,16 @@ void BLEHIDDevice::hidInfo(uint8_t country, uint8_t flags) {
  * @return pointer to new input report characteristic
  */
 BLECharacteristic* BLEHIDDevice::inputReport(uint8_t reportID) {
-	BLECharacteristic* 	inputReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
-	BLEDescriptor* inputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
-
-	uint8_t desc1_val[] = {reportID, 0x01};
-	inputReportDescriptor->setValue((uint8_t*)desc1_val, 2);
-	inputReportCharacteristic->addDescriptor(new BLE2902());
+	BLECharacteristic* inputReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+	BLEDescriptor* inputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t) 0x2908));
+	BLE2902* p2902 = new BLE2902();
+	inputReportCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
+	inputReportDescriptor->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
+	p2902->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
+
+	uint8_t desc1_val[] = { reportID, 0x01 };
+	inputReportDescriptor->setValue((uint8_t*) desc1_val, 2);
+	inputReportCharacteristic->addDescriptor(p2902);
 	inputReportCharacteristic->addDescriptor(inputReportDescriptor);
 
 	return inputReportCharacteristic;
@@ -126,11 +130,13 @@ BLECharacteristic* BLEHIDDevice::inputReport(uint8_t reportID) {
  * @return Pointer to new output report characteristic
  */
 BLECharacteristic* BLEHIDDevice::outputReport(uint8_t reportID) {
-	BLECharacteristic* 	outputReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
-	BLEDescriptor* outputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
+	BLECharacteristic* outputReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
+	BLEDescriptor* outputReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t) 0x2908));
+	outputReportCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
+	outputReportDescriptor->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
 
-	uint8_t desc1_val[] = {reportID, 0x02};
-	outputReportDescriptor->setValue((uint8_t*)desc1_val, 2);
+	uint8_t desc1_val[] = { reportID, 0x02 };
+	outputReportDescriptor->setValue((uint8_t*) desc1_val, 2);
 	outputReportCharacteristic->addDescriptor(outputReportDescriptor);
 
 	return outputReportCharacteristic;
@@ -142,11 +148,14 @@ BLECharacteristic* BLEHIDDevice::outputReport(uint8_t reportID) {
  * @return Pointer to new feature report characteristic
  */
 BLECharacteristic* BLEHIDDevice::featureReport(uint8_t reportID) {
-	BLECharacteristic* 	featureReportCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
-	BLEDescriptor* featureReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t)0x2908));
+	BLECharacteristic* featureReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+	BLEDescriptor* featureReportDescriptor = new BLEDescriptor(BLEUUID((uint16_t) 0x2908));
+
+	featureReportCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
+	featureReportDescriptor->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
 
-	uint8_t desc1_val[] = {reportID, 0x03};
-	featureReportDescriptor->setValue((uint8_t*)desc1_val, 2);
+	uint8_t desc1_val[] = { reportID, 0x03 };
+	featureReportDescriptor->setValue((uint8_t*) desc1_val, 2);
 	featureReportCharacteristic->addDescriptor(featureReportDescriptor);
 
 	return featureReportCharacteristic;
@@ -156,7 +165,7 @@ BLECharacteristic* BLEHIDDevice::featureReport(uint8_t reportID) {
  * @brief
  */
 BLECharacteristic* BLEHIDDevice::bootInput() {
-	BLECharacteristic* bootInputCharacteristic = m_hidService->createCharacteristic((uint16_t)0x2a22, BLECharacteristic::PROPERTY_NOTIFY);
+	BLECharacteristic* bootInputCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a22, BLECharacteristic::PROPERTY_NOTIFY);
 	bootInputCharacteristic->addDescriptor(new BLE2902());
 
 	return bootInputCharacteristic;
@@ -166,20 +175,20 @@ BLECharacteristic* BLEHIDDevice::bootInput() {
  * @brief
  */
 BLECharacteristic* BLEHIDDevice::bootOutput() {
-	return m_hidService->createCharacteristic((uint16_t)0x2a32, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
+	return m_hidService->createCharacteristic((uint16_t) 0x2a32, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
 }
 
 /*
  * @brief
  */
-BLECharacteristic* 	BLEHIDDevice::hidControl() {
+BLECharacteristic* BLEHIDDevice::hidControl() {
 	return m_hidControlCharacteristic;
 }
 
 /*
  * @brief
  */
-BLECharacteristic* 	BLEHIDDevice::protocolMode() {
+BLECharacteristic* BLEHIDDevice::protocolMode() {
 	return m_protocolModeCharacteristic;
 }
 
@@ -196,11 +205,11 @@ BLECharacteristic* BLEHIDDevice::batteryLevel() {
 
 
 
-BLECharacteristic* 	BLEHIDDevice::reportMap() {
+BLECharacteristic*	 BLEHIDDevice::reportMap() {
 	return m_reportMapCharacteristic;
 }
 
-BLECharacteristic* 	BLEHIDDevice::pnp() {
+BLECharacteristic*	 BLEHIDDevice::pnp() {
 	return m_pnpCharacteristic;
 }
 
diff --git a/sensor/patchedBLE/src/BLEHIDDevice.h b/sensor/patchedBLE/src/BLEHIDDevice.h
index 319fd42..33e6b46 100644
--- a/sensor/patchedBLE/src/BLEHIDDevice.h
+++ b/sensor/patchedBLE/src/BLEHIDDevice.h
@@ -17,15 +17,15 @@
 #include "BLE2902.h"
 #include "HIDTypes.h"
 
-#define GENERIC_HID		960
-#define HID_KEYBOARD	961
-#define HID_MOUSE		962
-#define HID_JOYSTICK	963
-#define HID_GAMEPAD		964
-#define HID_TABLET		965
-#define HID_CARD_READER	966
-#define HID_DIGITAL_PEN	967
-#define HID_BARCODE		968
+#define GENERIC_HID		0x03C0
+#define HID_KEYBOARD	   0x03C1
+#define HID_MOUSE		  0x03C2
+#define HID_JOYSTICK	   0x03C3
+#define HID_GAMEPAD		0x03C4
+#define HID_TABLET		 0x03C5
+#define HID_CARD_READER	0x03C6
+#define HID_DIGITAL_PEN	0x03C7
+#define HID_BARCODE		0x03C8
 
 class BLEHIDDevice {
 public:
diff --git a/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp b/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
index d9c64c9..b6d36d8 100644
--- a/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
+++ b/sensor/patchedBLE/src/BLERemoteCharacteristic.cpp
@@ -11,7 +11,6 @@
 #if defined(CONFIG_BT_ENABLED)
 
 #include <esp_gattc_api.h>
-#include <esp_log.h>
 #include <esp_err.h>
 
 #include <sstream>
@@ -19,12 +18,15 @@
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
 #include "BLERemoteDescriptor.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLERemoteCharacteristic";   // The logging tag for this class.
 #endif
 
 
-static const char* LOG_TAG = "BLERemoteCharacteristic";   // The logging tag for this class.
 
 /**
  * @brief Constructor.
@@ -146,12 +148,8 @@ static bool compareGattId(esp_gatt_id_t id1, esp_gatt_id_t id2) {
  * @param [in] evtParam Payload data for the event.
  * @returns N/A
  */
-void BLERemoteCharacteristic::gattClientEventHandler(
-	esp_gattc_cb_event_t      event,
-	esp_gatt_if_t             gattc_if,
-	esp_ble_gattc_cb_param_t* evtParam) {
+void BLERemoteCharacteristic::gattClientEventHandler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t* evtParam) {
 	switch(event) {
-		//
 		// ESP_GATTC_NOTIFY_EVT
 		//
 		// notify
@@ -165,25 +163,15 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		// We have received a notification event which means that the server wishes us to know about a notification
 		// piece of data.  What we must now do is find the characteristic with the associated handle and then
 		// invoke its notification callback (if it has one).
-		//
 		case ESP_GATTC_NOTIFY_EVT: {
-			if (evtParam->notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->notify.handle != getHandle()) break;
 			if (m_notifyCallback != nullptr) {
 				ESP_LOGD(LOG_TAG, "Invoking callback for notification on characteristic %s", toString().c_str());
-				m_notifyCallback(
-					this,
-					evtParam->notify.value,
-					evtParam->notify.value_len,
-					evtParam->notify.is_notify
-				);
+				m_notifyCallback(this, evtParam->notify.value, evtParam->notify.value_len, evtParam->notify.is_notify);
 			} // End we have a callback function ...
 			break;
 		} // ESP_GATTC_NOTIFY_EVT
 
-
-		//
 		// ESP_GATTC_READ_CHAR_EVT
 		// This event indicates that the server has responded to the read request.
 		//
@@ -193,17 +181,17 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		// - uint16_t           handle
 		// - uint8_t*           value
 		// - uint16_t           value_len
-	  //
 		case ESP_GATTC_READ_CHAR_EVT: {
 			// If this event is not for us, then nothing further to do.
-			if (evtParam->read.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->read.handle != getHandle()) break;
 
 			// At this point, we have determined that the event is for us, so now we save the value
 			// and unlock the semaphore to ensure that the requestor of the data can continue.
 			if (evtParam->read.status == ESP_GATT_OK) {
-				m_value = std::string((char*)evtParam->read.value, evtParam->read.value_len);
+				m_value = std::string((char*) evtParam->read.value, evtParam->read.value_len);
+				if(m_rawData != nullptr) free(m_rawData);
+				m_rawData = (uint8_t*) calloc(evtParam->read.value_len, sizeof(uint8_t));
+				memcpy(m_rawData, evtParam->read.value, evtParam->read.value_len);
 			} else {
 				m_value = "";
 			}
@@ -212,56 +200,41 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 			break;
 		} // ESP_GATTC_READ_CHAR_EVT
 
-
-		//
 		// ESP_GATTC_REG_FOR_NOTIFY_EVT
 		//
 		// reg_for_notify:
 		// - esp_gatt_status_t status
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_REG_FOR_NOTIFY_EVT: {
 			// If the request is not for this BLERemoteCharacteristic then move on to the next.
-			if (evtParam->reg_for_notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->reg_for_notify.handle != getHandle()) break;
 
 			// We have processed the notify registration and can unlock the semaphore.
 			m_semaphoreRegForNotifyEvt.give();
 			break;
 		} // ESP_GATTC_REG_FOR_NOTIFY_EVT
 
-
-		//
 		// ESP_GATTC_UNREG_FOR_NOTIFY_EVT
 		//
 		// unreg_for_notify:
 		// - esp_gatt_status_t status
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_UNREG_FOR_NOTIFY_EVT: {
-			if (evtParam->unreg_for_notify.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->unreg_for_notify.handle != getHandle()) break;
 			// We have processed the notify un-registration and can unlock the semaphore.
 			m_semaphoreRegForNotifyEvt.give();
 			break;
 		} // ESP_GATTC_UNREG_FOR_NOTIFY_EVT:
 
-
-		//
 		// ESP_GATTC_WRITE_CHAR_EVT
 		//
 		// write:
 		// - esp_gatt_status_t status
 		// - uint16_t          conn_id
 		// - uint16_t          handle
-		//
 		case ESP_GATTC_WRITE_CHAR_EVT: {
 			// Determine if this event is for us and, if not, pass onwards.
-			if (evtParam->write.handle != getHandle()) {
-				break;
-			}
+			if (evtParam->write.handle != getHandle()) break;
 
 			// There is nothing further we need to do here.  This is merely an indication
 			// that the write has completed and we can unlock the caller.
@@ -270,9 +243,8 @@ void BLERemoteCharacteristic::gattClientEventHandler(
 		} // ESP_GATTC_WRITE_CHAR_EVT
 
 
-		default: {
+		default:
 			break;
-		}
 	} // End switch
 }; // gattClientEventHandler
 
@@ -289,8 +261,8 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 	// For each descriptor we find, create a BLERemoteDescriptor instance.
 	uint16_t offset = 0;
 	esp_gattc_descr_elem_t result;
-	while(1) {
-		uint16_t count = 1;
+	while(true) {
+		uint16_t count = 10;
 		esp_gatt_status_t status = ::esp_ble_gattc_get_all_descr(
 			getRemoteService()->getClient()->getGattcIf(),
 			getRemoteService()->getClient()->getConnId(),
@@ -309,14 +281,12 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 			break;
 		}
 
-		if (count == 0) {
-			break;
-		}
-		ESP_LOGE(LOG_TAG, "");
+		if (count == 0) break;
+
 		ESP_LOGD(LOG_TAG, "Found a descriptor: Handle: %d, UUID: %s", result.handle, BLEUUID(result.uuid).toString().c_str());
 
 		// We now have a new characteristic ... let us add that to our set of known characteristics
-		BLERemoteDescriptor *pNewRemoteDescriptor = new BLERemoteDescriptor(
+		BLERemoteDescriptor* pNewRemoteDescriptor = new BLERemoteDescriptor(
 			result.handle,
 			BLEUUID(result.uuid),
 			this
@@ -334,7 +304,7 @@ void BLERemoteCharacteristic::retrieveDescriptors() {
 /**
  * @brief Retrieve the map of descriptors keyed by UUID.
  */
-std::map<std::string, BLERemoteDescriptor *>* BLERemoteCharacteristic::getDescriptors() {
+std::map<std::string, BLERemoteDescriptor*>* BLERemoteCharacteristic::getDescriptors() {
 	return &m_descriptorMap;
 } // getDescriptors
 
@@ -391,7 +361,7 @@ BLEUUID BLERemoteCharacteristic::getUUID() {
  * @brief Read an unsigned 16 bit value
  * @return The unsigned 16 bit value.
  */
-uint16_t BLERemoteCharacteristic::readUInt16(void) {
+uint16_t BLERemoteCharacteristic::readUInt16() {
 	std::string value = readValue();
 	if (value.length() >= 2) {
 		return *(uint16_t*)(value.data());
@@ -404,7 +374,7 @@ uint16_t BLERemoteCharacteristic::readUInt16(void) {
  * @brief Read an unsigned 32 bit value.
  * @return the unsigned 32 bit value.
  */
-uint32_t BLERemoteCharacteristic::readUInt32(void) {
+uint32_t BLERemoteCharacteristic::readUInt32() {
 	std::string value = readValue();
 	if (value.length() >= 4) {
 		return *(uint32_t*)(value.data());
@@ -417,7 +387,7 @@ uint32_t BLERemoteCharacteristic::readUInt32(void) {
  * @brief Read a byte value
  * @return The value as a byte
  */
-uint8_t BLERemoteCharacteristic::readUInt8(void) {
+uint8_t BLERemoteCharacteristic::readUInt8() {
 	std::string value = readValue();
 	if (value.length() >= 1) {
 		return (uint8_t)value[0];
@@ -470,12 +440,7 @@ std::string BLERemoteCharacteristic::readValue() {
  * unregistering a notification.
  * @return N/A.
  */
-void BLERemoteCharacteristic::registerForNotify(
-		void (*notifyCallback)(
-			BLERemoteCharacteristic* pBLERemoteCharacteristic,
-			uint8_t*                 pData,
-			size_t                   length,
-			bool                     isNotify)) {
+void BLERemoteCharacteristic::registerForNotify(notify_callback notifyCallback, bool notifications) {
 	ESP_LOGD(LOG_TAG, ">> registerForNotify(): %s", toString().c_str());
 
 	m_notifyCallback = notifyCallback;   // Save the notification callback.
@@ -492,6 +457,11 @@ void BLERemoteCharacteristic::registerForNotify(
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gattc_register_for_notify: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		}
+
+		uint8_t val[] = {0x01, 0x00};
+		if(!notifications) val[0] = 0x02;
+		BLERemoteDescriptor* desc = getDescriptor(BLEUUID((uint16_t)0x2902));
+		desc->writeValue(val, 2);
 	} // End Register
 	else {   // If we weren't passed a callback function, then this is an unregistration.
 		esp_err_t errRc = ::esp_ble_gattc_unregister_for_notify(
@@ -503,6 +473,10 @@ void BLERemoteCharacteristic::registerForNotify(
 		if (errRc != ESP_OK) {
 			ESP_LOGE(LOG_TAG, "esp_ble_gattc_unregister_for_notify: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
 		}
+
+		uint8_t val[] = {0x00, 0x00};
+		BLERemoteDescriptor* desc = getDescriptor((uint16_t)0x2902);
+		desc->writeValue(val, 2);
 	} // End Unregister
 
 	m_semaphoreRegForNotifyEvt.wait("registerForNotify");
@@ -548,7 +522,32 @@ std::string BLERemoteCharacteristic::toString() {
  * @return N/A.
  */
 void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
-	ESP_LOGD(LOG_TAG, ">> writeValue(), length: %d", newValue.length());
+	writeValue((uint8_t*)newValue.c_str(), strlen(newValue.c_str()), response);
+} // writeValue
+
+
+/**
+ * @brief Write the new value for the characteristic.
+ *
+ * This is a convenience function.  Many BLE characteristics are a single byte of data.
+ * @param [in] newValue The new byte value to write.
+ * @param [in] response Whether we require a response from the write.
+ * @return N/A.
+ */
+void BLERemoteCharacteristic::writeValue(uint8_t newValue, bool response) {
+	writeValue(&newValue, 1, response);
+} // writeValue
+
+
+/**
+ * @brief Write the new value for the characteristic from a data buffer.
+ * @param [in] data A pointer to a data buffer.
+ * @param [in] length The length of the data in the data buffer.
+ * @param [in] response Whether we require a response from the write.
+ */
+void BLERemoteCharacteristic::writeValue(uint8_t* data, size_t length, bool response) {
+	// writeValue(std::string((char*)data, length), response);
+	ESP_LOGD(LOG_TAG, ">> writeValue(), length: %d", length);
 
 	// Check to see that we are connected.
 	if (!getRemoteService()->getClient()->isConnected()) {
@@ -557,14 +556,13 @@ void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
 	}
 
 	m_semaphoreWriteCharEvt.take("writeValue");
-
 	// Invoke the ESP-IDF API to perform the write.
 	esp_err_t errRc = ::esp_ble_gattc_write_char(
 		m_pRemoteService->getClient()->getGattcIf(),
 		m_pRemoteService->getClient()->getConnId(),
 		getHandle(),
-		newValue.length(),
-		(uint8_t*)newValue.data(),
+		length,
+		data,
 		response?ESP_GATT_WRITE_TYPE_RSP:ESP_GATT_WRITE_TYPE_NO_RSP,
 		ESP_GATT_AUTH_REQ_NONE
 	);
@@ -579,28 +577,12 @@ void BLERemoteCharacteristic::writeValue(std::string newValue, bool response) {
 	ESP_LOGD(LOG_TAG, "<< writeValue");
 } // writeValue
 
-
-/**
- * @brief Write the new value for the characteristic.
- *
- * This is a convenience function.  Many BLE characteristics are a single byte of data.
- * @param [in] newValue The new byte value to write.
- * @param [in] response Whether we require a response from the write.
- * @return N/A.
- */
-void BLERemoteCharacteristic::writeValue(uint8_t newValue, bool response) {
-	writeValue(std::string(reinterpret_cast<char*>(&newValue), 1), response);
-} // writeValue
-
-
 /**
- * @brief Write the new value for the characteristic from a data buffer.
- * @param [in] data A pointer to a data buffer.
- * @param [in] length The length of the data in the data buffer.
- * @param [in] response Whether we require a response from the write.
+ * @brief Read raw data from remote characteristic as hex bytes
+ * @return return pointer data read
  */
-void BLERemoteCharacteristic::writeValue(uint8_t* data, size_t length, bool response) {
-	writeValue(std::string((char *)data, length), response);
-} // writeValue
+uint8_t* BLERemoteCharacteristic::readRawData() {
+	return m_rawData;
+}
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/sensor/patchedBLE/src/BLERemoteCharacteristic.h b/sensor/patchedBLE/src/BLERemoteCharacteristic.h
index 6f23f49..fbcafe8 100644
--- a/sensor/patchedBLE/src/BLERemoteCharacteristic.h
+++ b/sensor/patchedBLE/src/BLERemoteCharacteristic.h
@@ -21,6 +21,7 @@
 
 class BLERemoteService;
 class BLERemoteDescriptor;
+typedef void (*notify_callback)(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify);
 
 /**
  * @brief A model of a remote %BLE characteristic.
@@ -37,18 +38,19 @@ public:
 	bool        canWrite();
 	bool        canWriteNoResponse();
 	BLERemoteDescriptor* getDescriptor(BLEUUID uuid);
-	std::map<std::string, BLERemoteDescriptor *>* getDescriptors();
+	std::map<std::string, BLERemoteDescriptor*>* getDescriptors();
 	uint16_t    getHandle();
 	BLEUUID     getUUID();
-	std::string readValue(void);
-	uint8_t     readUInt8(void);
-	uint16_t    readUInt16(void);
-	uint32_t    readUInt32(void);
-	void        registerForNotify(void (*notifyCallback)(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify));
+	std::string readValue();
+	uint8_t     readUInt8();
+	uint16_t    readUInt16();
+	uint32_t    readUInt32();
+	void        registerForNotify(notify_callback _callback, bool notifications = true);
 	void        writeValue(uint8_t* data, size_t length, bool response = false);
 	void        writeValue(std::string newValue, bool response = false);
 	void        writeValue(uint8_t newValue, bool response = false);
-	std::string toString(void);
+	std::string toString();
+	uint8_t*	readRawData();
 
 private:
 	BLERemoteCharacteristic(uint16_t handle, BLEUUID uuid, esp_gatt_char_prop_t charProp, BLERemoteService* pRemoteService);
@@ -57,11 +59,7 @@ private:
 	friend class BLERemoteDescriptor;
 
 	// Private member functions
-	void gattClientEventHandler(
-		esp_gattc_cb_event_t      event,
-		esp_gatt_if_t             gattc_if,
-		esp_ble_gattc_cb_param_t* evtParam);
-
+	void gattClientEventHandler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t* evtParam);
 
 	BLERemoteService* getRemoteService();
 	void              removeDescriptors();
@@ -76,7 +74,8 @@ private:
 	FreeRTOS::Semaphore  m_semaphoreRegForNotifyEvt  = FreeRTOS::Semaphore("RegForNotifyEvt");
 	FreeRTOS::Semaphore  m_semaphoreWriteCharEvt     = FreeRTOS::Semaphore("WriteCharEvt");
 	std::string          m_value;
-  void (*m_notifyCallback)(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify);
+	uint8_t 			 *m_rawData;
+	notify_callback		 m_notifyCallback;
 
 	// We maintain a map of descriptors owned by this characteristic keyed by a string representation of the UUID.
 	std::map<std::string, BLERemoteDescriptor*> m_descriptorMap;
diff --git a/sensor/patchedBLE/src/BLERemoteDescriptor.cpp b/sensor/patchedBLE/src/BLERemoteDescriptor.cpp
index 4d14ba8..96a8a57 100644
--- a/sensor/patchedBLE/src/BLERemoteDescriptor.cpp
+++ b/sensor/patchedBLE/src/BLERemoteDescriptor.cpp
@@ -9,12 +9,15 @@
 #include <sstream>
 #include "BLERemoteDescriptor.h"
 #include "GeneralUtils.h"
-#include <esp_log.h>
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLERemoteDescriptor";
 #endif
 
-static const char* LOG_TAG = "BLERemoteDescriptor";
+
 
 
 BLERemoteDescriptor::BLERemoteDescriptor(
@@ -55,7 +58,7 @@ BLEUUID BLERemoteDescriptor::getUUID() {
 } // getUUID
 
 
-std::string BLERemoteDescriptor::readValue(void) {
+std::string BLERemoteDescriptor::readValue() {
 	ESP_LOGD(LOG_TAG, ">> readValue: %s", toString().c_str());
 
 	// Check to see that we are connected.
@@ -87,28 +90,28 @@ std::string BLERemoteDescriptor::readValue(void) {
 } // readValue
 
 
-uint8_t BLERemoteDescriptor::readUInt8(void) {
+uint8_t BLERemoteDescriptor::readUInt8() {
 	std::string value = readValue();
 	if (value.length() >= 1) {
-		return (uint8_t)value[0];
+		return (uint8_t) value[0];
 	}
 	return 0;
 } // readUInt8
 
 
-uint16_t BLERemoteDescriptor::readUInt16(void) {
+uint16_t BLERemoteDescriptor::readUInt16() {
 	std::string value = readValue();
 	if (value.length() >= 2) {
-		return *(uint16_t*)(value.data());
+		return *(uint16_t*) value.data();
 	}
 	return 0;
 } // readUInt16
 
 
-uint32_t BLERemoteDescriptor::readUInt32(void) {
+uint32_t BLERemoteDescriptor::readUInt32() {
 	std::string value = readValue();
 	if (value.length() >= 4) {
-		return *(uint32_t*)(value.data());
+		return *(uint32_t*) value.data();
 	}
 	return 0;
 } // readUInt32
@@ -118,7 +121,7 @@ uint32_t BLERemoteDescriptor::readUInt32(void) {
  * @brief Return a string representation of this BLE Remote Descriptor.
  * @retun A string representation of this BLE Remote Descriptor.
  */
-std::string BLERemoteDescriptor::toString(void) {
+std::string BLERemoteDescriptor::toString() {
 	std::stringstream ss;
 	ss << "handle: " << getHandle() << ", uuid: " << getUUID().toString();
 	return ss.str();
@@ -131,10 +134,7 @@ std::string BLERemoteDescriptor::toString(void) {
  * @param [in] length The length of the data to send.
  * @param [in] response True if we expect a response.
  */
-void BLERemoteDescriptor::writeValue(
-		uint8_t* data,
-		size_t   length,
-		bool     response) {
+void BLERemoteDescriptor::writeValue(uint8_t* data, size_t length, bool response) {
 	ESP_LOGD(LOG_TAG, ">> writeValue: %s", toString().c_str());
 	// Check to see that we are connected.
 	if (!getRemoteCharacteristic()->getRemoteService()->getClient()->isConnected()) {
@@ -148,7 +148,7 @@ void BLERemoteDescriptor::writeValue(
 		getHandle(),
 		length,                           // Data length
 		data,                             // Data
-		ESP_GATT_WRITE_TYPE_NO_RSP,
+		response ? ESP_GATT_WRITE_TYPE_RSP : ESP_GATT_WRITE_TYPE_NO_RSP,
 		ESP_GATT_AUTH_REQ_NONE
 	);
 	if (errRc != ESP_OK) {
@@ -163,10 +163,8 @@ void BLERemoteDescriptor::writeValue(
  * @param [in] newValue The data to send to the remote descriptor.
  * @param [in] response True if we expect a response.
  */
-void BLERemoteDescriptor::writeValue(
-		std::string newValue,
-		bool        response) {
-	writeValue(newValue.data(), newValue.length());
+void BLERemoteDescriptor::writeValue(std::string newValue, bool response) {
+	writeValue((uint8_t*) newValue.data(), newValue.length(), response);
 } // writeValue
 
 
@@ -175,9 +173,7 @@ void BLERemoteDescriptor::writeValue(
  * @param [in] The single byte to write.
  * @param [in] True if we expect a response.
  */
-void BLERemoteDescriptor::writeValue(
-		uint8_t newValue,
-		bool    response) {
+void BLERemoteDescriptor::writeValue(uint8_t newValue, bool response) {
 	writeValue(&newValue, 1, response);
 } // writeValue
 
diff --git a/sensor/patchedBLE/src/BLERemoteService.cpp b/sensor/patchedBLE/src/BLERemoteService.cpp
index 78ff991..c2b7d34 100644
--- a/sensor/patchedBLE/src/BLERemoteService.cpp
+++ b/sensor/patchedBLE/src/BLERemoteService.cpp
@@ -11,13 +11,16 @@
 #include "BLERemoteService.h"
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
-#include <esp_log.h>
 #include <esp_err.h>
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLERemoteService";
 #endif
 
-static const char* LOG_TAG = "BLERemoteService";
+
 
 BLERemoteService::BLERemoteService(
 		esp_gatt_id_t srvcId,
@@ -60,8 +63,8 @@ static bool compareSrvcId(esp_gatt_srvc_id_t id1, esp_gatt_srvc_id_t id2) {
 void BLERemoteService::gattClientEventHandler(
 	esp_gattc_cb_event_t      event,
 	esp_gatt_if_t             gattc_if,
-	esp_ble_gattc_cb_param_t *evtParam) {
-	switch(event) {
+	esp_ble_gattc_cb_param_t* evtParam) {
+	switch (event) {
 		//
 		// ESP_GATTC_GET_CHAR_EVT
 		//
@@ -108,13 +111,12 @@ void BLERemoteService::gattClientEventHandler(
 			break;
 		} // ESP_GATTC_GET_CHAR_EVT
 */
-		default: {
+		default:
 			break;
-		}
 	} // switch
 
 	// Send the event to each of the characteristics owned by this service.
-	for (auto &myPair : m_characteristicMap) {
+	for (auto &myPair : m_characteristicMapByHandle) {
 	   myPair.second->gattClientEventHandler(event, gattc_if, evtParam);
 	}
 } // gattClientEventHandler
@@ -130,7 +132,6 @@ BLERemoteCharacteristic* BLERemoteService::getCharacteristic(const char* uuid) {
     return getCharacteristic(BLEUUID(uuid));
 } // getCharacteristic
 	
-	
 /**
  * @brief Get the characteristic object for the UUID.
  * @param [in] uuid Characteristic uuid.
@@ -153,7 +154,8 @@ BLERemoteCharacteristic* BLERemoteService::getCharacteristic(BLEUUID uuid) {
 			return myPair.second;
 		}
 	}
-	throw new BLEUuidNotFoundException();
+	// throw new BLEUuidNotFoundException();  // <-- we dont want exception here, which will cause app crash, we want to search if any characteristic can be found one after another
+	return nullptr;
 } // getCharacteristic
 
 
@@ -163,15 +165,14 @@ BLERemoteCharacteristic* BLERemoteService::getCharacteristic(BLEUUID uuid) {
  * @return N/A
  */
 void BLERemoteService::retrieveCharacteristics() {
-
 	ESP_LOGD(LOG_TAG, ">> getCharacteristics() for service: %s", getUUID().toString().c_str());
 
 	removeCharacteristics(); // Forget any previous characteristics.
 
 	uint16_t offset = 0;
 	esp_gattc_char_elem_t result;
-	while(1) {
-		uint16_t count = 1;
+	while (true) {
+		uint16_t count = 10;  // this value is used as in parameter that allows to search max 10 chars with the same uuid
 		esp_gatt_status_t status = ::esp_ble_gattc_get_all_char(
 			getClient()->getGattcIf(),
 			getClient()->getConnId(),
@@ -206,7 +207,7 @@ void BLERemoteService::retrieveCharacteristics() {
 		);
 
 		m_characteristicMap.insert(std::pair<std::string, BLERemoteCharacteristic*>(pNewRemoteCharacteristic->getUUID().toString(), pNewRemoteCharacteristic));
-
+		m_characteristicMapByHandle.insert(std::pair<uint16_t, BLERemoteCharacteristic*>(result.char_handle, pNewRemoteCharacteristic));
 		offset++;   // Increment our count of number of descriptors found.
 	} // Loop forever (until we break inside the loop).
 
@@ -219,7 +220,7 @@ void BLERemoteService::retrieveCharacteristics() {
  * @brief Retrieve a map of all the characteristics of this service.
  * @return A map of all the characteristics of this service.
  */
-std::map<std::string, BLERemoteCharacteristic *> * BLERemoteService::getCharacteristics() {
+std::map<std::string, BLERemoteCharacteristic*>* BLERemoteService::getCharacteristics() {
 	ESP_LOGD(LOG_TAG, ">> getCharacteristics() for service: %s", getUUID().toString().c_str());
 	// If is possible that we have not read the characteristics associated with the service so do that
 	// now.  The request to retrieve the characteristics by calling "retrieveCharacteristics" is a blocking
@@ -231,6 +232,13 @@ std::map<std::string, BLERemoteCharacteristic *> * BLERemoteService::getCharacte
 	return &m_characteristicMap;
 } // getCharacteristics
 
+/**
+ * @brief This function is designed to get characteristics map when we have multiple characteristics with the same UUID
+ */
+void BLERemoteService::getCharacteristics(std::map<uint16_t, BLERemoteCharacteristic*>* pCharacteristicMap) {
+#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
+	pCharacteristicMap = &m_characteristicMapByHandle;
+}  // Get the characteristics map.
 
 /**
  * @brief Get the client associated with this service.
@@ -292,6 +300,10 @@ void BLERemoteService::removeCharacteristics() {
 	   //m_characteristicMap.erase(myPair.first);  // Should be no need to delete as it will be deleted by the clear
 	}
 	m_characteristicMap.clear();   // Clear the map
+	for (auto &myPair : m_characteristicMapByHandle) {
+	   delete myPair.second;
+	}
+	m_characteristicMapByHandle.clear();   // Clear the map
 } // removeCharacteristics
 
 
diff --git a/sensor/patchedBLE/src/BLERemoteService.h b/sensor/patchedBLE/src/BLERemoteService.h
index 222c6e4..2ab8673 100644
--- a/sensor/patchedBLE/src/BLERemoteService.h
+++ b/sensor/patchedBLE/src/BLERemoteService.h
@@ -26,7 +26,6 @@ class BLERemoteCharacteristic;
  */
 class BLERemoteService {
 public:
-
 	virtual ~BLERemoteService();
 
 	// Public methods
@@ -34,7 +33,8 @@ public:
 	BLERemoteCharacteristic* getCharacteristic(BLEUUID uuid);       // Get the specified characteristic reference.
 	BLERemoteCharacteristic* getCharacteristic(uint16_t uuid);      // Get the specified characteristic reference.
 	std::map<std::string, BLERemoteCharacteristic*>* getCharacteristics();
-	void getCharacteristics(std::map<uint16_t, BLERemoteCharacteristic*>* pCharacteristicMap);  // Get the characteristics map.
+	std::map<uint16_t, BLERemoteCharacteristic*>* getCharacteristicsByHandle();  // Get the characteristics map.
+	void getCharacteristics(std::map<uint16_t, BLERemoteCharacteristic*>* pCharacteristicMap);
 
 	BLEClient*               getClient(void);                                           // Get a reference to the client associated with this service.
 	uint16_t                 getHandle();                                               // Get the handle of this service.
@@ -67,10 +67,10 @@ private:
 	// Properties
 
 	// We maintain a map of characteristics owned by this service keyed by a string representation of the UUID.
-	std::map<std::string, BLERemoteCharacteristic *> m_characteristicMap;
+	std::map<std::string, BLERemoteCharacteristic*> m_characteristicMap;
 
 	// We maintain a map of characteristics owned by this service keyed by a handle.
-	std::map<uint16_t, BLERemoteCharacteristic *> m_characteristicMapByHandle;
+	std::map<uint16_t, BLERemoteCharacteristic*> m_characteristicMapByHandle;
 
 	bool                m_haveCharacteristics; // Have we previously obtained the characteristics.
 	BLEClient*          m_pClient;
diff --git a/sensor/patchedBLE/src/BLEScan.cpp b/sensor/patchedBLE/src/BLEScan.cpp
index 3046b7c..d851a47 100644
--- a/sensor/patchedBLE/src/BLEScan.cpp
+++ b/sensor/patchedBLE/src/BLEScan.cpp
@@ -8,7 +8,6 @@
 #if defined(CONFIG_BT_ENABLED)
 
 
-#include <esp_log.h>
 #include <esp_err.h>
 
 #include <map>
@@ -17,11 +16,15 @@
 #include "BLEScan.h"
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEScan";
 #endif
 
-static const char* LOG_TAG = "BLEScan";
+
 
 
 /**
@@ -50,20 +53,19 @@ void BLEScan::handleGAPEvent(
 
 	switch(event) {
 
-	// ESP_GAP_BLE_SCAN_RESULT_EVT
-	// ---------------------------
-	// scan_rst:
-	// esp_gap_search_evt_t search_evt
-	// esp_bd_addr_t bda
-	// esp_bt_dev_type_t dev_type
-	// esp_ble_addr_type_t ble_addr_type
-	// esp_ble_evt_type_t ble_evt_type
-	// int rssi
-	// uint8_t ble_adv[ESP_BLE_ADV_DATA_LEN_MAX]
-	// int flag
-	// int num_resps
-	// uint8_t adv_data_len
-	// uint8_t scan_rsp_len
+		// ---------------------------
+		// scan_rst:
+		// esp_gap_search_evt_t search_evt
+		// esp_bd_addr_t bda
+		// esp_bt_dev_type_t dev_type
+		// esp_ble_addr_type_t ble_addr_type
+		// esp_ble_evt_type_t ble_evt_type
+		// int rssi
+		// uint8_t ble_adv[ESP_BLE_ADV_DATA_LEN_MAX]
+		// int flag
+		// int num_resps
+		// uint8_t adv_data_len
+		// uint8_t scan_rsp_len
 		case ESP_GAP_BLE_SCAN_RESULT_EVT: {
 
 			switch(param->scan_rst.search_evt) {
@@ -73,11 +75,12 @@ void BLEScan::handleGAPEvent(
 				// Event that indicates that the duration allowed for the search has completed or that we have been
 				// asked to stop.
 				case ESP_GAP_SEARCH_INQ_CMPL_EVT: {
+					ESP_LOGW(LOG_TAG, "ESP_GAP_SEARCH_INQ_CMPL_EVT");
 					m_stopped = true;
+					m_semaphoreScanEnd.give();
 					if (m_scanCompleteCB != nullptr) {
 						m_scanCompleteCB(m_scanResults);
 					}
-					m_semaphoreScanEnd.give();
 					break;
 				} // ESP_GAP_SEARCH_INQ_CMPL_EVT
 
@@ -95,33 +98,37 @@ void BLEScan::handleGAPEvent(
 					BLEAddress advertisedAddress(param->scan_rst.bda);
 					bool found = false;
 
-					for (int i=0; i<m_scanResults.getCount(); i++) {
-						if (m_scanResults.getDevice(i).getAddress().equals(advertisedAddress)) {
-							found = true;
-							break;
-						}
+					if (m_scanResults.m_vectorAdvertisedDevices.count(advertisedAddress.toString()) != 0) {
+						found = true;
 					}
+
 					if (found && !m_wantDuplicates) {  // If we found a previous entry AND we don't want duplicates, then we are done.
 						ESP_LOGD(LOG_TAG, "Ignoring %s, already seen it.", advertisedAddress.toString().c_str());
+						vTaskDelay(1);  // <--- allow to switch task in case we scan infinity and dont have new devices to report, or we are blocked here
 						break;
 					}
 
 					// We now construct a model of the advertised device that we have just found for the first
 					// time.
-					BLEAdvertisedDevice advertisedDevice;
-					advertisedDevice.setAddress(advertisedAddress);
-					advertisedDevice.setRSSI(param->scan_rst.rssi);
-					advertisedDevice.setAdFlag(param->scan_rst.flag);
-					advertisedDevice.parseAdvertisement((uint8_t*)param->scan_rst.ble_adv);
-					advertisedDevice.setScan(this);
+					// ESP_LOG_BUFFER_HEXDUMP(LOG_TAG, (uint8_t*)param->scan_rst.ble_adv, param->scan_rst.adv_data_len + param->scan_rst.scan_rsp_len, ESP_LOG_DEBUG);
+					// ESP_LOGW(LOG_TAG, "bytes length: %d + %d, addr type: %d", param->scan_rst.adv_data_len, param->scan_rst.scan_rsp_len, param->scan_rst.ble_addr_type);
+					BLEAdvertisedDevice *advertisedDevice = new BLEAdvertisedDevice();
+					advertisedDevice->setAddress(advertisedAddress);
+					advertisedDevice->setRSSI(param->scan_rst.rssi);
+					advertisedDevice->setAdFlag(param->scan_rst.flag);
+					advertisedDevice->parseAdvertisement((uint8_t*)param->scan_rst.ble_adv, param->scan_rst.adv_data_len + param->scan_rst.scan_rsp_len);
+					advertisedDevice->setScan(this);
+					advertisedDevice->setAddressType(param->scan_rst.ble_addr_type);
 
-					if (m_pAdvertisedDeviceCallbacks) {
-						m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
+					if (!found) {   // If we have previously seen this device, don't record it again.
+						m_scanResults.m_vectorAdvertisedDevices.insert(std::pair<std::string, BLEAdvertisedDevice*>(advertisedAddress.toString(), advertisedDevice));
 					}
 
-					if (!found) {   // If we have previously seen this device, don't record it again.
-						m_scanResults.m_vectorAdvertisedDevices.push_back(advertisedDevice);
+					if (m_pAdvertisedDeviceCallbacks) {
+						m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
 					}
+					if(found)
+						delete advertisedDevice;
 
 					break;
 				} // ESP_GAP_SEARCH_INQ_RES_EVT
@@ -190,15 +197,23 @@ void BLEScan::setWindow(uint16_t windowMSecs) {
  * @brief Start scanning.
  * @param [in] duration The duration in seconds for which to scan.
  * @param [in] scanCompleteCB A function to be called when scanning has completed.
+ * @param [in] are we continue scan (true) or we want to clear stored devices (false)
  * @return True if scan started or false if there was an error.
  */
-bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults)) {
+bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults), bool is_continue) {
 	ESP_LOGD(LOG_TAG, ">> start(duration=%d)", duration);
 
 	m_semaphoreScanEnd.take(std::string("start"));
 	m_scanCompleteCB = scanCompleteCB;                  // Save the callback to be invoked when the scan completes.
 
-	m_scanResults.m_vectorAdvertisedDevices.clear();
+	//  if we are connecting to devices that are advertising even after being connected, multiconnecting peripherals
+	//  then we should not clear map or we will connect the same device few times
+	if(!is_continue) {  
+		for(auto _dev : m_scanResults.m_vectorAdvertisedDevices){
+			delete _dev.second;
+		}
+		m_scanResults.m_vectorAdvertisedDevices.clear();
+	}
 
 	esp_err_t errRc = ::esp_ble_gap_set_scan_params(&m_scan_params);
 
@@ -228,8 +243,8 @@ bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults)) {
  * @param [in] duration The duration in seconds for which to scan.
  * @return The BLEScanResults.
  */
-BLEScanResults BLEScan::start(uint32_t duration) {
-	if(start(duration, nullptr)) {
+BLEScanResults BLEScan::start(uint32_t duration, bool is_continue) {
+	if(start(duration, nullptr, is_continue)) {
 		m_semaphoreScanEnd.wait("start");   // Wait for the semaphore to release.
 	}
 	return m_scanResults;
@@ -246,17 +261,24 @@ void BLEScan::stop() {
 	esp_err_t errRc = ::esp_ble_gap_stop_scanning();
 
 	m_stopped = true;
+	m_semaphoreScanEnd.give();
 
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gap_stop_scanning: err: %d, text: %s", errRc, GeneralUtils::errorToString(errRc));
 		return;
 	}
 
-	m_semaphoreScanEnd.give();
-
 	ESP_LOGD(LOG_TAG, "<< stop()");
 } // stop
 
+// delete peer device from cache after disconnecting, it is required in case we are connecting to devices with not public address
+void BLEScan::erase(BLEAddress address) {
+	ESP_LOGI(LOG_TAG, "erase device: %s", address.toString().c_str());
+	BLEAdvertisedDevice *advertisedDevice = m_scanResults.m_vectorAdvertisedDevices.find(address.toString())->second;
+	m_scanResults.m_vectorAdvertisedDevices.erase(address.toString());
+	delete advertisedDevice;
+}
+
 
 /**
  * @brief Dump the scan results to the log.
@@ -285,8 +307,25 @@ int BLEScanResults::getCount() {
  * @return The device at the specified index.
  */
 BLEAdvertisedDevice BLEScanResults::getDevice(uint32_t i) {
-	return m_vectorAdvertisedDevices.at(i);
+	uint32_t x = 0;
+	BLEAdvertisedDevice dev = *m_vectorAdvertisedDevices.begin()->second;
+	for (auto it = m_vectorAdvertisedDevices.begin(); it != m_vectorAdvertisedDevices.end(); it++) {
+		dev = *it->second;
+		if (x==i)	break;
+		x++;
+	}
+	return dev;
 }
 
+BLEScanResults BLEScan::getResults() {
+	return m_scanResults;
+}
+
+void BLEScan::clearResults() {
+	for(auto _dev : m_scanResults.m_vectorAdvertisedDevices){
+		delete _dev.second;
+	}
+	m_scanResults.m_vectorAdvertisedDevices.clear();
+}
 
 #endif /* CONFIG_BT_ENABLED */
diff --git a/sensor/patchedBLE/src/BLEScan.h b/sensor/patchedBLE/src/BLEScan.h
index 76c7c7c..2f71a72 100644
--- a/sensor/patchedBLE/src/BLEScan.h
+++ b/sensor/patchedBLE/src/BLEScan.h
@@ -11,7 +11,8 @@
 #if defined(CONFIG_BT_ENABLED)
 #include <esp_gap_ble_api.h>
 
-#include <vector>
+// #include <vector>
+#include <string>
 #include "BLEAdvertisedDevice.h"
 #include "BLEClient.h"
 #include "FreeRTOS.h"
@@ -37,7 +38,7 @@ public:
 
 private:
 	friend BLEScan;
-	std::vector<BLEAdvertisedDevice> m_vectorAdvertisedDevices;
+	std::map<std::string, BLEAdvertisedDevice*> m_vectorAdvertisedDevices;
 };
 
 /**
@@ -53,9 +54,12 @@ public:
 										bool wantDuplicates = false);
 	void           setInterval(uint16_t intervalMSecs);
 	void           setWindow(uint16_t windowMSecs);
-	bool           start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults));
-	BLEScanResults start(uint32_t duration);
+	bool           start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults), bool is_continue = false);
+	BLEScanResults start(uint32_t duration, bool is_continue = false);
 	void           stop();
+	void 		   erase(BLEAddress address);
+	BLEScanResults getResults();
+	void			clearResults();
 
 private:
 	BLEScan();   // One doesn't create a new instance instead one asks the BLEDevice for the singleton.
@@ -67,8 +71,8 @@ private:
 
 
 	esp_ble_scan_params_t         m_scan_params;
-	BLEAdvertisedDeviceCallbacks* m_pAdvertisedDeviceCallbacks;
-	bool                          m_stopped;
+	BLEAdvertisedDeviceCallbacks* m_pAdvertisedDeviceCallbacks = nullptr;
+	bool                          m_stopped = true;
 	FreeRTOS::Semaphore           m_semaphoreScanEnd = FreeRTOS::Semaphore("ScanEnd");
 	BLEScanResults                m_scanResults;
 	bool                          m_wantDuplicates;
diff --git a/sensor/patchedBLE/src/BLESecurity.cpp b/sensor/patchedBLE/src/BLESecurity.cpp
index 4cf964a..921f542 100644
--- a/sensor/patchedBLE/src/BLESecurity.cpp
+++ b/sensor/patchedBLE/src/BLESecurity.cpp
@@ -5,7 +5,7 @@
  *      Author: chegewara
  */
 
-#include <BLESecurity.h>
+#include "BLESecurity.h"
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
 
@@ -65,40 +65,39 @@ void BLESecurity::setKeySize(uint8_t key_size) {
 /**
  * @brief Debug function to display what keys are exchanged by peers
  */
-char* BLESecurity::esp_key_type_to_str(esp_ble_key_type_t key_type)
-{
+char* BLESecurity::esp_key_type_to_str(esp_ble_key_type_t key_type) {
 	char* key_str = nullptr;
-	switch(key_type) {
-	case ESP_LE_KEY_NONE:
-		key_str = (char*)"ESP_LE_KEY_NONE";
-		break;
+	switch (key_type) {
+		case ESP_LE_KEY_NONE:
+			key_str = (char*) "ESP_LE_KEY_NONE";
+			break;
 		case ESP_LE_KEY_PENC:
-			key_str = (char*)"ESP_LE_KEY_PENC";
+			key_str = (char*) "ESP_LE_KEY_PENC";
 			break;
 		case ESP_LE_KEY_PID:
-			key_str = (char*)"ESP_LE_KEY_PID";
+			key_str = (char*) "ESP_LE_KEY_PID";
 			break;
 		case ESP_LE_KEY_PCSRK:
-			key_str = (char*)"ESP_LE_KEY_PCSRK";
+			key_str = (char*) "ESP_LE_KEY_PCSRK";
 			break;
 		case ESP_LE_KEY_PLK:
-			key_str = (char*)"ESP_LE_KEY_PLK";
+			key_str = (char*) "ESP_LE_KEY_PLK";
 			break;
 		case ESP_LE_KEY_LLK:
-			key_str = (char*)"ESP_LE_KEY_LLK";
+			key_str = (char*) "ESP_LE_KEY_LLK";
+			break;
+		case ESP_LE_KEY_LENC:
+			key_str = (char*) "ESP_LE_KEY_LENC";
+			break;
+		case ESP_LE_KEY_LID:
+			key_str = (char*) "ESP_LE_KEY_LID";
+			break;
+		case ESP_LE_KEY_LCSRK:
+			key_str = (char*) "ESP_LE_KEY_LCSRK";
+			break;
+		default:
+			key_str = (char*) "INVALID BLE KEY TYPE";
 			break;
-    case ESP_LE_KEY_LENC:
-    	key_str = (char*)"ESP_LE_KEY_LENC";
-    	break;
-    case ESP_LE_KEY_LID:
-    	key_str = (char*)"ESP_LE_KEY_LID";
-    	break;
-    case ESP_LE_KEY_LCSRK:
-    	key_str = (char*)"ESP_LE_KEY_LCSRK";
-    	break;
-    default:
-    	key_str = (char*)"INVALID BLE KEY TYPE";
-    	break;
 	}
 	return key_str;
 } // esp_key_type_to_str
diff --git a/sensor/patchedBLE/src/BLESecurity.h b/sensor/patchedBLE/src/BLESecurity.h
index 67c41ef..48d09d2 100644
--- a/sensor/patchedBLE/src/BLESecurity.h
+++ b/sensor/patchedBLE/src/BLESecurity.h
@@ -29,6 +29,7 @@ private:
 	uint8_t m_initKey;
 	uint8_t m_respKey;
 	uint8_t m_keySize;
+
 }; // BLESecurity
 
 
@@ -51,20 +52,20 @@ public:
 	 * It requires that our device is capable to display this code to end user
 	 * @param
 	 */
-	virtual void onPassKeyNotify(uint32_t pass_key);
+	virtual void onPassKeyNotify(uint32_t pass_key) = 0;
 
 	/**
 	 * @brief Here we can make decision if we want to let negotiate authorization with peer device or not
 	 * return Return true if we accept this peer device request
 	 */
 
-	virtual bool onSecurityRequest();
+	virtual bool onSecurityRequest() = 0 ;
 	/**
 	 * Provide us information when authentication process is completed
 	 */
-	virtual void onAuthenticationComplete(esp_ble_auth_cmpl_t);
+	virtual void onAuthenticationComplete(esp_ble_auth_cmpl_t) = 0;
 
-	virtual bool onConfirmPIN(uint32_t pin);
+	virtual bool onConfirmPIN(uint32_t pin) = 0;
 }; // BLESecurityCallbacks
 
 #endif // CONFIG_BT_ENABLED
diff --git a/sensor/patchedBLE/src/BLEServer.cpp b/sensor/patchedBLE/src/BLEServer.cpp
index d5c9de6..6a780aa 100644
--- a/sensor/patchedBLE/src/BLEServer.cpp
+++ b/sensor/patchedBLE/src/BLEServer.cpp
@@ -7,11 +7,9 @@
 
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-#include <esp_log.h>
 #include <esp_bt.h>
 #include <esp_bt_main.h>
-#include <esp_gap_ble_api.h>
-//#include <esp_gatts_api.h>
+#include "GeneralUtils.h"
 #include "BLEDevice.h"
 #include "BLEServer.h"
 #include "BLEService.h"
@@ -19,11 +17,15 @@
 #include <string.h>
 #include <string>
 #include <unordered_set>
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEServer";
 #endif
 
-static const char* LOG_TAG = "BLEServer";
+
 
 
 /**
@@ -33,19 +35,17 @@ static const char* LOG_TAG = "BLEServer";
  * the BLEDevice class.
  */
 BLEServer::BLEServer() {
-	m_appId            = -1;
-	m_gatts_if         = -1;
+	m_appId            = ESP_GATT_IF_NONE;
+	m_gatts_if         = ESP_GATT_IF_NONE;
 	m_connectedCount   = 0;
-	m_connId           = -1;
+	m_connId           = ESP_GATT_IF_NONE;
 	m_pServerCallbacks = nullptr;
-
-	//createApp(0);
 } // BLEServer
 
 
 void BLEServer::createApp(uint16_t appId) {
 	m_appId = appId;
-	registerApp();
+	registerApp(appId);
 } // createApp
 
 
@@ -80,12 +80,10 @@ BLEService* BLEServer::createService(BLEUUID uuid, uint32_t numHandles, uint8_t
 	if (m_serviceMap.getByUUID(uuid) != nullptr) {
 		ESP_LOGW(LOG_TAG, "<< Attempt to create a new service with uuid %s but a service with that UUID already exists.",
 			uuid.toString().c_str());
-		//m_semaphoreCreateEvt.give();
-		//return nullptr;
 	}
 
 	BLEService* pService = new BLEService(uuid, numHandles);
-	pService->m_id = inst_id;
+	pService->m_instId = inst_id;
 	m_serviceMap.setByUUID(uuid, pService); // Save a reference to this service being on this server.
 	pService->executeCreate(this);          // Perform the API calls to actually create the service.
 
@@ -97,12 +95,30 @@ BLEService* BLEServer::createService(BLEUUID uuid, uint32_t numHandles, uint8_t
 
 
 /**
+ * @brief Get a %BLE Service by its UUID
+ * @param [in] uuid The UUID of the new service.
+ * @return A reference to the service object.
+ */
+BLEService* BLEServer::getServiceByUUID(const char* uuid) {
+	return m_serviceMap.getByUUID(uuid);
+}
+
+/**
+ * @brief Get a %BLE Service by its UUID
+ * @param [in] uuid The UUID of the new service.
+ * @return A reference to the service object.
+ */
+BLEService* BLEServer::getServiceByUUID(BLEUUID uuid) {
+	return m_serviceMap.getByUUID(uuid);
+}
+
+/**
  * @brief Retrieve the advertising object that can be used to advertise the existence of the server.
  *
  * @return An advertising object.
  */
 BLEAdvertising* BLEServer::getAdvertising() {
-	return &m_bleAdvertising;
+	return BLEDevice::getAdvertising();
 }
 
 uint16_t BLEServer::getConnId() {
@@ -123,42 +139,6 @@ uint16_t BLEServer::getGattsIf() {
 	return m_gatts_if;
 }
 
-/**
- * @brief Handle a received GAP event.
- *
- * @param [in] event
- * @param [in] param
- */
-void BLEServer::handleGAPEvent(
-		esp_gap_ble_cb_event_t  event,
-		esp_ble_gap_cb_param_t* param) {
-	ESP_LOGD(LOG_TAG, "BLEServer ... handling GAP event!");
-	switch(event) {
-		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT: {
-			/*
-			esp_ble_adv_params_t adv_params;
-			adv_params.adv_int_min       = 0x20;
-			adv_params.adv_int_max       = 0x40;
-			adv_params.adv_type          = ADV_TYPE_IND;
-			adv_params.own_addr_type     = BLE_ADDR_TYPE_PUBLIC;
-			adv_params.channel_map       = ADV_CHNL_ALL;
-			adv_params.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
-			ESP_LOGD(tag, "Starting advertising");
-			esp_err_t errRc = ::esp_ble_gap_start_advertising(&adv_params);
-			if (errRc != ESP_OK) {
-				ESP_LOGE(tag, "esp_ble_gap_start_advertising: rc=%d %s", errRc, espToString(errRc));
-				return;
-			}
-			*/
-			break;
-		}
-
-		default:
-			break;
-	}
-} // handleGAPEvent
-
-
 
 /**
  * @brief Handle a GATT Server Event.
@@ -168,17 +148,10 @@ void BLEServer::handleGAPEvent(
  * @param [in] param
  *
  */
-void BLEServer::handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t* param) {
-
+void BLEServer::handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param) {
 	ESP_LOGD(LOG_TAG, ">> handleGATTServerEvent: %s",
 		BLEUtils::gattServerEventTypeToString(event).c_str());
 
-	// Invoke the handler for every Service we have.
-	m_serviceMap.handleGATTServerEvent(event, gatts_if, param);
-
 	switch(event) {
 		// ESP_GATTS_ADD_CHAR_EVT - Indicate that a characteristic was added to the service.
 		// add_char:
@@ -191,19 +164,23 @@ void BLEServer::handleGATTServerEvent(
 			break;
 		} // ESP_GATTS_ADD_CHAR_EVT
 
+		case ESP_GATTS_MTU_EVT:
+			updatePeerMTU(param->mtu.conn_id, param->mtu.mtu);
+			break;
 
 		// ESP_GATTS_CONNECT_EVT
 		// connect:
 		// - uint16_t      conn_id
 		// - esp_bd_addr_t remote_bda
-		// - bool          is_connected
 		//
 		case ESP_GATTS_CONNECT_EVT: {
-			m_connId = param->connect.conn_id; // Save the connection id.
+			m_connId = param->connect.conn_id;
+			addPeerDevice((void*)this, false, m_connId);
 			if (m_pServerCallbacks != nullptr) {
 				m_pServerCallbacks->onConnect(this);
+				m_pServerCallbacks->onConnect(this, param);			
 			}
-			m_connectedCount++;   // Increment the number of connected devices count.
+			m_connectedCount++;   // Increment the number of connected devices count.	
 			break;
 		} // ESP_GATTS_CONNECT_EVT
 
@@ -217,7 +194,7 @@ void BLEServer::handleGATTServerEvent(
 		// * esp_gatt_srvc_id_t service_id
 		//
 		case ESP_GATTS_CREATE_EVT: {
-			BLEService* pService = m_serviceMap.getByUUID(param->create.service_id.id.uuid);
+			BLEService* pService = m_serviceMap.getByUUID(param->create.service_id.id.uuid, param->create.service_id.id.inst_id);  // <--- very big bug for multi services with the same uuid
 			m_serviceMap.setByHandle(param->create.service_handle, pService);
 			m_semaphoreCreateEvt.give();
 			break;
@@ -227,9 +204,9 @@ void BLEServer::handleGATTServerEvent(
 		// ESP_GATTS_DISCONNECT_EVT
 		//
 		// disconnect
-		// - uint16_t      conn_id
-		// - esp_bd_addr_t remote_bda
-		// - bool          is_connected
+		// - uint16_t      					conn_id
+		// - esp_bd_addr_t 					remote_bda
+		// - esp_gatt_conn_reason_t         reason
 		//
 		// If we receive a disconnect event then invoke the callback for disconnects (if one is present).
 		// we also want to start advertising again.
@@ -239,6 +216,7 @@ void BLEServer::handleGATTServerEvent(
 				m_pServerCallbacks->onDisconnect(this);
 			}
 			startAdvertising(); //- do this with some delay from the loop()
+			removePeerDevice(param->disconnect.conn_id, false);
 			break;
 		} // ESP_GATTS_DISCONNECT_EVT
 
@@ -288,10 +266,17 @@ void BLEServer::handleGATTServerEvent(
 			break;
 		}
 
-		default: {
+		case ESP_GATTS_OPEN_EVT:
+			m_semaphoreOpenEvt.give(param->open.status);
+			break;
+
+		default:
 			break;
-		}
 	}
+
+	// Invoke the handler for every Service we have.
+	m_serviceMap.handleGATTServerEvent(event, gatts_if, param);
+
 	ESP_LOGD(LOG_TAG, "<< handleGATTServerEvent");
 } // handleGATTServerEvent
 
@@ -301,7 +286,7 @@ void BLEServer::handleGATTServerEvent(
  *
  * @return N/A
  */
-void BLEServer::registerApp() {
+void BLEServer::registerApp(uint16_t m_appId) {
 	ESP_LOGD(LOG_TAG, ">> registerApp - %d", m_appId);
 	m_semaphoreRegisterAppEvt.take("registerApp"); // Take the mutex, will be released by ESP_GATTS_REG_EVT event.
 	::esp_ble_gatts_app_register(m_appId);
@@ -326,7 +311,7 @@ void BLEServer::setCallbacks(BLEServerCallbacks* pCallbacks) {
 /*
  * Remove service
  */
-void BLEServer::removeService(BLEService *service) {
+void BLEServer::removeService(BLEService* service) {
 	service->stop();
 	service->executeDelete();	
 	m_serviceMap.removeService(service);
@@ -340,10 +325,35 @@ void BLEServer::removeService(BLEService *service) {
  */
 void BLEServer::startAdvertising() {
 	ESP_LOGD(LOG_TAG, ">> startAdvertising");
-	m_bleAdvertising.start();
+	BLEDevice::startAdvertising();
 	ESP_LOGD(LOG_TAG, "<< startAdvertising");
 } // startAdvertising
 
+/**
+ * Allow to connect GATT server to peer device
+ * Probably can be used in ANCS for iPhone
+ */
+bool BLEServer::connect(BLEAddress address) {
+	esp_bd_addr_t addr;
+	memcpy(&addr, address.getNative(), 6);
+	// Perform the open connection request against the target BLE Server.
+	m_semaphoreOpenEvt.take("connect");
+	esp_err_t errRc = ::esp_ble_gatts_open(
+		getGattsIf(),
+		addr, // address
+		1                              // direct connection
+	);
+	if (errRc != ESP_OK) {
+		ESP_LOGE(LOG_TAG, "esp_ble_gattc_open: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
+		return false;
+	}
+
+	uint32_t rc = m_semaphoreOpenEvt.wait("connect");   // Wait for the connection to complete.
+	ESP_LOGD(LOG_TAG, "<< connect(), rc=%d", rc==ESP_GATT_OK);
+	return rc == ESP_GATT_OK;
+} // connect
+
+
 
 void BLEServerCallbacks::onConnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", ">> onConnect(): Default");
@@ -351,6 +361,12 @@ void BLEServerCallbacks::onConnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", "<< onConnect()");
 } // onConnect
 
+void BLEServerCallbacks::onConnect(BLEServer* pServer, esp_ble_gatts_cb_param_t* param) {
+	ESP_LOGD("BLEServerCallbacks", ">> onConnect(): Default");
+	ESP_LOGD("BLEServerCallbacks", "Device: %s", BLEDevice::toString().c_str());
+	ESP_LOGD("BLEServerCallbacks", "<< onConnect()");
+} // onConnect
+
 
 void BLEServerCallbacks::onDisconnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", ">> onDisconnect(): Default");
@@ -358,4 +374,51 @@ void BLEServerCallbacks::onDisconnect(BLEServer* pServer) {
 	ESP_LOGD("BLEServerCallbacks", "<< onDisconnect()");
 } // onDisconnect
 
+/* multi connect support */
+/* TODO do some more tweaks */
+void BLEServer::updatePeerMTU(uint16_t conn_id, uint16_t mtu) {
+	// set mtu in conn_status_t
+	const std::map<uint16_t, conn_status_t>::iterator it = m_connectedServersMap.find(conn_id);
+	if (it != m_connectedServersMap.end()) {
+		it->second.mtu = mtu;
+		std::swap(m_connectedServersMap[conn_id], it->second);
+	}
+}
+
+std::map<uint16_t, conn_status_t> BLEServer::getPeerDevices(bool _client) {
+	return m_connectedServersMap;
+}
+
+
+uint16_t BLEServer::getPeerMTU(uint16_t conn_id) {
+	return m_connectedServersMap.find(conn_id)->second.mtu;
+}
+
+void BLEServer::addPeerDevice(void* peer, bool _client, uint16_t conn_id) {
+	conn_status_t status = {
+		.peer_device = peer,
+		.connected = true,
+		.mtu = 23
+	};
+
+	m_connectedServersMap.insert(std::pair<uint16_t, conn_status_t>(conn_id, status));	
+}
+
+void BLEServer::removePeerDevice(uint16_t conn_id, bool _client) {
+	m_connectedServersMap.erase(conn_id);
+}
+/* multi connect support */
+
+/**
+ * Update connection parameters can be called only after connection has been established
+ */
+void BLEServer::updateConnParams(esp_bd_addr_t remote_bda, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t timeout) {
+	esp_ble_conn_update_params_t conn_params;
+	memcpy(conn_params.bda, remote_bda, sizeof(esp_bd_addr_t));
+	conn_params.latency = latency;
+	conn_params.max_int = maxInterval;    // max_int = 0x20*1.25ms = 40ms
+	conn_params.min_int = minInterval;    // min_int = 0x10*1.25ms = 20ms
+	conn_params.timeout = timeout;    // timeout = 400*10ms = 4000ms
+	esp_ble_gap_update_conn_params(&conn_params); 
+}
 #endif // CONFIG_BT_ENABLED
diff --git a/sensor/patchedBLE/src/BLEServer.h b/sensor/patchedBLE/src/BLEServer.h
index 95c55d5..d39d8bf 100644
--- a/sensor/patchedBLE/src/BLEServer.h
+++ b/sensor/patchedBLE/src/BLEServer.h
@@ -13,6 +13,7 @@
 
 #include <string>
 #include <string.h>
+// #include "BLEDevice.h"
 
 #include "BLEUUID.h"
 #include "BLEAdvertising.h"
@@ -20,8 +21,15 @@
 #include "BLEService.h"
 #include "BLESecurity.h"
 #include "FreeRTOS.h"
+#include "BLEAddress.h"
 
 class BLEServerCallbacks;
+/* TODO possibly refactor this struct */ 
+typedef struct {
+	void *peer_device;		// peer device BLEClient or BLEServer - maybe its better to have 2 structures or union here
+	bool connected;			// do we need it?
+	uint16_t mtu;			// every peer device negotiate own mtu
+} conn_status_t;
 
 
 /**
@@ -31,11 +39,8 @@ class BLEServiceMap {
 public:
 	BLEService* getByHandle(uint16_t handle);
 	BLEService* getByUUID(const char* uuid);	
-	BLEService* getByUUID(BLEUUID uuid);
-	void        handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t* param);
+	BLEService* getByUUID(BLEUUID uuid, uint8_t inst_id = 0);
+	void        handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param);
 	void        setByHandle(uint16_t handle, BLEService* service);
 	void        setByUUID(const char* uuid, BLEService* service);
 	void        setByUUID(BLEUUID uuid, BLEService* service);
@@ -43,6 +48,7 @@ public:
 	BLEService* getFirst();
 	BLEService* getNext();
 	void 		removeService(BLEService *service);
+	int 		getRegisteredServiceCount();
 
 private:
 	std::map<uint16_t, BLEService*>    m_handleMap;
@@ -62,7 +68,21 @@ public:
 	BLEAdvertising* getAdvertising();
 	void            setCallbacks(BLEServerCallbacks* pCallbacks);
 	void            startAdvertising();
-	void 			removeService(BLEService *service);
+	void 			removeService(BLEService* service);
+	BLEService* 	getServiceByUUID(const char* uuid);
+	BLEService* 	getServiceByUUID(BLEUUID uuid);
+	bool 			connect(BLEAddress address);
+	uint16_t		m_appId;
+	void			updateConnParams(esp_bd_addr_t remote_bda, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t timeout);
+
+	/* multi connection support */
+	std::map<uint16_t, conn_status_t> getPeerDevices(bool client);
+	void addPeerDevice(void* peer, bool is_client, uint16_t conn_id);
+	void removePeerDevice(uint16_t conn_id, bool client);
+	BLEServer* getServerByConnId(uint16_t conn_id);
+	void updatePeerMTU(uint16_t connId, uint16_t mtu);
+	uint16_t getPeerMTU(uint16_t conn_id);
+	uint16_t        getConnId();
 
 
 private:
@@ -71,22 +91,22 @@ private:
 	friend class BLECharacteristic;
 	friend class BLEDevice;
 	esp_ble_adv_data_t  m_adv_data;
-	uint16_t            m_appId;
-	BLEAdvertising      m_bleAdvertising;
-  uint16_t						m_connId;
-  uint32_t            m_connectedCount;
-  uint16_t            m_gatts_if;
-	FreeRTOS::Semaphore m_semaphoreRegisterAppEvt = FreeRTOS::Semaphore("RegisterAppEvt");
-	FreeRTOS::Semaphore m_semaphoreCreateEvt = FreeRTOS::Semaphore("CreateEvt");
+	// BLEAdvertising      m_bleAdvertising;
+	uint16_t			m_connId;
+	uint32_t            m_connectedCount;
+	uint16_t            m_gatts_if;
+  	std::map<uint16_t, conn_status_t> m_connectedServersMap;
+
+	FreeRTOS::Semaphore m_semaphoreRegisterAppEvt 	= FreeRTOS::Semaphore("RegisterAppEvt");
+	FreeRTOS::Semaphore m_semaphoreCreateEvt 		= FreeRTOS::Semaphore("CreateEvt");
+	FreeRTOS::Semaphore m_semaphoreOpenEvt   		= FreeRTOS::Semaphore("OpenEvt");
 	BLEServiceMap       m_serviceMap;
-	BLEServerCallbacks* m_pServerCallbacks;
+	BLEServerCallbacks* m_pServerCallbacks = nullptr;
 
 	void            createApp(uint16_t appId);
-	uint16_t        getConnId();
 	uint16_t        getGattsIf();
-	void            handleGAPEvent(esp_gap_ble_cb_event_t event,	esp_ble_gap_cb_param_t *param);
 	void            handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param);
-	void            registerApp();
+	void            registerApp(uint16_t);
 }; // BLEServer
 
 
@@ -104,7 +124,7 @@ public:
 	 * @param [in] pServer A reference to the %BLE server that received the client connection.
 	 */
 	virtual void onConnect(BLEServer* pServer);
-
+	virtual void onConnect(BLEServer* pServer, esp_ble_gatts_cb_param_t *param);
 	/**
 	 * @brief Handle an existing client disconnection.
 	 *
@@ -116,6 +136,5 @@ public:
 }; // BLEServerCallbacks
 
 
-
 #endif /* CONFIG_BT_ENABLED */
 #endif /* COMPONENTS_CPP_UTILS_BLESERVER_H_ */
diff --git a/sensor/patchedBLE/src/BLEService.cpp b/sensor/patchedBLE/src/BLEService.cpp
index 340ea56..3034cf1 100644
--- a/sensor/patchedBLE/src/BLEService.cpp
+++ b/sensor/patchedBLE/src/BLEService.cpp
@@ -11,7 +11,6 @@
 #if defined(CONFIG_BT_ENABLED)
 #include <esp_err.h>
 #include <esp_gatts_api.h>
-#include <esp_log.h>
 
 #include <iomanip>
 #include <sstream>
@@ -22,20 +21,23 @@
 #include "BLEUtils.h"
 #include "GeneralUtils.h"
 
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEService"; // Tag for logging.
 #endif
 
 #define NULL_HANDLE (0xffff)
 
-static const char* LOG_TAG = "BLEService"; // Tag for logging.
 
 /**
  * @brief Construct an instance of the BLEService
  * @param [in] uuid The UUID of the service.
  * @param [in] numHandles The maximum number of handles associated with the service.
  */
-BLEService::BLEService(const char* uuid, uint32_t numHandles) : BLEService(BLEUUID(uuid), numHandles) {
+BLEService::BLEService(const char* uuid, uint16_t numHandles) : BLEService(BLEUUID(uuid), numHandles) {
 }
 
 
@@ -44,7 +46,7 @@ BLEService::BLEService(const char* uuid, uint32_t numHandles) : BLEService(BLEUU
  * @param [in] uuid The UUID of the service.
  * @param [in] numHandles The maximum number of handles associated with the service.
  */
-BLEService::BLEService(BLEUUID uuid, uint32_t numHandles) {
+BLEService::BLEService(BLEUUID uuid, uint16_t numHandles) {
 	m_uuid      = uuid;
 	m_handle    = NULL_HANDLE;
 	m_pServer   = nullptr;
@@ -61,25 +63,16 @@ BLEService::BLEService(BLEUUID uuid, uint32_t numHandles) {
  * @return N/A.
  */
 
-void BLEService::executeCreate(BLEServer *pServer) {
-	//ESP_LOGD(LOG_TAG, ">> executeCreate() - Creating service (esp_ble_gatts_create_service) service uuid: %s", getUUID().toString().c_str());
-	//getUUID(); // Needed for a weird bug fix
-	//char x[1000];
-	//memcpy(x, &m_uuid, sizeof(m_uuid));
-	//char x[10];
-	//memcpy(x, &deleteMe, 10);
+void BLEService::executeCreate(BLEServer* pServer) {
+	ESP_LOGD(LOG_TAG, ">> executeCreate() - Creating service (esp_ble_gatts_create_service) service uuid: %s", getUUID().toString().c_str());
 	m_pServer          = pServer;
 	m_semaphoreCreateEvt.take("executeCreate"); // Take the mutex and release at event ESP_GATTS_CREATE_EVT
 
 	esp_gatt_srvc_id_t srvc_id;
 	srvc_id.is_primary = true;
-	srvc_id.id.inst_id = m_id;
+	srvc_id.id.inst_id = m_instId;
 	srvc_id.id.uuid    = *m_uuid.getNative();
-	esp_err_t errRc = ::esp_ble_gatts_create_service(
-		getServer()->getGattsIf(),
-		&srvc_id,
-		m_numHandles   // The maximum number of handles associated with the service.
-	);
+	esp_err_t errRc = ::esp_ble_gatts_create_service(getServer()->getGattsIf(), &srvc_id, m_numHandles); // The maximum number of handles associated with the service.
 
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gatts_create_service: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -101,7 +94,7 @@ void BLEService::executeDelete() {
 	ESP_LOGD(LOG_TAG, ">> executeDelete()");
 	m_semaphoreDeleteEvt.take("executeDelete"); // Take the mutex and release at event ESP_GATTS_DELETE_EVT
 
-	esp_err_t errRc = ::esp_ble_gatts_delete_service( getHandle() );
+	esp_err_t errRc = ::esp_ble_gatts_delete_service(getHandle());
 
 	if (errRc != ESP_OK) {
 		ESP_LOGE(LOG_TAG, "esp_ble_gatts_delete_service: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -151,10 +144,9 @@ void BLEService::start() {
 		return;
 	}
 
-
 	BLECharacteristic *pCharacteristic = m_characteristicMap.getFirst();
 
-	while(pCharacteristic != nullptr) {
+	while (pCharacteristic != nullptr) {
 		m_lastCreatedCharacteristic = pCharacteristic;
 		pCharacteristic->executeCreate(this);
 
@@ -177,13 +169,11 @@ void BLEService::start() {
 
 /**
  * @brief Stop the service.
- * @return Stop the service.
  */
 void BLEService::stop() {
 // We ask the BLE runtime to start the service and then create each of the characteristics.
 // We start the service through its local handle which was returned in the ESP_GATTS_CREATE_EVT event
 // obtained as a result of calling esp_ble_gatts_create_service().
-//
 	ESP_LOGD(LOG_TAG, ">> stop(): Stopping service (esp_ble_gatts_stop_service): %s", toString().c_str());
 	if (m_handle == NULL_HANDLE) {
 		ESP_LOGE(LOG_TAG, "<< !!! We attempted to stop a service but don't know its handle!");
@@ -232,10 +222,10 @@ uint16_t BLEService::getHandle() {
  * @param [in] pCharacteristic A pointer to the characteristic to be added.
  */
 void BLEService::addCharacteristic(BLECharacteristic* pCharacteristic) {
-// We maintain a mapping of characteristics owned by this service.  These are managed by the
-// BLECharacteristicMap class instance found in m_characteristicMap.  We add the characteristic
-// to the map and then ask the service to add the characteristic at the BLE level (ESP-IDF).
-//
+	// We maintain a mapping of characteristics owned by this service.  These are managed by the
+	// BLECharacteristicMap class instance found in m_characteristicMap.  We add the characteristic
+	// to the map and then ask the service to add the characteristic at the BLE level (ESP-IDF).
+
 	ESP_LOGD(LOG_TAG, ">> addCharacteristic()");
 	ESP_LOGD(LOG_TAG, "Adding characteristic: uuid=%s to service: %s",
 		pCharacteristic->getUUID().toString().c_str(),
@@ -273,7 +263,7 @@ BLECharacteristic* BLEService::createCharacteristic(const char* uuid, uint32_t p
  * @return The new BLE characteristic.
  */
 BLECharacteristic* BLEService::createCharacteristic(BLEUUID uuid, uint32_t properties) {
-	BLECharacteristic *pCharacteristic = new BLECharacteristic(uuid, properties);
+	BLECharacteristic* pCharacteristic = new BLECharacteristic(uuid, properties);
 	addCharacteristic(pCharacteristic);
 	return pCharacteristic;
 } // createCharacteristic
@@ -282,13 +272,8 @@ BLECharacteristic* BLEService::createCharacteristic(BLEUUID uuid, uint32_t prope
 /**
  * @brief Handle a GATTS server event.
  */
-void BLEService::handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t *param) {
-
-
-	switch(event) {
+void BLEService::handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param) {
+	switch (event) {
 		// ESP_GATTS_ADD_CHAR_EVT - Indicate that a characteristic was added to the service.
 		// add_char:
 		// - esp_gatt_status_t status
@@ -309,7 +294,6 @@ void BLEService::handleGATTServerEvent(
 				}
 				pCharacteristic->setHandle(param->add_char.attr_handle);
 				m_characteristicMap.setByHandle(param->add_char.attr_handle, pCharacteristic);
-				//ESP_LOGD(tag, "Characteristic map: %s", m_characteristicMap.toString().c_str());
 				break;
 			} // Reached the correct service.
 			break;
@@ -355,7 +339,7 @@ void BLEService::handleGATTServerEvent(
 		// * - bool is_primary
 		//
 		case ESP_GATTS_CREATE_EVT: {
-			if (getUUID().equals(BLEUUID(param->create.service_id.id.uuid)) && m_id == param->create.service_id.id.inst_id) {
+			if (getUUID().equals(BLEUUID(param->create.service_id.id.uuid)) && m_instId == param->create.service_id.id.inst_id) {
 				setHandle(param->create.service_handle);
 				m_semaphoreCreateEvt.give();
 			}
@@ -377,9 +361,8 @@ void BLEService::handleGATTServerEvent(
 			break;
 		} // ESP_GATTS_DELETE_EVT
 
-		default: {
+		default:
 			break;
-		} // Default
 	} // Switch
 
 	// Invoke the GATTS handler in each of the associated characteristics.
diff --git a/sensor/patchedBLE/src/BLEService.h b/sensor/patchedBLE/src/BLEService.h
index 93b4b2c..b42d57f 100644
--- a/sensor/patchedBLE/src/BLEService.h
+++ b/sensor/patchedBLE/src/BLEService.h
@@ -33,11 +33,7 @@ public:
 	BLECharacteristic* getFirst();
 	BLECharacteristic* getNext();
 	std::string toString();
-	void handleGATTServerEvent(
-			esp_gatts_cb_event_t      event,
-			esp_gatt_if_t             gatts_if,
-			esp_ble_gatts_cb_param_t* param);
-
+	void handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param);
 
 private:
 	std::map<BLECharacteristic*, std::string> m_uuidMap;
@@ -66,11 +62,11 @@ public:
 	void			   stop();
 	std::string        toString();
 	uint16_t           getHandle();
-	uint8_t			   m_id = 0;
+	uint8_t			   m_instId = 0;
 
 private:
-	BLEService(const char* uuid, uint32_t numHandles);
-	BLEService(BLEUUID uuid, uint32_t numHandles);
+	BLEService(const char* uuid, uint16_t numHandles);
+	BLEService(BLEUUID uuid, uint16_t numHandles);
 	friend class BLEServer;
 	friend class BLEServiceMap;
 	friend class BLEDescriptor;
@@ -79,8 +75,8 @@ private:
 
 	BLECharacteristicMap m_characteristicMap;
 	uint16_t             m_handle;
-	BLECharacteristic*   m_lastCreatedCharacteristic;
-	BLEServer*           m_pServer;
+	BLECharacteristic*   m_lastCreatedCharacteristic = nullptr;
+	BLEServer*           m_pServer = nullptr;
 	BLEUUID              m_uuid;
 
 	FreeRTOS::Semaphore  m_semaphoreCreateEvt = FreeRTOS::Semaphore("CreateEvt");
@@ -88,13 +84,10 @@ private:
 	FreeRTOS::Semaphore  m_semaphoreStartEvt  = FreeRTOS::Semaphore("StartEvt");
 	FreeRTOS::Semaphore  m_semaphoreStopEvt   = FreeRTOS::Semaphore("StopEvt");
 
-	uint32_t             m_numHandles;
+	uint16_t             m_numHandles;
 
 	BLECharacteristic* getLastCreatedCharacteristic();
-	void               handleGATTServerEvent(
-		esp_gatts_cb_event_t      event,
-		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t* param);
+	void handleGATTServerEvent(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param);
 	void               setHandle(uint16_t handle);
 	//void               setService(esp_gatt_srvc_id_t srvc_id);
 }; // BLEService
diff --git a/sensor/patchedBLE/src/BLEServiceMap.cpp b/sensor/patchedBLE/src/BLEServiceMap.cpp
index dd828fa..cf4f75f 100644
--- a/sensor/patchedBLE/src/BLEServiceMap.cpp
+++ b/sensor/patchedBLE/src/BLEServiceMap.cpp
@@ -17,15 +17,15 @@
  * @return The characteristic.
  */
 BLEService* BLEServiceMap::getByUUID(const char* uuid) {
-    return getByUUID(BLEUUID(uuid));
+	return getByUUID(BLEUUID(uuid));
 }
-	
+
 /**
  * @brief Return the service by UUID.
  * @param [in] UUID The UUID to look up the service.
  * @return The characteristic.
  */
-BLEService* BLEServiceMap::getByUUID(BLEUUID uuid) {
+BLEService* BLEServiceMap::getByUUID(BLEUUID uuid, uint8_t inst_id) {
 	for (auto &myPair : m_uuidMap) {
 		if (myPair.first->getUUID().equals(uuid)) {
 			return myPair.first;
@@ -52,9 +52,8 @@ BLEService* BLEServiceMap::getByHandle(uint16_t handle) {
  * @param [in] characteristic The service to cache.
  * @return N/A.
  */
-void BLEServiceMap::setByUUID(BLEUUID uuid,
-		BLEService *service) {
-	m_uuidMap.insert(std::pair<BLEService *, std::string>(service, uuid.toString()));
+void BLEServiceMap::setByUUID(BLEUUID uuid, BLEService* service) {
+	m_uuidMap.insert(std::pair<BLEService*, std::string>(service, uuid.toString()));
 } // setByUUID
 
 
@@ -64,9 +63,8 @@ void BLEServiceMap::setByUUID(BLEUUID uuid,
  * @param [in] service The service to cache.
  * @return N/A.
  */
-void BLEServiceMap::setByHandle(uint16_t handle,
-		BLEService* service) {
-	m_handleMap.insert(std::pair<uint16_t, BLEService *>(handle, service));
+void BLEServiceMap::setByHandle(uint16_t handle, BLEService* service) {
+	m_handleMap.insert(std::pair<uint16_t, BLEService*>(handle, service));
 } // setByHandle
 
 
@@ -86,7 +84,7 @@ std::string BLEServiceMap::toString() {
 void BLEServiceMap::handleGATTServerEvent(
 		esp_gatts_cb_event_t      event,
 		esp_gatt_if_t             gatts_if,
-		esp_ble_gatts_cb_param_t *param) {
+		esp_ble_gatts_cb_param_t* param) {
 	// Invoke the handler for every Service we have.
 	for (auto &myPair : m_uuidMap) {
 		myPair.first->handleGATTServerEvent(event, gatts_if, param);
@@ -99,9 +97,7 @@ void BLEServiceMap::handleGATTServerEvent(
  */
 BLEService* BLEServiceMap::getFirst() {
 	m_iterator = m_uuidMap.begin();
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
+	if (m_iterator == m_uuidMap.end()) return nullptr;
 	BLEService* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
@@ -112,9 +108,7 @@ BLEService* BLEServiceMap::getFirst() {
  * @return The next service in the map.
  */
 BLEService* BLEServiceMap::getNext() {
-	if (m_iterator == m_uuidMap.end()) {
-		return nullptr;
-	}
+	if (m_iterator == m_uuidMap.end()) return nullptr;
 	BLEService* pRet = m_iterator->first;
 	m_iterator++;
 	return pRet;
@@ -124,9 +118,17 @@ BLEService* BLEServiceMap::getNext() {
  * @brief Removes service from maps.
  * @return N/A.
  */
-void BLEServiceMap::removeService(BLEService *service){
+void BLEServiceMap::removeService(BLEService* service) {
 	m_handleMap.erase(service->getHandle());
 	m_uuidMap.erase(service);
 } // removeService
 
+/**
+ * @brief Returns the amount of registered services
+ * @return amount of registered services
+ */
+int BLEServiceMap::getRegisteredServiceCount(){
+	return m_handleMap.size();
+}
+
 #endif /* CONFIG_BT_ENABLED */
diff --git a/sensor/patchedBLE/src/BLEUUID.cpp b/sensor/patchedBLE/src/BLEUUID.cpp
index 9ca7cdd..4ddf8fc 100644
--- a/sensor/patchedBLE/src/BLEUUID.cpp
+++ b/sensor/patchedBLE/src/BLEUUID.cpp
@@ -6,7 +6,6 @@
  */
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-#include <esp_log.h>
 #include <string.h>
 #include <sstream>
 #include <iomanip>
@@ -14,12 +13,16 @@
 #include <assert.h>
 #include <stdlib.h>
 #include "BLEUUID.h"
-static const char* LOG_TAG = "BLEUUID";
 
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEUUID";
 #endif
 
+
 /**
  * @brief Copy memory from source to target but in reverse order.
  *
@@ -41,7 +44,7 @@ static const char* LOG_TAG = "BLEUUID";
  */
 static void memrcpy(uint8_t* target, uint8_t* source, uint32_t size) {
 	assert(size > 0);
-	target+=(size-1); // Point target to the last byte of the target data
+	target += (size - 1); // Point target to the last byte of the target data
 	while (size > 0) {
 		*target = *source;
 		target--;
@@ -70,45 +73,51 @@ static void memrcpy(uint8_t* target, uint8_t* source, uint32_t size) {
  */
 BLEUUID::BLEUUID(std::string value) {
 	m_valueSet = true;
-	if (value.length() == 2) {
+	if (value.length() == 4) {
 		m_uuid.len         = ESP_UUID_LEN_16;
-		m_uuid.uuid.uuid16 = value[0] | (value[1] << 8);
+		m_uuid.uuid.uuid16 = 0;
+		for(int i=0;i<value.length();){
+			uint8_t MSB = value.c_str()[i];
+			uint8_t LSB = value.c_str()[i+1];
+			
+			if(MSB > '9') MSB -= 7;
+			if(LSB > '9') LSB -= 7;
+			m_uuid.uuid.uuid16 += (((MSB&0x0F) <<4) | (LSB & 0x0F))<<(2-i)*4;
+			i+=2;	
+		}
 	}
-	else if (value.length() == 4) {
+	else if (value.length() == 8) {
 		m_uuid.len         = ESP_UUID_LEN_32;
-		m_uuid.uuid.uuid32 = value[0] | (value[1] << 8) | (value[2] << 16) | (value[3] << 24);
+		m_uuid.uuid.uuid32 = 0;
+		for(int i=0;i<value.length();){
+			uint8_t MSB = value.c_str()[i];
+			uint8_t LSB = value.c_str()[i+1];
+			
+			if(MSB > '9') MSB -= 7; 
+			if(LSB > '9') LSB -= 7;
+			m_uuid.uuid.uuid32 += (((MSB&0x0F) <<4) | (LSB & 0x0F))<<(6-i)*4;
+			i+=2;
+		}		
 	}
-	else if (value.length() == 16) {
+	else if (value.length() == 16) {  // how we can have 16 byte length string reprezenting 128 bit uuid??? needs to be investigated (lack of time)
 		m_uuid.len = ESP_UUID_LEN_128;
 		memrcpy(m_uuid.uuid.uuid128, (uint8_t*)value.data(), 16);
 	}
 	else if (value.length() == 36) {
-// If the length of the string is 36 bytes then we will assume it is a long hex string in
-// UUID format.
+		// If the length of the string is 36 bytes then we will assume it is a long hex string in
+		// UUID format.
 		m_uuid.len = ESP_UUID_LEN_128;
-		int vals[16];
-		sscanf(value.c_str(), "%2x%2x%2x%2x-%2x%2x-%2x%2x-%2x%2x-%2x%2x%2x%2x%2x%2x",
-			&vals[15],
-			&vals[14],
-			&vals[13],
-			&vals[12],
-			&vals[11],
-			&vals[10],
-			&vals[9],
-			&vals[8],
-			&vals[7],
-			&vals[6],
-			&vals[5],
-			&vals[4],
-			&vals[3],
-			&vals[2],
-			&vals[1],
-			&vals[0]
-		);
-
-		int i;
-		for (i=0; i<16; i++) {
-			m_uuid.uuid.uuid128[i] = vals[i];
+		int n = 0;
+		for(int i=0;i<value.length();){
+			if(value.c_str()[i] == '-')
+				i++;
+			uint8_t MSB = value.c_str()[i];
+			uint8_t LSB = value.c_str()[i+1];
+			
+			if(MSB > '9') MSB -= 7; 
+			if(LSB > '9') LSB -= 7;
+			m_uuid.uuid.uuid128[15-n++] = ((MSB&0x0F) <<4) | (LSB & 0x0F);
+			i+=2;	
 		}
 	}
 	else {
@@ -136,7 +145,7 @@ BLEUUID::BLEUUID(uint8_t* pData, size_t size, bool msbFirst) {
 	} else {
 		memcpy(m_uuid.uuid.uuid128, pData, 16);
 	}
-	m_valueSet         = true;
+	m_valueSet = true;
 } // BLEUUID
 
 
@@ -149,7 +158,6 @@ BLEUUID::BLEUUID(uint16_t uuid) {
 	m_uuid.len         = ESP_UUID_LEN_16;
 	m_uuid.uuid.uuid16 = uuid;
 	m_valueSet         = true;
-
 } // BLEUUID
 
 
@@ -194,24 +202,18 @@ BLEUUID::BLEUUID() {
  * @brief Get the number of bits in this uuid.
  * @return The number of bits in the UUID.  One of 16, 32 or 128.
  */
-int BLEUUID::bitSize() {
-	if (m_valueSet == false) {
-		return 0;
-	}
-	switch(m_uuid.len) {
-		case ESP_UUID_LEN_16: {
+uint8_t BLEUUID::bitSize() {
+	if (!m_valueSet) return 0;
+	switch (m_uuid.len) {
+		case ESP_UUID_LEN_16:
 			return 16;
-		}
-		case ESP_UUID_LEN_32: {
+		case ESP_UUID_LEN_32:
 			return 32;
-		}
-		case ESP_UUID_LEN_128: {
+		case ESP_UUID_LEN_128:
 			return 128;
-		}
-		default: {
+		default:
 			ESP_LOGE(LOG_TAG, "Unknown UUID length: %d", m_uuid.len);
 			return 0;
-		}
 	} // End of switch
 } // bitSize
 
@@ -224,9 +226,7 @@ int BLEUUID::bitSize() {
  */
 bool BLEUUID::equals(BLEUUID uuid) {
 	//ESP_LOGD(TAG, "Comparing: %s to %s", toString().c_str(), uuid.toString().c_str());
-	if (m_valueSet == false || uuid.m_valueSet == false) {
-		return false;
-	}
+	if (!m_valueSet || !uuid.m_valueSet) return false;
 
 	if (uuid.m_uuid.len != m_uuid.len) {
 		return uuid.toString() == toString();
@@ -253,14 +253,14 @@ bool BLEUUID::equals(BLEUUID uuid) {
  * NNNNNNNN
  * <UUID>
  */
-BLEUUID BLEUUID::fromString(std::string _uuid){
+BLEUUID BLEUUID::fromString(std::string _uuid) {
 	uint8_t start = 0;
 	if (strstr(_uuid.c_str(), "0x") != nullptr) { // If the string starts with 0x, skip those characters.
 		start = 2;
 	}
 	uint8_t len = _uuid.length() - start; // Calculate the length of the string we are going to use.
 
-	if( len == 4) {
+	if(len == 4) {
 		uint16_t x = strtoul(_uuid.substr(start, len).c_str(), NULL, 16);
 		return BLEUUID(x);
 	} else if (len == 8) {
@@ -299,7 +299,7 @@ BLEUUID BLEUUID::to128() {
 	//ESP_LOGD(LOG_TAG, ">> toFull() - %s", toString().c_str());
 
 	// If we either don't have a value or are already a 128 bit UUID, nothing further to do.
-	if (m_valueSet == false || m_uuid.len == ESP_UUID_LEN_128) {
+	if (!m_valueSet || m_uuid.len == ESP_UUID_LEN_128) {
 		return *this;
 	}
 
@@ -356,9 +356,7 @@ BLEUUID BLEUUID::to128() {
  * @return A string representation of the UUID.
  */
 std::string BLEUUID::toString() {
-	if (m_valueSet == false) {   // If we have no value, nothing to format.
-		return "<NULL>";
-	}
+	if (!m_valueSet) return "<NULL>";   // If we have no value, nothing to format.
 
 	// If the UUIDs are 16 or 32 bit, pad correctly.
 	std::stringstream ss;
@@ -386,24 +384,23 @@ std::string BLEUUID::toString() {
 	//
 	// UUID string format:
 	// AABBCCDD-EEFF-GGHH-IIJJ-KKLLMMNNOOPP
-	//
 	ss << std::hex << std::setfill('0') <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[15] <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[14] <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[13] <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[12] << "-" <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[11] <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[10] << "-" <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[9]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[8]  << "-" <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[7]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[6]  << "-" <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[5]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[4]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[3]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[2]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[1]  <<
-		std::setw(2) << (int)m_uuid.uuid.uuid128[0];
+		std::setw(2) << (int) m_uuid.uuid.uuid128[15] <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[14] <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[13] <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[12] << "-" <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[11] <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[10] << "-" <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[9]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[8]  << "-" <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[7]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[6]  << "-" <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[5]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[4]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[3]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[2]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[1]  <<
+		std::setw(2) << (int) m_uuid.uuid.uuid128[0];
 	return ss.str();
 } // toString
 
diff --git a/sensor/patchedBLE/src/BLEUUID.h b/sensor/patchedBLE/src/BLEUUID.h
index 5fb7795..700739b 100644
--- a/sensor/patchedBLE/src/BLEUUID.h
+++ b/sensor/patchedBLE/src/BLEUUID.h
@@ -24,7 +24,7 @@ public:
 	BLEUUID(uint8_t* pData, size_t size, bool msbFirst);
 	BLEUUID(esp_gatt_id_t gattId);
 	BLEUUID();
-	int            bitSize();   // Get the number of bits in this uuid.
+	uint8_t        bitSize();   // Get the number of bits in this uuid.
 	bool           equals(BLEUUID uuid);
 	esp_bt_uuid_t* getNative();
 	BLEUUID        to128();
@@ -32,8 +32,8 @@ public:
 	static BLEUUID fromString(std::string uuid);  // Create a BLEUUID from a string
 
 private:
-	esp_bt_uuid_t m_uuid;       // The underlying UUID structure that this class wraps.
-	bool          m_valueSet;   // Is there a value set for this instance.
+	esp_bt_uuid_t m_uuid;       		// The underlying UUID structure that this class wraps.
+	bool          m_valueSet = false;   // Is there a value set for this instance.
 }; // BLEUUID
 #endif /* CONFIG_BT_ENABLED */
 #endif /* COMPONENTS_CPP_UTILS_BLEUUID_H_ */
diff --git a/sensor/patchedBLE/src/BLEUtils.cpp b/sensor/patchedBLE/src/BLEUtils.cpp
index a33ee27..5cd55f9 100644
--- a/sensor/patchedBLE/src/BLEUtils.cpp
+++ b/sensor/patchedBLE/src/BLEUtils.cpp
@@ -19,20 +19,22 @@
 #include <esp_gap_ble_api.h> // ESP32 BLE
 #include <esp_gattc_api.h>   // ESP32 BLE
 #include <esp_err.h>         // ESP32 ESP-IDF
-#include <esp_log.h>         // ESP32 ESP-IDF
 #include <map>               // Part of C++ STL
 #include <sstream>
 #include <iomanip>
 
-#ifdef ARDUINO_ARCH_ESP32
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG = "BLEUtils";  // Tag for logging.
 #endif
 
-static const char* LOG_TAG = "BLEUtils";  // Tag for logging.
 
 /*
-static std::map<std::string, BLEClient *> g_addressMap;
-static std::map<uint16_t, BLEClient *> g_connIdMap;
+static std::map<std::string, BLEClient*> g_addressMap;
+static std::map<uint16_t, BLEClient*> g_connIdMap;
 */
 
 typedef struct {
@@ -41,6 +43,7 @@ typedef struct {
 } member_t;
 
 static const member_t members_ids[] = {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 	{0xFE08, "Microsoft"},
 	{0xFE09, "Pillsy, Inc."},
 	{0xFE0A, "ruwido austria gmbh"},
@@ -290,6 +293,7 @@ static const member_t members_ids[] = {
 	{0xFEFE, "GN ReSound A/S"},
 	{0xFEFF, "GN Netcom"},
 	{0xFFFF, "Reserved"}, /*for testing purposes only*/
+#endif
 	{0, "" }
 };
 
@@ -299,6 +303,7 @@ typedef struct {
 } gattdescriptor_t;
 
 static const gattdescriptor_t g_descriptor_ids[] = {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		{0x2905,"Characteristic Aggregate Format"},
 		{0x2900,"Characteristic Extended Properties"},
 		{0x2904,"Characteristic Presentation Format"},
@@ -314,6 +319,7 @@ static const gattdescriptor_t g_descriptor_ids[] = {
 		{0x290E,"Time Trigger Setting"},
 		{0x2906,"Valid Range"},
 		{0x290A,"Value Trigger Setting"},
+#endif
 		{ 0, "" }
 };
 
@@ -323,6 +329,7 @@ typedef struct {
 } characteristicMap_t;
 
 static const characteristicMap_t g_characteristicsMappings[] = {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		{0x2A7E,"Aerobic Heart Rate Lower Limit"},
 		{0x2A84,"Aerobic Heart Rate Upper Limit"},
 		{0x2A7F,"Aerobic Threshold"},
@@ -539,6 +546,7 @@ static const characteristicMap_t g_characteristicsMappings[] = {
 		{0x2A9D,"Weight Measurement"},
 		{0x2A9E,"Weight Scale Feature"},
 		{0x2A79,"Wind Chill"},
+#endif
 		{0, ""}
 };
 
@@ -556,6 +564,7 @@ typedef struct {
  * Definition of the service ids to names that we know about.
  */
 static const gattService_t g_gattServices[] = {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 	{"Alert Notification Service", "org.bluetooth.service.alert_notification", 0x1811},
 	{"Automation IO", "org.bluetooth.service.automation_io",	0x1815 },
 	{"Battery Service","org.bluetooth.service.battery_service",	0x180F},
@@ -591,6 +600,7 @@ static const gattService_t g_gattServices[] = {
 	{"Tx Power", "org.bluetooth.service.tx_power", 0x1804},
 	{"User Data", "org.bluetooth.service.user_data", 0x181C},
 	{"Weight Scale", "org.bluetooth.service.weight_scale", 0x181D},
+#endif
 	{"", "", 0 }
 };
 
@@ -628,7 +638,8 @@ static std::string gattIdToString(esp_gatt_id_t gattId) {
  * @brief Convert an esp_ble_addr_type_t to a string representation.
  */
 const char* BLEUtils::addressTypeToString(esp_ble_addr_type_t type) {
-	switch(type) {
+	switch (type) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case BLE_ADDR_TYPE_PUBLIC:
 			return "BLE_ADDR_TYPE_PUBLIC";
 		case BLE_ADDR_TYPE_RANDOM:
@@ -637,6 +648,7 @@ const char* BLEUtils::addressTypeToString(esp_ble_addr_type_t type) {
 			return "BLE_ADDR_TYPE_RPA_PUBLIC";
 		case BLE_ADDR_TYPE_RPA_RANDOM:
 			return "BLE_ADDR_TYPE_RPA_RANDOM";
+#endif
 		default:
 			return " esp_ble_addr_type_t";
 	}
@@ -650,19 +662,19 @@ const char* BLEUtils::addressTypeToString(esp_ble_addr_type_t type) {
  */
 std::string BLEUtils::adFlagsToString(uint8_t adFlags) {
 	std::stringstream ss;
-	if (adFlags & (1<<0)) {
+	if (adFlags & (1 << 0)) {
 		ss << "[LE Limited Discoverable Mode] ";
 	}
-	if (adFlags & (1<<1)) {
+	if (adFlags & (1 << 1)) {
 		ss << "[LE General Discoverable Mode] ";
 	}
-	if (adFlags & (1<<2)) {
+	if (adFlags & (1 << 2)) {
 		ss << "[BR/EDR Not Supported] ";
 	}
-	if (adFlags & (1<<3)) {
+	if (adFlags & (1 << 3)) {
 		ss << "[Simultaneous LE and BR/EDR to Same Device Capable (Controller)] ";
 	}
-	if (adFlags & (1<<4)) {
+	if (adFlags & (1 << 4)) {
 		ss << "[Simultaneous LE and BR/EDR to Same Device Capable (Host)] ";
 	}
 	return ss.str();
@@ -678,84 +690,61 @@ std::string BLEUtils::adFlagsToString(uint8_t adFlags) {
  * @return A string representation of the type.
  */
 const char* BLEUtils::advTypeToString(uint8_t advType) {
-	switch(advType) {
-		case ESP_BLE_AD_TYPE_FLAG:                   // 0x01
+	switch (advType) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
+		case ESP_BLE_AD_TYPE_FLAG:				   // 0x01
 			return "ESP_BLE_AD_TYPE_FLAG";
-
-		case ESP_BLE_AD_TYPE_16SRV_PART:             // 0x02
+		case ESP_BLE_AD_TYPE_16SRV_PART:			 // 0x02
 			return "ESP_BLE_AD_TYPE_16SRV_PART";
-
-		case ESP_BLE_AD_TYPE_16SRV_CMPL:             // 0x03
+		case ESP_BLE_AD_TYPE_16SRV_CMPL:			 // 0x03
 			return "ESP_BLE_AD_TYPE_16SRV_CMPL";
-
-		case ESP_BLE_AD_TYPE_32SRV_PART:             // 0x04
+		case ESP_BLE_AD_TYPE_32SRV_PART:			 // 0x04
 			return "ESP_BLE_AD_TYPE_32SRV_PART";
-
-		case ESP_BLE_AD_TYPE_32SRV_CMPL:             // 0x05
+		case ESP_BLE_AD_TYPE_32SRV_CMPL:			 // 0x05
 			return "ESP_BLE_AD_TYPE_32SRV_CMPL";
-
-		case ESP_BLE_AD_TYPE_128SRV_PART:            // 0x06
+		case ESP_BLE_AD_TYPE_128SRV_PART:			// 0x06
 			return "ESP_BLE_AD_TYPE_128SRV_PART";
-
-		case ESP_BLE_AD_TYPE_128SRV_CMPL:            // 0x07
+		case ESP_BLE_AD_TYPE_128SRV_CMPL:			// 0x07
 			return "ESP_BLE_AD_TYPE_128SRV_CMPL";
-
-		case ESP_BLE_AD_TYPE_NAME_SHORT:             // 0x08
+		case ESP_BLE_AD_TYPE_NAME_SHORT:			 // 0x08
 			return "ESP_BLE_AD_TYPE_NAME_SHORT";
-
-		case ESP_BLE_AD_TYPE_NAME_CMPL:              // 0x09
+		case ESP_BLE_AD_TYPE_NAME_CMPL:			  // 0x09
 			return "ESP_BLE_AD_TYPE_NAME_CMPL";
-
-		case ESP_BLE_AD_TYPE_TX_PWR:                 // 0x0a
+		case ESP_BLE_AD_TYPE_TX_PWR:				 // 0x0a
 			return "ESP_BLE_AD_TYPE_TX_PWR";
-
-		case ESP_BLE_AD_TYPE_DEV_CLASS:              // 0x0b
+		case ESP_BLE_AD_TYPE_DEV_CLASS:			  // 0x0b
 			return "ESP_BLE_AD_TYPE_DEV_CLASS";
-
-		case ESP_BLE_AD_TYPE_SM_TK:                  // 0x10
+		case ESP_BLE_AD_TYPE_SM_TK:				  // 0x10
 			return "ESP_BLE_AD_TYPE_SM_TK";
-
-		case ESP_BLE_AD_TYPE_SM_OOB_FLAG:            // 0x11
+		case ESP_BLE_AD_TYPE_SM_OOB_FLAG:			// 0x11
 			return "ESP_BLE_AD_TYPE_SM_OOB_FLAG";
-
-		case ESP_BLE_AD_TYPE_INT_RANGE:              // 0x12
+		case ESP_BLE_AD_TYPE_INT_RANGE:			  // 0x12
 			return "ESP_BLE_AD_TYPE_INT_RANGE";
-
-		case ESP_BLE_AD_TYPE_SOL_SRV_UUID:           // 0x14
+		case ESP_BLE_AD_TYPE_SOL_SRV_UUID:		   // 0x14
 			return "ESP_BLE_AD_TYPE_SOL_SRV_UUID";
-
-		case ESP_BLE_AD_TYPE_128SOL_SRV_UUID:        // 0x15
+		case ESP_BLE_AD_TYPE_128SOL_SRV_UUID:		// 0x15
 			return "ESP_BLE_AD_TYPE_128SOL_SRV_UUID";
-
-		case ESP_BLE_AD_TYPE_SERVICE_DATA:           // 0x16
+		case ESP_BLE_AD_TYPE_SERVICE_DATA:		   // 0x16
 			return "ESP_BLE_AD_TYPE_SERVICE_DATA";
-
-		case ESP_BLE_AD_TYPE_PUBLIC_TARGET:          // 0x17
+		case ESP_BLE_AD_TYPE_PUBLIC_TARGET:		  // 0x17
 			return "ESP_BLE_AD_TYPE_PUBLIC_TARGET";
-
-		case ESP_BLE_AD_TYPE_RANDOM_TARGET:          // 0x18
+		case ESP_BLE_AD_TYPE_RANDOM_TARGET:		  // 0x18
 			return "ESP_BLE_AD_TYPE_RANDOM_TARGET";
-
-		case ESP_BLE_AD_TYPE_APPEARANCE:             // 0x19
+		case ESP_BLE_AD_TYPE_APPEARANCE:			 // 0x19
 			return "ESP_BLE_AD_TYPE_APPEARANCE";
-
-		case ESP_BLE_AD_TYPE_ADV_INT:                // 0x1a
+		case ESP_BLE_AD_TYPE_ADV_INT:				// 0x1a
 			return "ESP_BLE_AD_TYPE_ADV_INT";
-
 		case ESP_BLE_AD_TYPE_32SOL_SRV_UUID:
 			return "ESP_BLE_AD_TYPE_32SOL_SRV_UUID";
-
-		case ESP_BLE_AD_TYPE_32SERVICE_DATA:         // 0x20
+		case ESP_BLE_AD_TYPE_32SERVICE_DATA:		 // 0x20
 			return "ESP_BLE_AD_TYPE_32SERVICE_DATA";
-
-		case ESP_BLE_AD_TYPE_128SERVICE_DATA:        // 0x21
+		case ESP_BLE_AD_TYPE_128SERVICE_DATA:		// 0x21
 			return "ESP_BLE_AD_TYPE_128SERVICE_DATA";
-
 		case ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE:  // 0xff
 			return "ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE";
-
+#endif
 		default:
-			ESP_LOGD(LOG_TAG, " adv data type: 0x%x", advType);
+			ESP_LOGV(LOG_TAG, " adv data type: 0x%x", advType);
 			return "";
 	} // End switch
 } // advTypeToString
@@ -768,8 +757,7 @@ esp_gatt_id_t BLEUtils::buildGattId(esp_bt_uuid_t uuid, uint8_t inst_id) {
 	return retGattId;
 }
 
-esp_gatt_srvc_id_t BLEUtils::buildGattSrvcId(esp_gatt_id_t gattId,
-		bool is_primary) {
+esp_gatt_srvc_id_t BLEUtils::buildGattSrvcId(esp_gatt_id_t gattId, bool is_primary) {
 	esp_gatt_srvc_id_t retSrvcId;
 	retSrvcId.id = gattId;
 	retSrvcId.is_primary = is_primary;
@@ -784,29 +772,26 @@ esp_gatt_srvc_id_t BLEUtils::buildGattSrvcId(esp_gatt_id_t gattId,
  * @param [in] length The length of the data to convert.
  * @return A pointer to the formatted buffer.
  */
-char* BLEUtils::buildHexData(uint8_t *target, uint8_t *source, uint8_t length) {
-// Guard against too much data.
-	if (length > 100) {
-		length = 100;
-	}
+char* BLEUtils::buildHexData(uint8_t* target, uint8_t* source, uint8_t length) {
+	// Guard against too much data.
+	if (length > 100) length = 100;
 
 	if (target == nullptr) {
-		target = (uint8_t *)malloc(length * 2 + 1);
+		target = (uint8_t*) malloc(length * 2 + 1);
 		if (target == nullptr) {
 			ESP_LOGE(LOG_TAG, "buildHexData: malloc failed");
 			return nullptr;
 		}
 	}
-	char *startOfData = (char *)target;
+	char* startOfData = (char*) target;
 
-	int i;
-	for (i=0; i<length; i++) {
-		sprintf((char *)target, "%.2x", (char)*source);
+	for (int i = 0; i < length; i++) {
+		sprintf((char*) target, "%.2x", (char) *source);
 		source++;
-		target +=2;
+		target += 2;
 	}
 
-// Handle the special case where there was no data.
+	// Handle the special case where there was no data.
 	if (length == 0) {
 		*startOfData = 0;
 	}
@@ -825,13 +810,9 @@ char* BLEUtils::buildHexData(uint8_t *target, uint8_t *source, uint8_t length) {
  */
 std::string BLEUtils::buildPrintData(uint8_t* source, size_t length) {
 	std::ostringstream ss;
-	for (int i=0; i<length; i++) {
+	for (int i = 0; i < length; i++) {
 		char c = *source;
-		if (isprint(c)) {
-			ss << c;
-		} else {
-			ss << '.';
-		}
+		ss << (isprint(c) ? c : '.');
 		source++;
 	}
 	return ss.str();
@@ -844,43 +825,36 @@ std::string BLEUtils::buildPrintData(uint8_t* source, size_t length) {
  * @return A string representation of the reason.
  */
 std::string BLEUtils::gattCloseReasonToString(esp_gatt_conn_reason_t reason) {
-	switch(reason) {
+	switch (reason) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_GATT_CONN_UNKNOWN: {
 			return "ESP_GATT_CONN_UNKNOWN";
 		}
-
 		case ESP_GATT_CONN_L2C_FAILURE: {
 			return "ESP_GATT_CONN_L2C_FAILURE";
 		}
-
 		case ESP_GATT_CONN_TIMEOUT: {
 			return "ESP_GATT_CONN_TIMEOUT";
 		}
-
 		case ESP_GATT_CONN_TERMINATE_PEER_USER: {
 			return "ESP_GATT_CONN_TERMINATE_PEER_USER";
 		}
-
 		case ESP_GATT_CONN_TERMINATE_LOCAL_HOST: {
 			return "ESP_GATT_CONN_TERMINATE_LOCAL_HOST";
 		}
-
 		case ESP_GATT_CONN_FAIL_ESTABLISH: {
 			return "ESP_GATT_CONN_FAIL_ESTABLISH";
 		}
-
 		case ESP_GATT_CONN_LMP_TIMEOUT: {
 			return "ESP_GATT_CONN_LMP_TIMEOUT";
 		}
-
 		case ESP_GATT_CONN_CONN_CANCEL: {
 			return "ESP_GATT_CONN_CONN_CANCEL";
 		}
-
 		case ESP_GATT_CONN_NONE: {
 			return "ESP_GATT_CONN_NONE";
 		}
-
+#endif
 		default: {
 			return "Unknown";
 		}
@@ -889,7 +863,8 @@ std::string BLEUtils::gattCloseReasonToString(esp_gatt_conn_reason_t reason) {
 
 
 std::string BLEUtils::gattClientEventTypeToString(esp_gattc_cb_event_t eventType) {
-	switch(eventType) {
+	switch (eventType) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_GATTC_ACL_EVT:
 			return "ESP_GATTC_ACL_EVT";
 		case ESP_GATTC_ADV_DATA_EVT:
@@ -972,8 +947,9 @@ std::string BLEUtils::gattClientEventTypeToString(esp_gattc_cb_event_t eventType
 			return "ESP_GATTC_WRITE_CHAR_EVT";
 		case ESP_GATTC_WRITE_DESCR_EVT:
 			return "ESP_GATTC_WRITE_DESCR_EVT";
+#endif
 		default:
-			ESP_LOGW(LOG_TAG, "Unknown GATT Client event type: %d", eventType);
+			ESP_LOGV(LOG_TAG, "Unknown GATT Client event type: %d", eventType);
 			return "Unknown";
 	}
 } // gattClientEventTypeToString
@@ -985,81 +961,62 @@ std::string BLEUtils::gattClientEventTypeToString(esp_gattc_cb_event_t eventType
  * @return A string representation of the GATT server event code.
  */
 std::string BLEUtils::gattServerEventTypeToString(esp_gatts_cb_event_t eventType) {
-	switch(eventType) {
-	case ESP_GATTS_REG_EVT:
-		return "ESP_GATTS_REG_EVT";
-
-	case ESP_GATTS_READ_EVT:
-		return "ESP_GATTS_READ_EVT";
-
-	case ESP_GATTS_WRITE_EVT:
-		return "ESP_GATTS_WRITE_EVT";
-
-	case ESP_GATTS_EXEC_WRITE_EVT:
-		return "ESP_GATTS_EXEC_WRITE_EVT";
-
-	case ESP_GATTS_MTU_EVT:
-		return "ESP_GATTS_MTU_EVT";
-
-	case ESP_GATTS_CONF_EVT:
-		return "ESP_GATTS_CONF_EVT";
-
-	case ESP_GATTS_UNREG_EVT:
-		return "ESP_GATTS_UNREG_EVT";
-
-	case ESP_GATTS_CREATE_EVT:
-		return "ESP_GATTS_CREATE_EVT";
-
-	case ESP_GATTS_ADD_INCL_SRVC_EVT:
-		return "ESP_GATTS_ADD_INCL_SRVC_EVT";
-
-	case ESP_GATTS_ADD_CHAR_EVT:
-		return "ESP_GATTS_ADD_CHAR_EVT";
-
-	case ESP_GATTS_ADD_CHAR_DESCR_EVT:
-		return "ESP_GATTS_ADD_CHAR_DESCR_EVT";
-
-	case ESP_GATTS_DELETE_EVT:
-		return "ESP_GATTS_DELETE_EVT";
-
-	case ESP_GATTS_START_EVT:
-		return "ESP_GATTS_START_EVT";
-
-	case ESP_GATTS_STOP_EVT:
-		return "ESP_GATTS_STOP_EVT";
-
-	case ESP_GATTS_CONNECT_EVT:
-		return "ESP_GATTS_CONNECT_EVT";
-
-	case ESP_GATTS_DISCONNECT_EVT:
-		return "ESP_GATTS_DISCONNECT_EVT";
-
-	case ESP_GATTS_OPEN_EVT:
-		return "ESP_GATTS_OPEN_EVT";
-
-	case ESP_GATTS_CANCEL_OPEN_EVT:
-		return "ESP_GATTS_CANCEL_OPEN_EVT";
-
-	case ESP_GATTS_CLOSE_EVT:
-		return "ESP_GATTS_CLOSE_EVT";
-
-	case ESP_GATTS_LISTEN_EVT:
-		return "ESP_GATTS_LISTEN_EVT";
-
-	case ESP_GATTS_CONGEST_EVT:
-		return "ESP_GATTS_CONGEST_EVT";
-
-	case ESP_GATTS_RESPONSE_EVT:
-		return "ESP_GATTS_RESPONSE_EVT";
-
-	case ESP_GATTS_CREAT_ATTR_TAB_EVT:
-		return "ESP_GATTS_CREAT_ATTR_TAB_EVT";
-
-	case ESP_GATTS_SET_ATTR_VAL_EVT:
-		return "ESP_GATTS_SET_ATTR_VAL_EVT";
-
+	switch (eventType) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
+		case ESP_GATTS_REG_EVT:
+			return "ESP_GATTS_REG_EVT";
+		case ESP_GATTS_READ_EVT:
+			return "ESP_GATTS_READ_EVT";
+		case ESP_GATTS_WRITE_EVT:
+			return "ESP_GATTS_WRITE_EVT";
+		case ESP_GATTS_EXEC_WRITE_EVT:
+			return "ESP_GATTS_EXEC_WRITE_EVT";
+		case ESP_GATTS_MTU_EVT:
+			return "ESP_GATTS_MTU_EVT";
+		case ESP_GATTS_CONF_EVT:
+			return "ESP_GATTS_CONF_EVT";
+		case ESP_GATTS_UNREG_EVT:
+			return "ESP_GATTS_UNREG_EVT";
+		case ESP_GATTS_CREATE_EVT:
+			return "ESP_GATTS_CREATE_EVT";
+		case ESP_GATTS_ADD_INCL_SRVC_EVT:
+			return "ESP_GATTS_ADD_INCL_SRVC_EVT";
+		case ESP_GATTS_ADD_CHAR_EVT:
+			return "ESP_GATTS_ADD_CHAR_EVT";
+		case ESP_GATTS_ADD_CHAR_DESCR_EVT:
+			return "ESP_GATTS_ADD_CHAR_DESCR_EVT";
+		case ESP_GATTS_DELETE_EVT:
+			return "ESP_GATTS_DELETE_EVT";
+		case ESP_GATTS_START_EVT:
+			return "ESP_GATTS_START_EVT";
+		case ESP_GATTS_STOP_EVT:
+			return "ESP_GATTS_STOP_EVT";
+		case ESP_GATTS_CONNECT_EVT:
+			return "ESP_GATTS_CONNECT_EVT";
+		case ESP_GATTS_DISCONNECT_EVT:
+			return "ESP_GATTS_DISCONNECT_EVT";
+		case ESP_GATTS_OPEN_EVT:
+			return "ESP_GATTS_OPEN_EVT";
+		case ESP_GATTS_CANCEL_OPEN_EVT:
+			return "ESP_GATTS_CANCEL_OPEN_EVT";
+		case ESP_GATTS_CLOSE_EVT:
+			return "ESP_GATTS_CLOSE_EVT";
+		case ESP_GATTS_LISTEN_EVT:
+			return "ESP_GATTS_LISTEN_EVT";
+		case ESP_GATTS_CONGEST_EVT:
+			return "ESP_GATTS_CONGEST_EVT";
+		case ESP_GATTS_RESPONSE_EVT:
+			return "ESP_GATTS_RESPONSE_EVT";
+		case ESP_GATTS_CREAT_ATTR_TAB_EVT:
+			return "ESP_GATTS_CREAT_ATTR_TAB_EVT";
+		case ESP_GATTS_SET_ATTR_VAL_EVT:
+			return "ESP_GATTS_SET_ATTR_VAL_EVT";
+		case ESP_GATTS_SEND_SERVICE_CHANGE_EVT:
+			return "ESP_GATTS_SEND_SERVICE_CHANGE_EVT";
+#endif
+		default:
+			return "Unknown";
 	}
-	return "Unknown";
 } // gattServerEventTypeToString
 
 
@@ -1069,15 +1026,17 @@ std::string BLEUtils::gattServerEventTypeToString(esp_gatts_cb_event_t eventType
  * @param [in] type The device type.
  */
 const char* BLEUtils::devTypeToString(esp_bt_dev_type_t type) {
-	switch(type) {
-	case ESP_BT_DEVICE_TYPE_BREDR:
-		return "ESP_BT_DEVICE_TYPE_BREDR";
-	case ESP_BT_DEVICE_TYPE_BLE:
-		return "ESP_BT_DEVICE_TYPE_BLE";
-	case ESP_BT_DEVICE_TYPE_DUMO:
-		return "ESP_BT_DEVICE_TYPE_DUMO";
-	default:
-		return "Unknown";
+	switch (type) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
+		case ESP_BT_DEVICE_TYPE_BREDR:
+			return "ESP_BT_DEVICE_TYPE_BREDR";
+		case ESP_BT_DEVICE_TYPE_BLE:
+			return "ESP_BT_DEVICE_TYPE_BLE";
+		case ESP_BT_DEVICE_TYPE_DUMO:
+			return "ESP_BT_DEVICE_TYPE_DUMO";
+#endif
+		default:
+			return "Unknown";
 	}
 } // devTypeToString
 
@@ -1088,56 +1047,44 @@ const char* BLEUtils::devTypeToString(esp_bt_dev_type_t type) {
 void BLEUtils::dumpGapEvent(
 	esp_gap_ble_cb_event_t  event,
 	esp_ble_gap_cb_param_t* param) {
-	ESP_LOGD(LOG_TAG, "Received a GAP event: %s", gapEventToString(event));
-	switch(event) {
-		//
+	ESP_LOGV(LOG_TAG, "Received a GAP event: %s", gapEventToString(event));
+	switch (event) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		// ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT
 		// adv_data_cmpl
 		// - esp_bt_status_t
-		//
 		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->adv_data_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->adv_data_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT
 		//
 		// adv_data_raw_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->adv_data_raw_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->adv_data_raw_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_ADV_START_COMPLETE_EVT
 		//
 		// adv_start_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->adv_start_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->adv_start_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_ADV_START_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT
 		//
 		// adv_stop_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->adv_stop_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->adv_stop_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_AUTH_CMPL_EVT
 		//
 		// auth_cmpl
@@ -1148,9 +1095,8 @@ void BLEUtils::dumpGapEvent(
 		// - uint8_t fail_reason
 		// - esp_bd_addr_type_t addr_type
 		// - esp_bt_dev_type_t dev_type
-		//
 		case ESP_GAP_BLE_AUTH_CMPL_EVT: {
-			ESP_LOGD(LOG_TAG, "[bd_addr: %s, key_present: %d, key: ***, key_type: %d, success: %d, fail_reason: %d, addr_type: ***, dev_type: %s]",
+			ESP_LOGV(LOG_TAG, "[bd_addr: %s, key_present: %d, key: ***, key_type: %d, success: %d, fail_reason: %d, addr_type: ***, dev_type: %s]",
 				BLEAddress(param->ble_security.auth_cmpl.bd_addr).toString().c_str(),
 				param->ble_security.auth_cmpl.key_present,
 				param->ble_security.auth_cmpl.key_type,
@@ -1161,56 +1107,41 @@ void BLEUtils::dumpGapEvent(
 			break;
 		} // ESP_GAP_BLE_AUTH_CMPL_EVT
 
-
-		//
 		// ESP_GAP_BLE_CLEAR_BOND_DEV_COMPLETE_EVT
 		//
 		// clear_bond_dev_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_CLEAR_BOND_DEV_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->clear_bond_dev_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->clear_bond_dev_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_CLEAR_BOND_DEV_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_LOCAL_IR_EVT
-		//
 		case ESP_GAP_BLE_LOCAL_IR_EVT: {
 			break;
 		} // ESP_GAP_BLE_LOCAL_IR_EVT
 
-
-		//
 		// ESP_GAP_BLE_LOCAL_ER_EVT
-		//
 		case ESP_GAP_BLE_LOCAL_ER_EVT: {
 			break;
 		} // ESP_GAP_BLE_LOCAL_ER_EVT
 
-
-		//
 		// ESP_GAP_BLE_NC_REQ_EVT
-		//
 		case ESP_GAP_BLE_NC_REQ_EVT: {
-			ESP_LOGD(LOG_TAG, "[bd_addr: %s, passkey: %d]",
+			ESP_LOGV(LOG_TAG, "[bd_addr: %s, passkey: %d]",
 				BLEAddress(param->ble_security.key_notif.bd_addr).toString().c_str(),
 				param->ble_security.key_notif.passkey);
 			break;
 		} // ESP_GAP_BLE_NC_REQ_EVT
 
-
-		//
 		// ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT
 		//
 		// read_rssi_cmpl
 		// - esp_bt_status_t status
 		// - int8_t rssi
 		// - esp_bd_addr_t remote_addr
-		//
 		case ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d, rssi: %d, remote_addr: %s]",
+			ESP_LOGV(LOG_TAG, "[status: %d, rssi: %d, remote_addr: %s]",
 					param->read_rssi_cmpl.status,
 					param->read_rssi_cmpl.rssi,
 					BLEAddress(param->read_rssi_cmpl.remote_addr).toString().c_str()
@@ -1218,20 +1149,15 @@ void BLEUtils::dumpGapEvent(
 			break;
 		} // ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT
 		//
 		// scan_param_cmpl.
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->scan_param_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->scan_param_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_RESULT_EVT
 		//
 		// scan_rst:
@@ -1246,11 +1172,10 @@ void BLEUtils::dumpGapEvent(
 		// - num_resps
 		// - adv_data_len
 		// - scan_rsp_len
-		//
 		case ESP_GAP_BLE_SCAN_RESULT_EVT: {
-			switch(param->scan_rst.search_evt) {
+			switch (param->scan_rst.search_evt) {
 				case ESP_GAP_SEARCH_INQ_RES_EVT: {
-					ESP_LOGD(LOG_TAG, "search_evt: %s, bda: %s, dev_type: %s, ble_addr_type: %s, ble_evt_type: %s, rssi: %d, ble_adv: ??, flag: %d (%s), num_resps: %d, adv_data_len: %d, scan_rsp_len: %d",
+					ESP_LOGV(LOG_TAG, "search_evt: %s, bda: %s, dev_type: %s, ble_addr_type: %s, ble_evt_type: %s, rssi: %d, ble_adv: ??, flag: %d (%s), num_resps: %d, adv_data_len: %d, scan_rsp_len: %d",
 						searchEventTypeToString(param->scan_rst.search_evt),
 						BLEAddress(param->scan_rst.bda).toString().c_str(),
 						devTypeToString(param->scan_rst.dev_type),
@@ -1267,59 +1192,46 @@ void BLEUtils::dumpGapEvent(
 				} // ESP_GAP_SEARCH_INQ_RES_EVT
 
 				default: {
-					ESP_LOGD(LOG_TAG, "search_evt: %s",searchEventTypeToString(param->scan_rst.search_evt));
+					ESP_LOGV(LOG_TAG, "search_evt: %s",searchEventTypeToString(param->scan_rst.search_evt));
 					break;
 				}
 			}
 			break;
 		} // ESP_GAP_BLE_SCAN_RESULT_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT
 		//
 		// scan_rsp_data_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->scan_rsp_data_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->scan_rsp_data_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT
-		//
 		case ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]",	param->scan_rsp_data_raw_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->scan_rsp_data_raw_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_START_COMPLETE_EVT
 		//
 		// scan_start_cmpl
 		// - esp_bt_status_t status
 		case ESP_GAP_BLE_SCAN_START_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]", param->scan_start_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->scan_start_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_SCAN_START_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT
 		//
 		// scan_stop_cmpl
 		// - esp_bt_status_t status
-		//
 		case ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d]", param->scan_stop_cmpl.status);
+			ESP_LOGV(LOG_TAG, "[status: %d]", param->scan_stop_cmpl.status);
 			break;
 		} // ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT
 
-
-		//
 		// ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT
 		//
 		// update_conn_params
@@ -1330,9 +1242,8 @@ void BLEUtils::dumpGapEvent(
 		// - uint16_t latency
 		// - uint16_t conn_int
 		// - uint16_t timeout
-		//
 		case ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %d, bd_addr: %s, min_int: %d, max_int: %d, latency: %d, conn_int: %d, timeout: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %d, bd_addr: %s, min_int: %d, max_int: %d, latency: %d, conn_int: %d, timeout: %d]",
 				param->update_conn_params.status,
 				BLEAddress(param->update_conn_params.bda).toString().c_str(),
 				param->update_conn_params.min_int,
@@ -1344,18 +1255,14 @@ void BLEUtils::dumpGapEvent(
 			break;
 		} // ESP_GAP_BLE_SCAN_UPDATE_CONN_PARAMS_EVT
 
-
-		//
 		// ESP_GAP_BLE_SEC_REQ_EVT
-		//
 		case ESP_GAP_BLE_SEC_REQ_EVT: {
-			ESP_LOGD(LOG_TAG, "[bd_addr: %s]", BLEAddress(param->ble_security.ble_req.bd_addr).toString().c_str());
+			ESP_LOGV(LOG_TAG, "[bd_addr: %s]", BLEAddress(param->ble_security.ble_req.bd_addr).toString().c_str());
 			break;
 		} // ESP_GAP_BLE_SEC_REQ_EVT
-
-
+#endif
 		default: {
-			ESP_LOGD(LOG_TAG, "*** dumpGapEvent: Logger not coded ***");
+			ESP_LOGV(LOG_TAG, "*** dumpGapEvent: Logger not coded ***");
 			break;
 		} // default
 	} // switch
@@ -1373,10 +1280,10 @@ void BLEUtils::dumpGattClientEvent(
 	esp_gatt_if_t             gattc_if,
 	esp_ble_gattc_cb_param_t* evtParam) {
 
-	//esp_ble_gattc_cb_param_t *evtParam = (esp_ble_gattc_cb_param_t *)param;
-	ESP_LOGD(LOG_TAG, "GATT Event: %s", BLEUtils::gattClientEventTypeToString(event).c_str());
-	switch(event) {
-		//
+	//esp_ble_gattc_cb_param_t* evtParam = (esp_ble_gattc_cb_param_t*) param;
+	ESP_LOGV(LOG_TAG, "GATT Event: %s", BLEUtils::gattClientEventTypeToString(event).c_str());
+	switch (event) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		// ESP_GATTC_CLOSE_EVT
 		//
 		// close:
@@ -1384,16 +1291,14 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t               conn_id
 		// - esp_bd_addr_t          remote_bda
 		// - esp_gatt_conn_reason_t reason
-		//
 		case ESP_GATTC_CLOSE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, reason:%s, conn_id: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, reason:%s, conn_id: %d]",
 				BLEUtils::gattStatusToString(evtParam->close.status).c_str(),
 				BLEUtils::gattCloseReasonToString(evtParam->close.reason).c_str(),
 				evtParam->close.conn_id);
 			break;
 		}
 
-		//
 		// ESP_GATTC_CONNECT_EVT
 		//
 		// connect:
@@ -1401,14 +1306,13 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t          conn_id
 		// - esp_bd_addr_t     remote_bda
 		case ESP_GATTC_CONNECT_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
 				evtParam->connect.conn_id,
 				BLEAddress(evtParam->connect.remote_bda).toString().c_str()
 			);
 			break;
 		}
 
-		//
 		// ESP_GATTC_DISCONNECT_EVT
 		//
 		// disconnect:
@@ -1416,7 +1320,7 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t               conn_id
 		// - esp_bd_addr_t          remote_bda
 		case ESP_GATTC_DISCONNECT_EVT: {
-			ESP_LOGD(LOG_TAG, "[reason: %s, conn_id: %d, remote_bda: %s]",
+			ESP_LOGV(LOG_TAG, "[reason: %s, conn_id: %d, remote_bda: %s]",
 				BLEUtils::gattCloseReasonToString(evtParam->disconnect.reason).c_str(),
 				evtParam->disconnect.conn_id,
 				BLEAddress(evtParam->disconnect.remote_bda).toString().c_str()
@@ -1424,7 +1328,6 @@ void BLEUtils::dumpGattClientEvent(
 			break;
 		} // ESP_GATTC_DISCONNECT_EVT
 
-		//
 		// ESP_GATTC_GET_CHAR_EVT
 		//
 		// get_char:
@@ -1433,7 +1336,6 @@ void BLEUtils::dumpGattClientEvent(
 		// - esp_gatt_srvc_id_t   srvc_id
 		// - esp_gatt_id_t        char_id
 		// - esp_gatt_char_prop_t char_prop
-		//
 		/*
 		case ESP_GATTC_GET_CHAR_EVT: {
 
@@ -1444,7 +1346,7 @@ void BLEUtils::dumpGattClientEvent(
 				if (evtParam->get_char.char_id.uuid.len == ESP_UUID_LEN_16) {
 					description = BLEUtils::gattCharacteristicUUIDToString(evtParam->get_char.char_id.uuid.uuid.uuid16);
 				}
-				ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, srvc_id: %s, char_id: %s [description: %s]\nchar_prop: %s]",
+				ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d, srvc_id: %s, char_id: %s [description: %s]\nchar_prop: %s]",
 					BLEUtils::gattStatusToString(evtParam->get_char.status).c_str(),
 					evtParam->get_char.conn_id,
 					BLEUtils::gattServiceIdToString(evtParam->get_char.srvc_id).c_str(),
@@ -1453,7 +1355,7 @@ void BLEUtils::dumpGattClientEvent(
 					BLEUtils::characteristicPropertiesToString(evtParam->get_char.char_prop).c_str()
 				);
 			} else {
-				ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, srvc_id: %s]",
+				ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d, srvc_id: %s]",
 					BLEUtils::gattStatusToString(evtParam->get_char.status).c_str(),
 					evtParam->get_char.conn_id,
 					BLEUtils::gattServiceIdToString(evtParam->get_char.srvc_id).c_str()
@@ -1463,7 +1365,6 @@ void BLEUtils::dumpGattClientEvent(
 		} // ESP_GATTC_GET_CHAR_EVT
 		*/
 
-		//
 		// ESP_GATTC_NOTIFY_EVT
 		//
 		// notify
@@ -1475,7 +1376,7 @@ void BLEUtils::dumpGattClientEvent(
 		// bool               is_notify
 		//
 		case ESP_GATTC_NOTIFY_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, remote_bda: %s, handle: %d 0x%.2x, value_len: %d, is_notify: %d]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, remote_bda: %s, handle: %d 0x%.2x, value_len: %d, is_notify: %d]",
 				evtParam->notify.conn_id,
 				BLEAddress(evtParam->notify.remote_bda).toString().c_str(),
 				evtParam->notify.handle,
@@ -1486,7 +1387,6 @@ void BLEUtils::dumpGattClientEvent(
 			break;
 		}
 
-		//
 		// ESP_GATTC_OPEN_EVT
 		//
 		// open:
@@ -1496,7 +1396,7 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t          mtu
 		//
 		case ESP_GATTC_OPEN_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, remote_bda: %s, mtu: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d, remote_bda: %s, mtu: %d]",
 				BLEUtils::gattStatusToString(evtParam->open.status).c_str(),
 				evtParam->open.conn_id,
 				BLEAddress(evtParam->open.remote_bda).toString().c_str(),
@@ -1504,8 +1404,6 @@ void BLEUtils::dumpGattClientEvent(
 			break;
 		} // ESP_GATTC_OPEN_EVT
 
-
-		//
 		// ESP_GATTC_READ_CHAR_EVT
 		//
 		// Callback to indicate that requested data that we wanted to read is now available.
@@ -1518,7 +1416,7 @@ void BLEUtils::dumpGattClientEvent(
 		// uint16_t           value_type
 		// uint16_t           value_len
 		case ESP_GATTC_READ_CHAR_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x, value_len: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x, value_len: %d]",
 				BLEUtils::gattStatusToString(evtParam->read.status).c_str(),
 				evtParam->read.conn_id,
 				evtParam->read.handle,
@@ -1528,38 +1426,33 @@ void BLEUtils::dumpGattClientEvent(
 			if (evtParam->read.status == ESP_GATT_OK) {
 				GeneralUtils::hexDump(evtParam->read.value, evtParam->read.value_len);
 				/*
-				char *pHexData = BLEUtils::buildHexData(nullptr, evtParam->read.value, evtParam->read.value_len);
-				ESP_LOGD(LOG_TAG, "value: %s \"%s\"", pHexData, BLEUtils::buildPrintData(evtParam->read.value, evtParam->read.value_len).c_str());
+				char* pHexData = BLEUtils::buildHexData(nullptr, evtParam->read.value, evtParam->read.value_len);
+				ESP_LOGV(LOG_TAG, "value: %s \"%s\"", pHexData, BLEUtils::buildPrintData(evtParam->read.value, evtParam->read.value_len).c_str());
 				free(pHexData);
 				*/
 			}
 			break;
 		} // ESP_GATTC_READ_CHAR_EVT
 
-
-		//
 		// ESP_GATTC_REG_EVT
 		//
 		// reg:
 		// - esp_gatt_status_t status
 		// - uint16_t          app_id
-		//
 		case ESP_GATTC_REG_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, app_id: 0x%x]",
+			ESP_LOGV(LOG_TAG, "[status: %s, app_id: 0x%x]",
 				BLEUtils::gattStatusToString(evtParam->reg.status).c_str(),
 				evtParam->reg.app_id);
 			break;
 		} // ESP_GATTC_REG_EVT
 
-
-		//
 		// ESP_GATTC_REG_FOR_NOTIFY_EVT
 		//
 		// reg_for_notify:
 		// - esp_gatt_status_t status
 		// - uint16_t          handle
 		case ESP_GATTC_REG_FOR_NOTIFY_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, handle: %d 0x%.2x]",
+			ESP_LOGV(LOG_TAG, "[status: %s, handle: %d 0x%.2x]",
 				BLEUtils::gattStatusToString(evtParam->reg_for_notify.status).c_str(),
 				evtParam->reg_for_notify.handle,
 				evtParam->reg_for_notify.handle
@@ -1567,23 +1460,18 @@ void BLEUtils::dumpGattClientEvent(
 			break;
 		} // ESP_GATTC_REG_FOR_NOTIFY_EVT
 
-
-		//
 		// ESP_GATTC_SEARCH_CMPL_EVT
 		//
 		// search_cmpl:
 		// - esp_gatt_status_t status
 		// - uint16_t          conn_id
-		//
 		case ESP_GATTC_SEARCH_CMPL_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d]",
 				BLEUtils::gattStatusToString(evtParam->search_cmpl.status).c_str(),
 				evtParam->search_cmpl.conn_id);
 			break;
 		} // ESP_GATTC_SEARCH_CMPL_EVT
 
-
-		//
 		// ESP_GATTC_SEARCH_RES_EVT
 		//
 		// search_res:
@@ -1591,9 +1479,8 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t      start_handle
 		// - uint16_t      end_handle
 		// - esp_gatt_id_t srvc_id
-		//
 		case ESP_GATTC_SEARCH_RES_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, start_handle: %d 0x%.2x, end_handle: %d 0x%.2x, srvc_id: %s",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, start_handle: %d 0x%.2x, end_handle: %d 0x%.2x, srvc_id: %s",
 				evtParam->search_res.conn_id,
 				evtParam->search_res.start_handle,
 				evtParam->search_res.start_handle,
@@ -1603,8 +1490,6 @@ void BLEUtils::dumpGattClientEvent(
 			break;
 		} // ESP_GATTC_SEARCH_RES_EVT
 
-
-		//
 		// ESP_GATTC_WRITE_CHAR_EVT
 		//
 		// write:
@@ -1612,9 +1497,8 @@ void BLEUtils::dumpGattClientEvent(
 		// - uint16_t          conn_id
 		// - uint16_t          handle
 		// - uint16_t          offset
-		//
 		case ESP_GATTC_WRITE_CHAR_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x, offset: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, conn_id: %d, handle: %d 0x%.2x, offset: %d]",
 				BLEUtils::gattStatusToString(evtParam->write.status).c_str(),
 				evtParam->write.conn_id,
 				evtParam->write.handle,
@@ -1623,7 +1507,7 @@ void BLEUtils::dumpGattClientEvent(
 			);
 			break;
 		} // ESP_GATTC_WRITE_CHAR_EVT
-
+#endif
 		default:
 			break;
 	}
@@ -1644,11 +1528,12 @@ void BLEUtils::dumpGattServerEvent(
 		esp_gatts_cb_event_t      event,
 		esp_gatt_if_t             gatts_if,
 		esp_ble_gatts_cb_param_t* evtParam) {
-	ESP_LOGD(LOG_TAG, "GATT ServerEvent: %s", BLEUtils::gattServerEventTypeToString(event).c_str());
-	switch(event) {
+	ESP_LOGV(LOG_TAG, "GATT ServerEvent: %s", BLEUtils::gattServerEventTypeToString(event).c_str());
+	switch (event) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 
 		case ESP_GATTS_ADD_CHAR_DESCR_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, attr_handle: %d 0x%.2x, service_handle: %d 0x%.2x, char_uuid: %s]",
+			ESP_LOGV(LOG_TAG, "[status: %s, attr_handle: %d 0x%.2x, service_handle: %d 0x%.2x, char_uuid: %s]",
 				gattStatusToString(evtParam->add_char_descr.status).c_str(),
 				evtParam->add_char_descr.attr_handle,
 				evtParam->add_char_descr.attr_handle,
@@ -1660,7 +1545,7 @@ void BLEUtils::dumpGattServerEvent(
 
 		case ESP_GATTS_ADD_CHAR_EVT: {
 			if (evtParam->add_char.status == ESP_GATT_OK) {
-				ESP_LOGD(LOG_TAG, "[status: %s, attr_handle: %d 0x%.2x, service_handle: %d 0x%.2x, char_uuid: %s]",
+				ESP_LOGV(LOG_TAG, "[status: %s, attr_handle: %d 0x%.2x, service_handle: %d 0x%.2x, char_uuid: %s]",
 					gattStatusToString(evtParam->add_char.status).c_str(),
 					evtParam->add_char.attr_handle,
 					evtParam->add_char.attr_handle,
@@ -1685,9 +1570,8 @@ void BLEUtils::dumpGattServerEvent(
 		// conf:
 		// - esp_gatt_status_t status  – The status code.
 		// - uint16_t          conn_id – The connection used.
-		//
 		case ESP_GATTS_CONF_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, conn_id: 0x%.2x]",
+			ESP_LOGV(LOG_TAG, "[status: %s, conn_id: 0x%.2x]",
 				gattStatusToString(evtParam->conf.status).c_str(),
 				evtParam->conf.conn_id);
 			break;
@@ -1695,21 +1579,21 @@ void BLEUtils::dumpGattServerEvent(
 
 
 		case ESP_GATTS_CONGEST_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, congested: %d]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, congested: %d]",
 				evtParam->congest.conn_id,
 				evtParam->congest.congested);
 			break;
 		} // ESP_GATTS_CONGEST_EVT
 
 		case ESP_GATTS_CONNECT_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
 				evtParam->connect.conn_id,
 				BLEAddress(evtParam->connect.remote_bda).toString().c_str());
 			break;
 		} // ESP_GATTS_CONNECT_EVT
 
 		case ESP_GATTS_CREATE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, service_handle: %d 0x%.2x, service_id: [%s]]",
+			ESP_LOGV(LOG_TAG, "[status: %s, service_handle: %d 0x%.2x, service_id: [%s]]",
 				gattStatusToString(evtParam->create.status).c_str(),
 				evtParam->create.service_handle,
 				evtParam->create.service_handle,
@@ -1718,7 +1602,7 @@ void BLEUtils::dumpGattServerEvent(
 		} // ESP_GATTS_CREATE_EVT
 
 		case ESP_GATTS_DISCONNECT_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, remote_bda: %s]",
 				evtParam->connect.conn_id,
 				BLEAddress(evtParam->connect.remote_bda).toString().c_str());
 			break;
@@ -1731,26 +1615,25 @@ void BLEUtils::dumpGattServerEvent(
 		// - uint32_t trans_id
 		// - esp_bd_addr_t bda
 		// - uint8_t exec_write_flag
-		//
 		case ESP_GATTS_EXEC_WRITE_EVT: {
 			char* pWriteFlagText;
-			switch(evtParam->exec_write.exec_write_flag) {
+			switch (evtParam->exec_write.exec_write_flag) {
 				case ESP_GATT_PREP_WRITE_EXEC: {
-					pWriteFlagText = (char*)"WRITE";
+					pWriteFlagText = (char*) "WRITE";
 					break;
 				}
 
 				case ESP_GATT_PREP_WRITE_CANCEL: {
-					pWriteFlagText = (char*)"CANCEL";
+					pWriteFlagText = (char*) "CANCEL";
 					break;
 				}
 
 				default:
-					pWriteFlagText = (char*)"<Unknown>";
+					pWriteFlagText = (char*) "<Unknown>";
 					break;
 			}
 
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, exec_write_flag: 0x%.2x=%s]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, exec_write_flag: 0x%.2x=%s]",
 				evtParam->exec_write.conn_id,
 				evtParam->exec_write.trans_id,
 				BLEAddress(evtParam->exec_write.bda).toString().c_str(),
@@ -1761,14 +1644,14 @@ void BLEUtils::dumpGattServerEvent(
 
 
 		case ESP_GATTS_MTU_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, mtu: %d]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, mtu: %d]",
 					evtParam->mtu.conn_id,
 					evtParam->mtu.mtu);
 			break;
 		} // ESP_GATTS_MTU_EVT
 
 		case ESP_GATTS_READ_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, handle: 0x%.2x, is_long: %d, need_rsp:%d]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, handle: 0x%.2x, is_long: %d, need_rsp:%d]",
 					evtParam->read.conn_id,
 					evtParam->read.trans_id,
 					BLEAddress(evtParam->read.bda).toString().c_str(),
@@ -1779,14 +1662,14 @@ void BLEUtils::dumpGattServerEvent(
 		} // ESP_GATTS_READ_EVT
 
 		case ESP_GATTS_RESPONSE_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, handle: 0x%.2x]",
+			ESP_LOGV(LOG_TAG, "[status: %s, handle: 0x%.2x]",
 				gattStatusToString(evtParam->rsp.status).c_str(),
 				evtParam->rsp.handle);
 			break;
 		} // ESP_GATTS_RESPONSE_EVT
 
 		case ESP_GATTS_REG_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, app_id: %d]",
+			ESP_LOGV(LOG_TAG, "[status: %s, app_id: %d]",
 				gattStatusToString(evtParam->reg.status).c_str(),
 				evtParam->reg.app_id);
 			break;
@@ -1798,9 +1681,8 @@ void BLEUtils::dumpGattServerEvent(
 		// start:
 		// - esp_gatt_status_t status
 		// - uint16_t          service_handle
-		//
 		case ESP_GATTS_START_EVT: {
-			ESP_LOGD(LOG_TAG, "[status: %s, service_handle: 0x%.2x]",
+			ESP_LOGV(LOG_TAG, "[status: %s, service_handle: 0x%.2x]",
 				gattStatusToString(evtParam->start.status).c_str(),
 				evtParam->start.service_handle);
 			break;
@@ -1819,9 +1701,8 @@ void BLEUtils::dumpGattServerEvent(
 		// - bool          is_prep  – Is this a write prepare?  If set, then this is to be considered part of the received value and not the whole value.  A subsequent ESP_GATTS_EXEC_WRITE will mark the total.
 		// - uint16_t      len      – The length of the incoming value part.
 		// - uint8_t*      value    – The data for this value part.
-		//
 		case ESP_GATTS_WRITE_EVT: {
-			ESP_LOGD(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, handle: 0x%.2x, offset: %d, need_rsp: %d, is_prep: %d, len: %d]",
+			ESP_LOGV(LOG_TAG, "[conn_id: %d, trans_id: %d, bda: %s, handle: 0x%.2x, offset: %d, need_rsp: %d, is_prep: %d, len: %d]",
 					evtParam->write.conn_id,
 					evtParam->write.trans_id,
 					BLEAddress(evtParam->write.bda).toString().c_str(),
@@ -1831,13 +1712,13 @@ void BLEUtils::dumpGattServerEvent(
 					evtParam->write.is_prep,
 					evtParam->write.len);
 			char* pHex = buildHexData(nullptr, evtParam->write.value, evtParam->write.len);
-			ESP_LOGD(LOG_TAG, "[Data: %s]", pHex);
+			ESP_LOGV(LOG_TAG, "[Data: %s]", pHex);
 			free(pHex);
 			break;
 		} // ESP_GATTS_WRITE_EVT
-
+#endif
 		default:
-			ESP_LOGD(LOG_TAG, "dumpGattServerEvent: *** NOT CODED ***");
+			ESP_LOGV(LOG_TAG, "dumpGattServerEvent: *** NOT CODED ***");
 			break;
 		}
 } // dumpGattServerEvent
@@ -1849,7 +1730,8 @@ void BLEUtils::dumpGattServerEvent(
  * @return The event type as a string.
  */
 const char* BLEUtils::eventTypeToString(esp_ble_evt_type_t eventType) {
-	switch(eventType) {
+	switch (eventType) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_BLE_EVT_CONN_ADV:
 			return "ESP_BLE_EVT_CONN_ADV";
 		case ESP_BLE_EVT_CONN_DIR_ADV:
@@ -1860,8 +1742,9 @@ const char* BLEUtils::eventTypeToString(esp_ble_evt_type_t eventType) {
 			return "ESP_BLE_EVT_NON_CONN_ADV";
 		case ESP_BLE_EVT_SCAN_RSP:
 			return "ESP_BLE_EVT_SCAN_RSP";
+#endif
 		default:
-			ESP_LOGD(LOG_TAG, "Unknown esp_ble_evt_type_t: %d (0x%.2x)", eventType, eventType);
+			ESP_LOGV(LOG_TAG, "Unknown esp_ble_evt_type_t: %d (0x%.2x)", eventType, eventType);
 			return "*** Unknown ***";
 	}
 } // eventTypeToString
@@ -1874,98 +1757,72 @@ const char* BLEUtils::eventTypeToString(esp_ble_evt_type_t eventType) {
  * @return A string representation of the event type.
  */
 const char* BLEUtils::gapEventToString(uint32_t eventType) {
-	switch(eventType) {
+	switch (eventType) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT:
 			return "ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT:
 			return "ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
 			return "ESP_GAP_BLE_ADV_START_COMPLETE_EVT";
-
-		case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:                      /*!< When stop adv complete, the event comes */
+		case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:					  /* !< When stop adv complete, the event comes */
 			return "ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT";
-
-		case ESP_GAP_BLE_AUTH_CMPL_EVT:                              /* Authentication complete indication. */
+		case ESP_GAP_BLE_AUTH_CMPL_EVT:							  /* Authentication complete indication. */
 			return "ESP_GAP_BLE_AUTH_CMPL_EVT";
-
 		case ESP_GAP_BLE_CLEAR_BOND_DEV_COMPLETE_EVT:
 			return "ESP_GAP_BLE_CLEAR_BOND_DEV_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_GET_BOND_DEV_COMPLETE_EVT:
 			return "ESP_GAP_BLE_GET_BOND_DEV_COMPLETE_EVT";
-
-		case ESP_GAP_BLE_KEY_EVT:                                    /* BLE  key event for peer device keys */
+		case ESP_GAP_BLE_KEY_EVT:									/* BLE  key event for peer device keys */
 			return "ESP_GAP_BLE_KEY_EVT";
-
-		case ESP_GAP_BLE_LOCAL_IR_EVT:                               /* BLE local IR event */
+		case ESP_GAP_BLE_LOCAL_IR_EVT:							   /* BLE local IR event */
 			return "ESP_GAP_BLE_LOCAL_IR_EVT";
-
-		case ESP_GAP_BLE_LOCAL_ER_EVT:                               /* BLE local ER event */
+		case ESP_GAP_BLE_LOCAL_ER_EVT:							   /* BLE local ER event */
 			return "ESP_GAP_BLE_LOCAL_ER_EVT";
-
-		case ESP_GAP_BLE_NC_REQ_EVT:                                 /* Numeric Comparison request event */
+		case ESP_GAP_BLE_NC_REQ_EVT:								 /* Numeric Comparison request event */
 			return "ESP_GAP_BLE_NC_REQ_EVT";
-
-		case ESP_GAP_BLE_OOB_REQ_EVT:                                /* OOB request event */
+		case ESP_GAP_BLE_OOB_REQ_EVT:								/* OOB request event */
 			return "ESP_GAP_BLE_OOB_REQ_EVT";
-
-		case ESP_GAP_BLE_PASSKEY_NOTIF_EVT:                          /* passkey notification event */
+		case ESP_GAP_BLE_PASSKEY_NOTIF_EVT:						  /* passkey notification event */
 			return "ESP_GAP_BLE_PASSKEY_NOTIF_EVT";
-
-		case ESP_GAP_BLE_PASSKEY_REQ_EVT:                            /* passkey request event */
+		case ESP_GAP_BLE_PASSKEY_REQ_EVT:							/* passkey request event */
 			return "ESP_GAP_BLE_PASSKEY_REQ_EVT";
-
 		case ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT:
 			return "ESP_GAP_BLE_READ_RSSI_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_REMOVE_BOND_DEV_COMPLETE_EVT:
 			return "ESP_GAP_BLE_REMOVE_BOND_DEV_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SCAN_RESULT_EVT:
 			return "ESP_GAP_BLE_SCAN_RESULT_EVT";
-
 		case ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SCAN_RSP_DATA_RAW_SET_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SCAN_START_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SCAN_START_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SCAN_STOP_COMPLETE_EVT";
-
-		case ESP_GAP_BLE_SEC_REQ_EVT:                                /* BLE  security request */
+		case ESP_GAP_BLE_SEC_REQ_EVT:								/* BLE  security request */
 			return "ESP_GAP_BLE_SEC_REQ_EVT";
-
 		case ESP_GAP_BLE_SET_LOCAL_PRIVACY_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SET_LOCAL_PRIVACY_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SET_PKT_LENGTH_COMPLETE_EVT:
 			return "ESP_GAP_BLE_SET_PKT_LENGTH_COMPLETE_EVT";
-
 		case ESP_GAP_BLE_SET_STATIC_RAND_ADDR_EVT:
 			return "ESP_GAP_BLE_SET_STATIC_RAND_ADDR_EVT";
-
 		case ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT:
 			return "ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT";
-
-
+#endif
 		default:
-			ESP_LOGD(LOG_TAG, "gapEventToString: Unknown event type %d 0x%.2x", eventType, eventType);
+			ESP_LOGV(LOG_TAG, "gapEventToString: Unknown event type %d 0x%.2x", eventType, eventType);
 			return "Unknown event type";
 	}
 } // gapEventToString
 
 
 std::string BLEUtils::gattCharacteristicUUIDToString(uint32_t characteristicUUID) {
-	const characteristicMap_t *p = g_characteristicsMappings;
+	const characteristicMap_t* p = g_characteristicsMappings;
 	while (strlen(p->name) > 0) {
 		if (p->assignedNumber == characteristicUUID) {
 			return std::string(p->name);
@@ -1982,7 +1839,7 @@ std::string BLEUtils::gattCharacteristicUUIDToString(uint32_t characteristicUUID
  * @return The string representation of a descriptor UUID.
  */
 std::string BLEUtils::gattDescriptorUUIDToString(uint32_t descriptorUUID) {
-	gattdescriptor_t* p = (gattdescriptor_t *)g_descriptor_ids;
+	gattdescriptor_t* p = (gattdescriptor_t*) g_descriptor_ids;
 	while (strlen(p->name) > 0) {
 		if (p->assignedNumber == descriptorUUID) {
 			return std::string(p->name);
@@ -2018,7 +1875,7 @@ std::string BLEUtils::gattServiceIdToString(esp_gatt_srvc_id_t srvcId) {
 
 
 std::string BLEUtils::gattServiceToString(uint32_t serviceId) {
-	gattService_t* p = (gattService_t *)g_gattServices;
+	gattService_t* p = (gattService_t*) g_gattServices;
 	while (strlen(p->name) > 0) {
 		if (p->assignedNumber == serviceId) {
 			return std::string(p->name);
@@ -2036,7 +1893,8 @@ std::string BLEUtils::gattServiceToString(uint32_t serviceId) {
  * @return A string representation of the status.
  */
 std::string BLEUtils::gattStatusToString(esp_gatt_status_t status) {
-	switch(status) {
+	switch (status) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_GATT_OK:
 			return "ESP_GATT_OK";
 		case ESP_GATT_INVALID_HANDLE:
@@ -2123,6 +1981,7 @@ std::string BLEUtils::gattStatusToString(esp_gatt_status_t status) {
 			return "ESP_GATT_PRC_IN_PROGRESS";
 		case ESP_GATT_OUT_OF_RANGE:
 			return "ESP_GATT_OUT_OF_RANGE";
+#endif
 		default:
 			return "Unknown";
 	}
@@ -2131,11 +1990,11 @@ std::string BLEUtils::gattStatusToString(esp_gatt_status_t status) {
 
 
 std::string BLEUtils::getMember(uint32_t memberId) {
-	member_t* p = (member_t *)members_ids;
+	member_t* p = (member_t*) members_ids;
 
 	while (strlen(p->name) > 0) {
 		if (p->assignedNumber == memberId) {
-				return std::string(p->name);
+			return std::string(p->name);
 		}
 		p++;
 	}
@@ -2148,7 +2007,8 @@ std::string BLEUtils::getMember(uint32_t memberId) {
  * @return The search event type as a string.
  */
 const char* BLEUtils::searchEventTypeToString(esp_gap_search_evt_t searchEvt) {
-	switch(searchEvt) {
+	switch (searchEvt) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_GAP_SEARCH_INQ_RES_EVT:
 			return "ESP_GAP_SEARCH_INQ_RES_EVT";
 		case ESP_GAP_SEARCH_INQ_CMPL_EVT:
@@ -2163,8 +2023,9 @@ const char* BLEUtils::searchEventTypeToString(esp_gap_search_evt_t searchEvt) {
 			return "ESP_GAP_SEARCH_DI_DISC_CMPL_EVT";
 		case ESP_GAP_SEARCH_SEARCH_CANCEL_CMPL_EVT:
 			return "ESP_GAP_SEARCH_SEARCH_CANCEL_CMPL_EVT";
+#endif
 		default:
-			ESP_LOGD(LOG_TAG, "Unknown event type: 0x%x", searchEvt);
+			ESP_LOGV(LOG_TAG, "Unknown event type: 0x%x", searchEvt);
 			return "Unknown event type";
 	}
 } // searchEventTypeToString
diff --git a/sensor/patchedBLE/src/BLEUtils.h b/sensor/patchedBLE/src/BLEUtils.h
index b2baee5..7981691 100644
--- a/sensor/patchedBLE/src/BLEUtils.h
+++ b/sensor/patchedBLE/src/BLEUtils.h
@@ -23,12 +23,12 @@ public:
 	static const char*        addressTypeToString(esp_ble_addr_type_t type);
 	static std::string        adFlagsToString(uint8_t adFlags);
 	static const char*        advTypeToString(uint8_t advType);
-	static esp_gatt_id_t      buildGattId(esp_bt_uuid_t uuid, uint8_t inst_id=0);
-	static esp_gatt_srvc_id_t buildGattSrvcId(esp_gatt_id_t gattId, bool is_primary=true);
 	static char*              buildHexData(uint8_t* target, uint8_t* source, uint8_t length);
 	static std::string        buildPrintData(uint8_t* source, size_t length);
 	static std::string        characteristicPropertiesToString(esp_gatt_char_prop_t prop);
 	static const char*        devTypeToString(esp_bt_dev_type_t type);
+	static esp_gatt_id_t	  buildGattId(esp_bt_uuid_t uuid, uint8_t inst_id = 0);
+	static esp_gatt_srvc_id_t buildGattSrvcId(esp_gatt_id_t gattId, bool is_primary = true);
 	static void dumpGapEvent(
 		esp_gap_ble_cb_event_t  event,
 		esp_ble_gap_cb_param_t* param);
@@ -47,7 +47,7 @@ public:
 	static std::string gattCharacteristicUUIDToString(uint32_t characteristicUUID);
 	static std::string gattClientEventTypeToString(esp_gattc_cb_event_t eventType);
 	static std::string gattCloseReasonToString(esp_gatt_conn_reason_t reason);
-	static std::string gattcServiceElementToString(esp_gattc_service_elem_t *pGATTCServiceElement);
+	static std::string gattcServiceElementToString(esp_gattc_service_elem_t* pGATTCServiceElement);
 	static std::string gattDescriptorUUIDToString(uint32_t descriptorUUID);
 	static std::string gattServerEventTypeToString(esp_gatts_cb_event_t eventType);
 	static std::string gattServiceIdToString(esp_gatt_srvc_id_t srvcId);
diff --git a/sensor/patchedBLE/src/BLEValue.cpp b/sensor/patchedBLE/src/BLEValue.cpp
index 49818e2..ec1e61f 100644
--- a/sensor/patchedBLE/src/BLEValue.cpp
+++ b/sensor/patchedBLE/src/BLEValue.cpp
@@ -6,15 +6,17 @@
  */
 #include "sdkconfig.h"
 #if defined(CONFIG_BT_ENABLED)
-
-#include <esp_log.h>
-
 #include "BLEValue.h"
-#ifdef ARDUINO_ARCH_ESP32
+
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
 #include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
+static const char* LOG_TAG="BLEValue";
 #endif
 
-static const char* LOG_TAG="BLEValue";
+
 
 BLEValue::BLEValue() {
 	m_accumulation = "";
@@ -42,7 +44,7 @@ void BLEValue::addPart(std::string part) {
  */
 void BLEValue::addPart(uint8_t* pData, size_t length) {
 	ESP_LOGD(LOG_TAG, ">> addPart: length=%d", length);
-	m_accumulation += std::string((char *)pData, length);
+	m_accumulation += std::string((char*) pData, length);
 } // addPart
 
 
@@ -65,9 +67,7 @@ void BLEValue::cancel() {
 void BLEValue::commit() {
 	ESP_LOGD(LOG_TAG, ">> commit");
 	// If there is nothing to commit, do nothing.
-	if (m_accumulation.length() == 0) {
-		return;
-	}
+	if (m_accumulation.length() == 0) return;
 	setValue(m_accumulation);
 	m_accumulation = "";
 	m_readOffset   = 0;
@@ -79,7 +79,7 @@ void BLEValue::commit() {
  * @return A pointer to the data.
  */
 uint8_t* BLEValue::getData() {
-	return (uint8_t*)m_value.data();
+	return (uint8_t*) m_value.data();
 }
 
 
@@ -132,11 +132,8 @@ void BLEValue::setValue(std::string value) {
  * @param [in] The length of the new current value.
  */
 void BLEValue::setValue(uint8_t* pData, size_t length) {
-	m_value = std::string((char*)pData, length);
+	m_value = std::string((char*) pData, length);
 } // setValue
 
 
-
-
-
 #endif // CONFIG_BT_ENABLED
diff --git a/sensor/patchedBLE/src/BLEValue.h b/sensor/patchedBLE/src/BLEValue.h
index 92a7f9a..5df904c 100644
--- a/sensor/patchedBLE/src/BLEValue.h
+++ b/sensor/patchedBLE/src/BLEValue.h
@@ -17,13 +17,13 @@
 class BLEValue {
 public:
 	BLEValue();
-	void        addPart(std::string part);
-	void        addPart(uint8_t* pData, size_t length);
-	void        cancel();
-	void        commit();
-	uint8_t*    getData();
-	size_t      getLength();
-	uint16_t    getReadOffset();
+	void		addPart(std::string part);
+	void		addPart(uint8_t* pData, size_t length);
+	void		cancel();
+	void		commit();
+	uint8_t*	getData();
+	size_t	  getLength();
+	uint16_t	getReadOffset();
 	std::string getValue();
 	void        setReadOffset(uint16_t readOffset);
 	void        setValue(std::string value);
@@ -33,6 +33,7 @@ private:
 	std::string m_accumulation;
 	uint16_t    m_readOffset;
 	std::string m_value;
+
 };
 #endif // CONFIG_BT_ENABLED
 #endif /* COMPONENTS_CPP_UTILS_BLEVALUE_H_ */
diff --git a/sensor/patchedBLE/src/FreeRTOS.cpp b/sensor/patchedBLE/src/FreeRTOS.cpp
index 1ae01d7..1f12c88 100644
--- a/sensor/patchedBLE/src/FreeRTOS.cpp
+++ b/sensor/patchedBLE/src/FreeRTOS.cpp
@@ -11,17 +11,22 @@
 #include <sstream>
 #include <iomanip>
 #include "FreeRTOS.h"
-#include <esp_log.h>
 #include "sdkconfig.h"
-
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
+#include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
 static const char* LOG_TAG = "FreeRTOS";
+#endif
+
 
 /**
  * Sleep for the specified number of milliseconds.
  * @param[in] ms The period in milliseconds for which to sleep.
  */
 void FreeRTOS::sleep(uint32_t ms) {
-	::vTaskDelay(ms/portTICK_PERIOD_MS);
+	::vTaskDelay(ms / portTICK_PERIOD_MS);
 } // sleep
 
 
@@ -32,7 +37,7 @@ void FreeRTOS::sleep(uint32_t ms) {
  * @param[in] param An optional parameter to be passed to the started task.
  * @param[in] stackSize An optional paremeter supplying the size of the stack in which to run the task.
  */
-void FreeRTOS::startTask(void task(void*), std::string taskName, void *param, int stackSize) {
+void FreeRTOS::startTask(void task(void*), std::string taskName, void* param, uint32_t stackSize) {
 	::xTaskCreate(task, taskName.data(), stackSize, param, 5, NULL);
 } // startTask
 
@@ -51,23 +56,9 @@ void FreeRTOS::deleteTask(TaskHandle_t pTask) {
  * @return The time in milliseconds since the %FreeRTOS scheduler started.
  */
 uint32_t FreeRTOS::getTimeSinceStart() {
-	return (uint32_t)(xTaskGetTickCount()*portTICK_PERIOD_MS);
+	return (uint32_t) (xTaskGetTickCount() * portTICK_PERIOD_MS);
 } // getTimeSinceStart
 
-/*
- * 	public:
-		Semaphore(std::string = "<Unknown>");
-		~Semaphore();
-		void give();
-		void take(std::string owner="<Unknown>");
-		void take(uint32_t timeoutMs, std::string owner="<Unknown>");
-	private:
-		SemaphoreHandle_t m_semaphore;
-		std::string m_name;
-		std::string m_owner;
-	};
- *
- */
 
 /**
  * @brief Wait for a semaphore to be released by trying to take it and
@@ -78,7 +69,6 @@ uint32_t FreeRTOS::getTimeSinceStart() {
 uint32_t FreeRTOS::Semaphore::wait(std::string owner) {
 	ESP_LOGV(LOG_TAG, ">> wait: Semaphore waiting: %s for %s", toString().c_str(), owner.c_str());
 
-
 	if (m_usePthreads) {
 		pthread_mutex_lock(&m_pthread_mutex);
 	} else {
@@ -134,7 +124,7 @@ void FreeRTOS::Semaphore::give() {
 		xSemaphoreGive(m_semaphore);
 	}
 // #ifdef ARDUINO_ARCH_ESP32
-// 	FreeRTOS::sleep(10); 
+// 	FreeRTOS::sleep(10);
 // #endif
 
 	m_owner = std::string("<N/A>");
@@ -171,14 +161,13 @@ void FreeRTOS::Semaphore::giveFromISR() {
  * @param [in] owner The new owner (for debugging)
  * @return True if we took the semaphore.
  */
-bool FreeRTOS::Semaphore::take(std::string owner)
-{
+bool FreeRTOS::Semaphore::take(std::string owner) {
 	ESP_LOGD(LOG_TAG, "Semaphore taking: %s for %s", toString().c_str(), owner.c_str());
 	bool rc = false;
 	if (m_usePthreads) {
 		pthread_mutex_lock(&m_pthread_mutex);
 	} else {
-		rc = ::xSemaphoreTake(m_semaphore, portMAX_DELAY);
+		rc = ::xSemaphoreTake(m_semaphore, portMAX_DELAY) == pdTRUE;
 	}
 	m_owner = owner;
 	if (rc) {
@@ -198,13 +187,12 @@ bool FreeRTOS::Semaphore::take(std::string owner)
  * @return True if we took the semaphore.
  */
 bool FreeRTOS::Semaphore::take(uint32_t timeoutMs, std::string owner) {
-
 	ESP_LOGV(LOG_TAG, "Semaphore taking: %s for %s", toString().c_str(), owner.c_str());
 	bool rc = false;
 	if (m_usePthreads) {
 		assert(false);  // We apparently don't have a timed wait for pthreads.
 	} else {
-		rc = ::xSemaphoreTake(m_semaphore, timeoutMs/portTICK_PERIOD_MS);
+		rc = ::xSemaphoreTake(m_semaphore, timeoutMs / portTICK_PERIOD_MS) == pdTRUE;
 	}
 	m_owner = owner;
 	if (rc) {
@@ -279,8 +267,8 @@ void Ringbuffer::returnItem(void* item) {
  * @param [in] wait How long to wait before giving up.  The default is to wait indefinitely.
  * @return
  */
-uint32_t Ringbuffer::send(void* data, size_t length, TickType_t wait) {
-	return ::xRingbufferSend(m_handle, data, length, wait);
+bool Ringbuffer::send(void* data, size_t length, TickType_t wait) {
+	return ::xRingbufferSend(m_handle, data, length, wait) == pdTRUE;
 } // send
 
 
diff --git a/sensor/patchedBLE/src/FreeRTOS.h b/sensor/patchedBLE/src/FreeRTOS.h
index ab0e83d..b861c87 100644
--- a/sensor/patchedBLE/src/FreeRTOS.h
+++ b/sensor/patchedBLE/src/FreeRTOS.h
@@ -23,7 +23,7 @@
 class FreeRTOS {
 public:
 	static void sleep(uint32_t ms);
-	static void startTask(void task(void *), std::string taskName, void *param=nullptr, int stackSize = 2048);
+	static void startTask(void task(void*), std::string taskName, void* param = nullptr, uint32_t stackSize = 2048);
 	static void deleteTask(TaskHandle_t pTask = nullptr);
 
 	static uint32_t getTimeSinceStart();
@@ -36,10 +36,10 @@ public:
 		void        give(uint32_t value);
 		void        giveFromISR();
 		void        setName(std::string name);
-		bool        take(std::string owner="<Unknown>");
-		bool        take(uint32_t timeoutMs, std::string owner="<Unknown>");
+		bool        take(std::string owner = "<Unknown>");
+		bool        take(uint32_t timeoutMs, std::string owner = "<Unknown>");
 		std::string toString();
-		uint32_t    wait(std::string owner="<Unknown>");
+		uint32_t	wait(std::string owner = "<Unknown>");
 
 	private:
 		SemaphoreHandle_t m_semaphore;
@@ -48,6 +48,7 @@ public:
 		std::string       m_owner;
 		uint32_t          m_value;
 		bool              m_usePthreads;
+
 	};
 };
 
@@ -62,7 +63,7 @@ public:
 
 	void*    receive(size_t* size, TickType_t wait = portMAX_DELAY);
 	void     returnItem(void* item);
-	uint32_t send(void* data, size_t length, TickType_t wait = portMAX_DELAY);
+	bool     send(void* data, size_t length, TickType_t wait = portMAX_DELAY);
 private:
 	RingbufHandle_t m_handle;
 };
diff --git a/sensor/patchedBLE/src/GeneralUtils.cpp b/sensor/patchedBLE/src/GeneralUtils.cpp
index ccf74f7..019c81b 100644
--- a/sensor/patchedBLE/src/GeneralUtils.cpp
+++ b/sensor/patchedBLE/src/GeneralUtils.cpp
@@ -6,37 +6,43 @@
  */
 
 #include "GeneralUtils.h"
-#include <esp_log.h>
 #include <esp_system.h>
 #include <string.h>
 #include <stdio.h>
 #include <string>
 #include <sstream>
 #include <iomanip>
-#include <FreeRTOS.h>
+#include "FreeRTOS.h"
 #include <esp_err.h>
 #include <nvs.h>
 #include <esp_wifi.h>
 #include <esp_heap_caps.h>
 #include <esp_system.h>
 
+#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
+#include "esp32-hal-log.h"
+#define LOG_TAG ""
+#else
+#include "esp_log.h"
 static const char* LOG_TAG = "GeneralUtils";
+#endif
+
 
 static const char kBase64Alphabet[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
-    "abcdefghijklmnopqrstuvwxyz"
-    "0123456789+/";
+	"abcdefghijklmnopqrstuvwxyz"
+	"0123456789+/";
 
 static int base64EncodedLength(size_t length) {
 	return (length + 2 - ((length + 2) % 3)) / 3 * 4;
 } // base64EncodedLength
 
 
-static int base64EncodedLength(const std::string &in) {
+static int base64EncodedLength(const std::string& in) {
 	return base64EncodedLength(in.length());
 } // base64EncodedLength
 
 
-static void a3_to_a4(unsigned char * a4, unsigned char * a3) {
+static void a3_to_a4(unsigned char* a4, unsigned char* a3) {
 	a4[0] = (a3[0] & 0xfc) >> 2;
 	a4[1] = ((a3[0] & 0x03) << 4) + ((a3[1] & 0xf0) >> 4);
 	a4[2] = ((a3[1] & 0x0f) << 2) + ((a3[2] & 0xc0) >> 6);
@@ -44,7 +50,7 @@ static void a3_to_a4(unsigned char * a4, unsigned char * a3) {
 } // a3_to_a4
 
 
-static void a4_to_a3(unsigned char * a3, unsigned char * a4) {
+static void a4_to_a3(unsigned char* a3, unsigned char* a4) {
 	a3[0] = (a4[0] << 2) + ((a4[1] & 0x30) >> 4);
 	a3[1] = ((a4[1] & 0xf) << 4) + ((a4[2] & 0x3c) >> 2);
 	a3[2] = ((a4[2] & 0x3) << 6) + a4[3];
@@ -56,7 +62,7 @@ static void a4_to_a3(unsigned char * a3, unsigned char * a4) {
  * @param [in] in
  * @param [out] out
  */
-bool GeneralUtils::base64Encode(const std::string &in, std::string *out) {
+bool GeneralUtils::base64Encode(const std::string& in, std::string* out) {
 	int i = 0, j = 0;
 	size_t enc_len = 0;
 	unsigned char a3[3];
@@ -109,11 +115,11 @@ void GeneralUtils::dumpInfo() {
 	size_t freeHeap = heap_caps_get_free_size(MALLOC_CAP_8BIT);
 	esp_chip_info_t chipInfo;
 	esp_chip_info(&chipInfo);
-	ESP_LOGD(LOG_TAG, "--- dumpInfo ---");
-	ESP_LOGD(LOG_TAG, "Free heap: %d", freeHeap);
-	ESP_LOGD(LOG_TAG, "Chip Info: Model: %d, cores: %d, revision: %d", chipInfo.model, chipInfo.cores, chipInfo.revision);
-	ESP_LOGD(LOG_TAG, "ESP-IDF version: %s", esp_get_idf_version());
-	ESP_LOGD(LOG_TAG, "---");
+	ESP_LOGV(LOG_TAG, "--- dumpInfo ---");
+	ESP_LOGV(LOG_TAG, "Free heap: %d", freeHeap);
+	ESP_LOGV(LOG_TAG, "Chip Info: Model: %d, cores: %d, revision: %d", chipInfo.model, chipInfo.cores, chipInfo.revision);
+	ESP_LOGV(LOG_TAG, "ESP-IDF version: %s", esp_get_idf_version());
+	ESP_LOGV(LOG_TAG, "---");
 } // dumpInfo
 
 
@@ -127,16 +133,16 @@ bool GeneralUtils::endsWith(std::string str, char c) {
 	if (str.empty()) {
 		return false;
 	}
-	if (str.at(str.length()-1) == c) {
+	if (str.at(str.length() - 1) == c) {
 		return true;
 	}
 	return false;
 } // endsWidth
 
 
-static int DecodedLength(const std::string &in) {
+static int DecodedLength(const std::string& in) {
 	int numEq = 0;
-	int n = in.size();
+	int n = (int) in.size();
 
 	for (std::string::const_reverse_iterator it = in.rbegin(); *it == '='; ++it) {
 		++numEq;
@@ -160,7 +166,7 @@ static unsigned char b64_lookup(unsigned char c) {
  * @param [in] in The string to be decoded.
  * @param [out] out The resulting data.
  */
-bool GeneralUtils::base64Decode(const std::string &in, std::string *out) {
+bool GeneralUtils::base64Decode(const std::string& in, std::string* out) {
 	int i = 0, j = 0;
 	size_t dec_len = 0;
 	unsigned char a3[3];
@@ -231,7 +237,7 @@ void GeneralUtils::hexDump(uint8_t* pData, uint32_t length) {
 		if (index % 16 == 0) {
 			strcpy(hexBuf, hex.str().c_str());
 			strcpy(asciiBuf, ascii.str().c_str());
-			ESP_LOGD(tag, "%s %s", hexBuf, asciiBuf);
+			ESP_LOGV(tag, "%s %s", hexBuf, asciiBuf);
 			hex.str("");
 			ascii.str("");
 		}
@@ -243,8 +249,8 @@ void GeneralUtils::hexDump(uint8_t* pData, uint32_t length) {
 		}
 		strcpy(hexBuf, hex.str().c_str());
 		strcpy(asciiBuf, ascii.str().c_str());
-		ESP_LOGD(tag, "%s %s", hexBuf, asciiBuf);
-		//ESP_LOGD(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
+		ESP_LOGV(tag, "%s %s", hexBuf, asciiBuf);
+		//ESP_LOGV(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
 	}
 	FreeRTOS::sleep(1000);
 }
@@ -266,7 +272,7 @@ void GeneralUtils::hexDump(uint8_t* pData, uint32_t length) {
 		}
 		index++;
 		if (index % 16 == 0) {
-			ESP_LOGD(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
+			ESP_LOGV(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
 			hex.str("");
 			ascii.str("");
 		}
@@ -276,7 +282,7 @@ void GeneralUtils::hexDump(uint8_t* pData, uint32_t length) {
 			hex << "   ";
 			index++;
 		}
-		ESP_LOGD(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
+		ESP_LOGV(tag, "%s %s", hex.str().c_str(), ascii.str().c_str());
 	}
 	FreeRTOS::sleep(1000);
 }
@@ -296,11 +302,12 @@ void GeneralUtils::hexDump(const uint8_t* pData, uint32_t length) {
 	char tempBuf[80];
 	uint32_t lineNumber = 0;
 
-	ESP_LOGD(LOG_TAG, "     00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f  ----------------");
+	ESP_LOGV(LOG_TAG, "     00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f");
+	ESP_LOGV(LOG_TAG, "     -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --");
 	strcpy(ascii, "");
 	strcpy(hex, "");
-	uint32_t index=0;
-	while(index < length) {
+	uint32_t index = 0;
+	while (index < length) {
 		sprintf(tempBuf, "%.2x ", pData[index]);
 		strcat(hex, tempBuf);
 		if (isprint(pData[index])) {
@@ -311,18 +318,18 @@ void GeneralUtils::hexDump(const uint8_t* pData, uint32_t length) {
 		strcat(ascii, tempBuf);
 		index++;
 		if (index % 16 == 0) {
-			ESP_LOGD(LOG_TAG, "%.4x %s %s", lineNumber*16, hex, ascii);
+			ESP_LOGV(LOG_TAG, "%.4x %s %s", lineNumber * 16, hex, ascii);
 			strcpy(ascii, "");
 			strcpy(hex, "");
 			lineNumber++;
 		}
 	}
 	if (index %16 != 0) {
-		while(index % 16 != 0) {
+		while (index % 16 != 0) {
 			strcat(hex, "   ");
 			index++;
 		}
-		ESP_LOGD(LOG_TAG, "%.4x %s %s", lineNumber*16, hex, ascii);
+		ESP_LOGV(LOG_TAG, "%.4x %s %s", lineNumber * 16, hex, ascii);
 	}
 } // hexDump
 
@@ -334,7 +341,7 @@ void GeneralUtils::hexDump(const uint8_t* pData, uint32_t length) {
  */
 std::string GeneralUtils::ipToString(uint8_t *ip) {
 	std::stringstream s;
-	s << (int)ip[0] << '.' << (int)ip[1] << '.' << (int)ip[2] << '.' << (int)ip[3];
+	s << (int) ip[0] << '.' << (int) ip[1] << '.' << (int) ip[2] << '.' << (int) ip[3];
 	return s.str();
 } // ipToString
 
@@ -350,7 +357,7 @@ std::vector<std::string> GeneralUtils::split(std::string source, char delimiter)
 	std::vector<std::string> strings;
 	std::istringstream iss(source);
 	std::string s;
-	while(std::getline(iss, s, delimiter)) {
+	while (std::getline(iss, s, delimiter)) {
 		strings.push_back(trim(s));
 	}
 	return strings;
@@ -363,25 +370,26 @@ std::vector<std::string> GeneralUtils::split(std::string source, char delimiter)
  * @return A string representation of the error code.
  */
 const char* GeneralUtils::errorToString(esp_err_t errCode) {
-	switch(errCode) {
+	switch (errCode) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
 		case ESP_OK:
-			return "OK";
+			return "ESP_OK";
 		case ESP_FAIL:
-			return "Fail";
+			return "ESP_FAIL";
 		case ESP_ERR_NO_MEM:
-			return "No memory";
+			return "ESP_ERR_NO_MEM";
 		case ESP_ERR_INVALID_ARG:
-			return "Invalid argument";
+			return "ESP_ERR_INVALID_ARG";
 		case ESP_ERR_INVALID_SIZE:
-			return "Invalid state";
+			return "ESP_ERR_INVALID_SIZE";
 		case ESP_ERR_INVALID_STATE:
-			return "Invalid state";
+			return "ESP_ERR_INVALID_STATE";
 		case ESP_ERR_NOT_FOUND:
-			return "Not found";
+			return "ESP_ERR_NOT_FOUND";
 		case ESP_ERR_NOT_SUPPORTED:
-			return "Not supported";
+			return "ESP_ERR_NOT_SUPPORTED";
 		case ESP_ERR_TIMEOUT:
-			return "Timeout";
+			return "ESP_ERR_TIMEOUT";
 		case ESP_ERR_NVS_NOT_INITIALIZED:
 			return "ESP_ERR_NVS_NOT_INITIALIZED";
 		case ESP_ERR_NVS_NOT_FOUND:
@@ -430,10 +438,87 @@ const char* GeneralUtils::errorToString(esp_err_t errCode) {
 			return "ESP_ERR_WIFI_TIMEOUT";
 		case ESP_ERR_WIFI_WAKE_FAIL:
 			return "ESP_ERR_WIFI_WAKE_FAIL";
+#endif
+		default:
+			return "Unknown ESP_ERR error";
 		}
-	return "Unknown ESP_ERR error";
 } // errorToString
 
+/**
+ * @brief Convert a wifi_err_reason_t code to a string.
+ * @param [in] errCode The errCode to be converted.
+ * @return A string representation of the error code.
+ *
+ * @note: wifi_err_reason_t values as of April 2018 are: (1-24, 200-204) and are defined in ~/esp-idf/components/esp32/include/esp_wifi_types.h.
+ */
+const char* GeneralUtils::wifiErrorToString(uint8_t errCode) {
+	if (errCode == ESP_OK) return "ESP_OK (received SYSTEM_EVENT_STA_GOT_IP event)";
+	if (errCode == UINT8_MAX) return "Not Connected (default value)";
+
+	switch ((wifi_err_reason_t) errCode) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 4
+		case WIFI_REASON_UNSPECIFIED:
+			return "WIFI_REASON_UNSPECIFIED";
+		case WIFI_REASON_AUTH_EXPIRE:
+			return "WIFI_REASON_AUTH_EXPIRE";
+		case WIFI_REASON_AUTH_LEAVE:
+			return "WIFI_REASON_AUTH_LEAVE";
+		case WIFI_REASON_ASSOC_EXPIRE:
+			return "WIFI_REASON_ASSOC_EXPIRE";
+		case WIFI_REASON_ASSOC_TOOMANY:
+			return "WIFI_REASON_ASSOC_TOOMANY";
+		case WIFI_REASON_NOT_AUTHED:
+			return "WIFI_REASON_NOT_AUTHED";
+		case WIFI_REASON_NOT_ASSOCED:
+			return "WIFI_REASON_NOT_ASSOCED";
+		case WIFI_REASON_ASSOC_LEAVE:
+			return "WIFI_REASON_ASSOC_LEAVE";
+		case WIFI_REASON_ASSOC_NOT_AUTHED:
+			return "WIFI_REASON_ASSOC_NOT_AUTHED";
+		case WIFI_REASON_DISASSOC_PWRCAP_BAD:
+			return "WIFI_REASON_DISASSOC_PWRCAP_BAD";
+		case WIFI_REASON_DISASSOC_SUPCHAN_BAD:
+			return "WIFI_REASON_DISASSOC_SUPCHAN_BAD";
+		case WIFI_REASON_IE_INVALID:
+			return "WIFI_REASON_IE_INVALID";
+		case WIFI_REASON_MIC_FAILURE:
+			return "WIFI_REASON_MIC_FAILURE";
+		case WIFI_REASON_4WAY_HANDSHAKE_TIMEOUT:
+			return "WIFI_REASON_4WAY_HANDSHAKE_TIMEOUT";
+		case WIFI_REASON_GROUP_KEY_UPDATE_TIMEOUT:
+			return "WIFI_REASON_GROUP_KEY_UPDATE_TIMEOUT";
+		case WIFI_REASON_IE_IN_4WAY_DIFFERS:
+			return "WIFI_REASON_IE_IN_4WAY_DIFFERS";
+		case WIFI_REASON_GROUP_CIPHER_INVALID:
+			return "WIFI_REASON_GROUP_CIPHER_INVALID";
+		case WIFI_REASON_PAIRWISE_CIPHER_INVALID:
+			return "WIFI_REASON_PAIRWISE_CIPHER_INVALID";
+		case WIFI_REASON_AKMP_INVALID:
+			return "WIFI_REASON_AKMP_INVALID";
+		case WIFI_REASON_UNSUPP_RSN_IE_VERSION:
+			return "WIFI_REASON_UNSUPP_RSN_IE_VERSION";
+		case WIFI_REASON_INVALID_RSN_IE_CAP:
+			return "WIFI_REASON_INVALID_RSN_IE_CAP";
+		case WIFI_REASON_802_1X_AUTH_FAILED:
+			return "WIFI_REASON_802_1X_AUTH_FAILED";
+		case WIFI_REASON_CIPHER_SUITE_REJECTED:
+			return "WIFI_REASON_CIPHER_SUITE_REJECTED";
+		case WIFI_REASON_BEACON_TIMEOUT:
+			return "WIFI_REASON_BEACON_TIMEOUT";
+		case WIFI_REASON_NO_AP_FOUND:
+			return "WIFI_REASON_NO_AP_FOUND";
+		case WIFI_REASON_AUTH_FAIL:
+			return "WIFI_REASON_AUTH_FAIL";
+		case WIFI_REASON_ASSOC_FAIL:
+			return "WIFI_REASON_ASSOC_FAIL";
+		case WIFI_REASON_HANDSHAKE_TIMEOUT:
+			return "WIFI_REASON_HANDSHAKE_TIMEOUT";
+#endif
+		default:
+			return "Unknown ESP_ERR error";
+	}
+} // wifiErrorToString
+
 
 /**
  * @brief Convert a string to lower case.
@@ -451,15 +536,9 @@ std::string GeneralUtils::toLower(std::string& value) {
 /**
  * @brief Remove white space from a string.
  */
-std::string GeneralUtils::trim(const std::string& str)
-{
-    size_t first = str.find_first_not_of(' ');
-    if (std::string::npos == first)
-    {
-        return str;
-    }
-    size_t last = str.find_last_not_of(' ');
-    return str.substr(first, (last - first + 1));
+std::string GeneralUtils::trim(const std::string& str) {
+	size_t first = str.find_first_not_of(' ');
+	if (std::string::npos == first) return str;
+	size_t last = str.find_last_not_of(' ');
+	return str.substr(first, (last - first + 1));
 } // trim
-
-
diff --git a/sensor/patchedBLE/src/GeneralUtils.h b/sensor/patchedBLE/src/GeneralUtils.h
index 013953d..8eecbd4 100644
--- a/sensor/patchedBLE/src/GeneralUtils.h
+++ b/sensor/patchedBLE/src/GeneralUtils.h
@@ -23,6 +23,7 @@ public:
 	static void        dumpInfo();
 	static bool        endsWith(std::string str, char c);
 	static const char* errorToString(esp_err_t errCode);
+	static const char* wifiErrorToString(uint8_t value);
 	static void        hexDump(const uint8_t* pData, uint32_t length);
 	static std::string ipToString(uint8_t* ip);
 	static std::vector<std::string> split(std::string source, char delimiter);
diff --git a/sensor/patchedBLE/src/HIDKeyboardTypes.h b/sensor/patchedBLE/src/HIDKeyboardTypes.h
index ef48a52..4e221d5 100644
--- a/sensor/patchedBLE/src/HIDKeyboardTypes.h
+++ b/sensor/patchedBLE/src/HIDKeyboardTypes.h
@@ -26,9 +26,9 @@
 
 /* Modifiers */
 enum MODIFIER_KEY {
-    KEY_CTRL = 1,
-    KEY_SHIFT = 2,
-    KEY_ALT = 4,
+	KEY_CTRL = 1,
+	KEY_SHIFT = 2,
+	KEY_ALT = 4,
 };
 
 
@@ -43,37 +43,37 @@ enum MEDIA_KEY {
 };
 
 enum FUNCTION_KEY {
-    KEY_F1 = 128,   /* F1 key */
-    KEY_F2,         /* F2 key */
-    KEY_F3,         /* F3 key */
-    KEY_F4,         /* F4 key */
-    KEY_F5,         /* F5 key */
-    KEY_F6,         /* F6 key */
-    KEY_F7,         /* F7 key */
-    KEY_F8,         /* F8 key */
-    KEY_F9,         /* F9 key */
-    KEY_F10,        /* F10 key */
-    KEY_F11,        /* F11 key */
-    KEY_F12,        /* F12 key */
+	KEY_F1 = 128,   /* F1 key */
+	KEY_F2,		 /* F2 key */
+	KEY_F3,		 /* F3 key */
+	KEY_F4,		 /* F4 key */
+	KEY_F5,		 /* F5 key */
+	KEY_F6,		 /* F6 key */
+	KEY_F7,		 /* F7 key */
+	KEY_F8,		 /* F8 key */
+	KEY_F9,		 /* F9 key */
+	KEY_F10,		/* F10 key */
+	KEY_F11,		/* F11 key */
+	KEY_F12,		/* F12 key */
 
-    KEY_PRINT_SCREEN,   /* Print Screen key */
-    KEY_SCROLL_LOCK,    /* Scroll lock */
-    KEY_CAPS_LOCK,      /* caps lock */
-    KEY_NUM_LOCK,       /* num lock */
-    KEY_INSERT,         /* Insert key */
-    KEY_HOME,           /* Home key */
-    KEY_PAGE_UP,        /* Page Up key */
-    KEY_PAGE_DOWN,      /* Page Down key */
+	KEY_PRINT_SCREEN,   /* Print Screen key */
+	KEY_SCROLL_LOCK,	/* Scroll lock */
+	KEY_CAPS_LOCK,	  /* caps lock */
+	KEY_NUM_LOCK,	   /* num lock */
+	KEY_INSERT,		 /* Insert key */
+	KEY_HOME,		   /* Home key */
+	KEY_PAGE_UP,		/* Page Up key */
+	KEY_PAGE_DOWN,	  /* Page Down key */
 
-    RIGHT_ARROW,        /* Right arrow */
-    LEFT_ARROW,         /* Left arrow */
-    DOWN_ARROW,         /* Down arrow */
-    UP_ARROW,           /* Up arrow */
+	RIGHT_ARROW,		/* Right arrow */
+	LEFT_ARROW,		 /* Left arrow */
+	DOWN_ARROW,		 /* Down arrow */
+	UP_ARROW,		   /* Up arrow */
 };
 
 typedef struct {
-    unsigned char usage;
-    unsigned char modifier;
+	unsigned char usage;
+	unsigned char modifier;
 } KEYMAP;
 
 #ifdef US_KEYBOARD
diff --git a/sensor/patchedBLE/src/HIDTypes.h b/sensor/patchedBLE/src/HIDTypes.h
index 726b84b..64850ef 100644
--- a/sensor/patchedBLE/src/HIDTypes.h
+++ b/sensor/patchedBLE/src/HIDTypes.h
@@ -89,8 +89,8 @@
 #define MAX_HID_REPORT_SIZE (64)
 
 typedef struct {
-    uint32_t length;
-    uint8_t data[MAX_HID_REPORT_SIZE];
+	uint32_t length;
+	uint8_t data[MAX_HID_REPORT_SIZE];
 } HID_REPORT;
 
 #endif