From f9ba3b3bc19dbf7c043ce97eba64bd2946ff6352 Mon Sep 17 00:00:00 2001
From: Paolo Calao <paolo.calao@gmail.com>
Date: Tue, 11 Aug 2020 12:18:33 +0200
Subject: [PATCH] [WIP] Implement and test multiple concurrent connections

---
 src/local/BLELocalDevice.cpp | 35 ++++++++++++++++++++
 src/local/BLELocalDevice.h   |  5 +++
 src/utility/ATT.cpp          | 63 ++++++++++++++++++++++++++++++++++--
 src/utility/ATT.h            |  5 +++
 4 files changed, 106 insertions(+), 2 deletions(-)

diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp
index 57477e23..e54215f8 100644
--- a/src/local/BLELocalDevice.cpp
+++ b/src/local/BLELocalDevice.cpp
@@ -265,6 +265,41 @@ BLEDevice BLELocalDevice::central()
   return ATT.central();
 }
 
+BLEDevice BLELocalDevice::central(int index)
+{
+  HCI.poll();
+
+  return ATT.central(index);
+}
+
+int BLELocalDevice::centralCount()
+{
+  HCI.poll();
+
+  return ATT.centralCount();
+}
+
+BLEDevice BLELocalDevice::peripheral() 
+{
+  HCI.poll();
+
+  return ATT.peripheral();
+}
+
+BLEDevice BLELocalDevice::peripheral(int index) 
+{
+  HCI.poll();
+
+  return ATT.peripheral(index);
+}
+
+int BLELocalDevice::peripheralCount()
+{
+  HCI.poll();
+
+  return ATT.peripheralCount();
+}
+
 BLEDevice BLELocalDevice::available()
 {
   HCI.poll();
diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h
index 20837c14..36654332 100644
--- a/src/local/BLELocalDevice.h
+++ b/src/local/BLELocalDevice.h
@@ -62,6 +62,11 @@ class BLELocalDevice {
   void stopScan();
 
   BLEDevice central();
+  BLEDevice central(int index); 
+  int centralCount();
+  BLEDevice peripheral(); 
+  BLEDevice peripheral(int index); 
+  int peripheralCount();
   BLEDevice available();
 
   void setEventHandler(BLEDeviceEvent event, BLEDeviceEventHandler eventHandler);
diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp
index 8c8c20c0..83fb7e65 100644
--- a/src/utility/ATT.cpp
+++ b/src/utility/ATT.cpp
@@ -497,19 +497,78 @@ bool ATTClass::disconnect()
   return (numDisconnects > 0);
 }
 
-BLEDevice ATTClass::central()
+BLEDevice ATTClass::central() 
 {
+  return central(0);
+}
+
+BLEDevice ATTClass::central(int index) 
+{
+  int currentIndex = 0;
   for (int i = 0; i < ATT_MAX_PEERS; i++) {
     if (_peers[i].connectionHandle == 0xffff || _peers[i].role != 0x01) {
       continue;
     }
 
-    return BLEDevice(_peers[i].addressType, _peers[i].address);
+    if (currentIndex == index) {
+      return BLEDevice(_peers[i].addressType, _peers[i].address);
+    }
+    currentIndex++;
   }
 
   return BLEDevice();
 }
 
+int ATTClass::centralCount()
+{
+  int count = 0;
+  for (int i = 0; i < ATT_MAX_PEERS; i++) {
+    if (_peers[i].connectionHandle == 0xffff || _peers[i].role != 0x01) {
+      continue;
+    }
+
+    count++;
+  }
+
+  return count;
+}
+
+BLEDevice ATTClass::peripheral()
+{
+  return peripheral(0);
+}
+
+BLEDevice ATTClass::peripheral(int index)
+{
+  int currentIndex = 0;
+  for (int i = 0; i < ATT_MAX_PEERS; i++) {
+    if (_peers[i].connectionHandle == 0xffff || _peers[i].role != 0x00) {
+      continue;
+    }
+
+    if (currentIndex == index) {
+      return BLEDevice(_peers[i].addressType, _peers[i].address);
+    }
+    currentIndex++;
+  }
+
+  return BLEDevice();
+}
+
+int ATTClass::peripheralCount()
+{
+  int count = 0;
+  for (int i = 0; i < ATT_MAX_PEERS; i++) {
+    if (_peers[i].connectionHandle == 0xffff || _peers[i].role != 0x00) {
+      continue;
+    }
+
+    count++;
+  }
+
+  return count;
+}
+
 bool ATTClass::handleNotify(uint16_t handle, const uint8_t* value, int length)
 {
   int numNotifications = 0;
diff --git a/src/utility/ATT.h b/src/utility/ATT.h
index 6a1611aa..22d61fcd 100644
--- a/src/utility/ATT.h
+++ b/src/utility/ATT.h
@@ -67,6 +67,11 @@ class ATTClass {
   bool disconnect();
 
   BLEDevice central();
+  BLEDevice central(int index); 
+  int centralCount();
+  BLEDevice peripheral(); 
+  BLEDevice peripheral(int index); 
+  int peripheralCount();
 
   bool handleNotify(uint16_t handle, const uint8_t* value, int length);
   bool handleInd(uint16_t handle, const uint8_t* value, int length);