o Overhaul of the bluetooth code.

 - Adding support for reading FLOAT (specified in IEEE 11073-20601) values from a bluetooth device.
 - More shared pointers to help keep track of the object's lifecycle. Makes sure that the connections are released back to Linux, Linux is way to sensitive with crashing applications.
o Adding support for reading the temperature sensors from the SoilMoisture device.

4 files changed, 187 insertions, 73 deletions

diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp
index 3db0bfc..13c81b3 100644
--- a/ble/Bluetooth.cpp
+++ b/ble/Bluetooth.cpp
@@ -201,19 +201,26 @@ BluetoothAdapter::BluetoothAdapter() {
 BluetoothAdapter::~BluetoothAdapter() {
 }
 
+// -----------------------------------------------------------------------
+// Bluetooth System. This is not sub-classed by implementations.
+// -----------------------------------------------------------------------
+
 BluetoothSystem::BluetoothSystem() {
 }
 
 BluetoothSystem::~BluetoothSystem() {
-    shutdown();
+    adapters.clear();
 }
 
-shared_ptr<BluetoothAdapter> getAdapter(int hciDevice) {
-    return getAdapterImpl(hciDevice);
-}
+shared_ptr<BluetoothAdapter> BluetoothSystem::getAdapter(string name) {
+    auto it = adapters.find(name);
+
+    if (it == adapters.end()) {
+        auto adapter = adapters[name] = getAdapterImpl(name);
+        return adapter;
+    }
 
-void shutdown() {
-    shutdownImpl();
+    return it->second;
 }
 
 uuid_t makeUuid(const uuid_t base, uint8_t a, uint8_t b) {
diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h
index 3f4615e..4627443 100644
--- a/ble/BluetoothImpl.h
+++ b/ble/BluetoothImpl.h
@@ -43,11 +43,11 @@ protected:
 
 // Shared classes
 
-class DefaultBluetoothGattCharacteristic : LogSetup, public BluetoothGattCharacteristic {
+class DefaultBluetoothGattCharacteristic : protected LogSetup, public BluetoothGattCharacteristic {
 public:
     DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties,
                                        uint16_t valueHandle)
-            : LogSetup("DefaultBluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid),
+            : LogSetup("BluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid),
               properties(properties), valueHandle(valueHandle) {
     }
 
@@ -117,14 +117,14 @@ public:
         characteristics.push_back(characteristic);
     }
 
-    virtual const o<BluetoothGattCharacteristic &> findCharacteristic(uuid_t uuid) const {
+    virtual const o<const BluetoothGattCharacteristic &> findCharacteristic(uuid_t uuid) const {
         for (auto c: characteristics) {
             if (memcmp(c->getUuid().data, uuid.data, 16) == 0) {
-                return o<BluetoothGattCharacteristic &>(*c);
+                return o<const BluetoothGattCharacteristic &>(*c);
             }
         }
 
-        return o<BluetoothGattCharacteristic &>();
+        return o<const BluetoothGattCharacteristic &>();
     }
 
 protected:
@@ -187,7 +187,7 @@ protected:
 };
 
 template<class A>
-class DefaultBluetoothDevice : public BluetoothDevice {
+class DefaultBluetoothDevice : protected LogSetup, public BluetoothDevice {
 public:
 
     virtual Mac const &getMac() override {
@@ -199,7 +199,7 @@ public:
     }
 
 protected:
-    DefaultBluetoothDevice(A &adapter, Mac &mac) :
+    DefaultBluetoothDevice(A &adapter, Mac &mac) : LogSetup("BluetoothDevice"),
             adapter(adapter), mac(mac) {
     }
 
@@ -233,9 +233,7 @@ protected:
     Mac &mac;
 };
 
-shared_ptr<BluetoothAdapter> getAdapterImpl(int hciDevice);
-
-void shutdownImpl();
+shared_ptr<BluetoothAdapter> getAdapterImpl(string name);
 
 }
 };
diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
index f5e50d8..da61427 100644
--- a/ble/ByteBuffer.cpp
+++ b/ble/ByteBuffer.cpp
@@ -3,6 +3,8 @@
 #include <cassert>
 #include <cstring>
 #include <iomanip>
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 
@@ -48,6 +50,15 @@ ByteBuffer &ByteBuffer::write16le(uint16_t value) {
     return *this;
 }
 
+ByteBuffer &ByteBuffer::write32le(uint32_t value) {
+    checkAndUpdateEnd(4);
+    (*ptr++) = (uint8_t) (value & 0xff);
+    (*ptr++) = (uint8_t) ((value >> 8) & 0xff);
+    (*ptr++) = (uint8_t) ((value >> 16) & 0xff);
+    (*ptr++) = (uint8_t) ((value >> 24) & 0xff);
+    return *this;
+}
+
 ByteBuffer &ByteBuffer::write(const ByteBuffer &value) {
     return write(value.zero, value.getSize());
 }
@@ -62,6 +73,64 @@ ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) {
     return *this;
 }
 
+ByteBuffer &ByteBuffer::writeFLOAT(double d) {
+    uint32_t result;
+
+    if (std::isnan(d)) {
+        result = static_cast<uint32_t>(FLOAT::NaN);
+    } else if (d > FLOAT::max) {
+        result = static_cast<uint32_t>(FLOAT::positive_infinity);
+    } else if (d < FLOAT::min) {
+        result = static_cast<uint32_t>(FLOAT::negative_infinity);
+    } else if (d >= -FLOAT::epsilon && d <= FLOAT::epsilon) {
+        result = 0;
+    } else {
+        double sgn = d > 0 ? 1 : -1;
+        double mantissa = fabs(d);
+        int exponent = 0;
+
+        if (mantissa > FLOAT::mantissa_max) {
+            while (mantissa > FLOAT::mantissa_max) {
+                mantissa /= 10.0;
+                ++exponent;
+
+//                if (exponent > FLOAT::exponent_max) {
+//                    result = sgn ? FLOAT::positive_infinity : FLOAT::negative_infinity;
+//                    return write32le(result);
+//                }
+            }
+        } else if (mantissa < 1) {
+            while (mantissa < 1) {
+                mantissa *= 10;
+                --exponent;
+
+//                if (exponent < FLOAT::exponent_min) {
+//                    result = 0;
+//                    return write32le(result);
+//                }
+            }
+        }
+
+        // scale down if number needs more precision
+        double smantissa = round(mantissa * FLOAT::precision);
+        double rmantissa = round(mantissa) * FLOAT::precision;
+        double mdiff = abs(smantissa - rmantissa);
+        while (mdiff > 0.5 && exponent > FLOAT::exponent_min &&
+               (mantissa * 10) <= FLOAT::mantissa_max) {
+            mantissa *= 10;
+            --exponent;
+            smantissa = round(mantissa * FLOAT::precision);
+            rmantissa = round(mantissa) * FLOAT::precision;
+            mdiff = abs(smantissa - rmantissa);
+        }
+
+        uint32_t int_mantissa = (uint32_t) round(sgn * mantissa);
+        result = (exponent << 24) | (int_mantissa & 0xFFFFFF);
+    }
+
+    return write32le(result);
+}
+
 uint8_t ByteBuffer::get8(size_t index) const {
     assertCanAccessRelative(index);
     return ptr[index];
@@ -73,13 +142,49 @@ uint8_t ByteBuffer::read8() {
 }
 
 uint16_t ByteBuffer::read16le() {
-    assertCanAccessRelative(0);
+    assertCanAccessRelative(1);
     uint16_t value;
     value = *ptr++;
     value |= ((uint16_t) *ptr++) << 8;
     return value;
 }
 
+uint32_t ByteBuffer::read32le() {
+    assertCanAccessRelative(3);
+    uint32_t value;
+    value = *ptr++;
+    value |= ((uint32_t) *ptr++) << 8;
+    value |= ((uint32_t) *ptr++) << 16;
+    value |= ((uint32_t) *ptr++) << 24;
+    return value;
+}
+
+double ByteBuffer::readFLOAT() {
+    uint32_t data = read32le();
+
+    int32_t mantissa = data & 0xFFFFFF;
+    int8_t exponent = (int8_t) (data >> 24);
+    double output = 0;
+
+    if (mantissa >= FLOAT::positive_infinity &&
+        mantissa <= FLOAT::negative_infinity) {
+        if (mantissa == FLOAT::positive_infinity) {
+            output = INFINITY;
+        } else if (mantissa == FLOAT::negative_infinity) {
+            output = -INFINITY;
+        } else {
+            output = NAN;
+        }
+    } else {
+        if (mantissa >= 0x800000) {
+            mantissa = -((0xFFFFFF + 1) - mantissa);
+        }
+        output = mantissa * pow(10.0f, exponent);
+    }
+
+    return output;
+}
+
 void ByteBuffer::copy(uint8_t *bytes, size_t length) const {
     assertCanAccessRelative(length - 1);
 
@@ -103,7 +208,7 @@ void ByteBuffer::checkAndUpdateEnd(size_t newBytes) {
     uint8_t *newEnd = ptr + newBytes;
     if (newEnd >= end) {
         if (newEnd >= &zero[capacity]) {
-            throw ByteBufferException(string("New size is too large! cursor=") + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes));
+            throw ByteBufferException("New size is too large! cursor=" + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes));
         }
         end = newEnd;
     }
@@ -115,7 +220,7 @@ void ByteBuffer::assertCanAccessRelative(size_t diff) const {
 
 void ByteBuffer::assertCanAccessIndex(uint8_t *p) const {
     if (p >= end || p < zero) {
-        throw ByteBufferException(string("Out of bounds! size=") + to_string(getSize()) + ", index=" + to_string(p - zero));
+        throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(p - zero));
     }
 }
 
diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index f662d45..b774644 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -28,15 +28,19 @@ class LinuxBluetoothAdapter;
 
 class LinuxBluetoothManager;
 
-class LinuxBluetoothAdapter : public DefaultBluetoothAdapter {
+class LinuxBluetoothAdapter final : public DefaultBluetoothAdapter {
 public:
     LinuxBluetoothAdapter(int hciDeviceId, Mac &mac);
 
     ~LinuxBluetoothAdapter();
 
-    void runScan(std::function<void(BluetoothDevice &device)>) override;
+    LinuxBluetoothAdapter(const LinuxBluetoothAdapter &) = delete;
 
-    BluetoothDevice &getDevice(Mac &mac) override;
+    LinuxBluetoothAdapter& operator=(const LinuxBluetoothAdapter&) = delete;
+
+    void runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)>) override;
+
+    shared_ptr <BluetoothDevice> getDevice(Mac &mac) override;
 
 private:
     void startScan() override;
@@ -48,25 +52,29 @@ private:
     struct hci_filter hciFilter;
     bool scanning;
 
-    map<Mac, LinuxBluetoothDevice *> devices;
+    map<Mac, shared_ptr<LinuxBluetoothDevice>> devices;
 };
 
-class LinuxBluetoothDevice : public DefaultBluetoothDevice<LinuxBluetoothAdapter> {
+class LinuxBluetoothDevice final : public DefaultBluetoothDevice<LinuxBluetoothAdapter>,
+                                   public std::enable_shared_from_this<LinuxBluetoothDevice> {
 public:
     LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac);
 
-    ~LinuxBluetoothDevice();
+    virtual ~LinuxBluetoothDevice();
+
+    LinuxBluetoothDevice(const LinuxBluetoothDevice &) = delete;
+
+    LinuxBluetoothDevice& operator=(const LinuxBluetoothDevice&) = delete;
 
     shared_ptr<BluetoothGatt> connectGatt() override;
 
 private:
-    weak_ptr<LinuxBluetoothGatt> gatt;
-    int l2cap;
+    LinuxBluetoothGatt *gatt;
 };
 
-class LinuxBluetoothGatt : public DefaultBluetoothGatt<LinuxBluetoothDevice> {
+class LinuxBluetoothGatt final : public DefaultBluetoothGatt<LinuxBluetoothDevice> {
 public:
-    LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap);
+    LinuxBluetoothGatt(LinuxBluetoothDevice &device);
 
     ~LinuxBluetoothGatt();
 
@@ -94,8 +102,6 @@ private:
     ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size);
 
     int l2cap;
-
-    bool connected;
 };
 
 // Utilities
@@ -121,28 +127,29 @@ LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &
 }
 
 LinuxBluetoothDevice::~LinuxBluetoothDevice() {
-//    if (gatt) {
-//        delete gatt;
-//    }
+    LOG_DEBUG("Closing device " << mac.str());
+    if (gatt) {
+        delete gatt;
+    }
 };
 
 shared_ptr<BluetoothGatt> LinuxBluetoothDevice::connectGatt() {
-    if (auto p = gatt.lock()) {
-        return p;
+    // Make sure that we close the old connection and create a new one when the user asks for it.
+    if (gatt) {
+        delete gatt;
     }
-    auto ref = make_shared<LinuxBluetoothGatt>(*this, l2cap);
 
-    gatt = ref;
+    gatt = new LinuxBluetoothGatt(*this);
 
-    return ref;
+    return shared_ptr<LinuxBluetoothGatt>(shared_from_this(), gatt);
 }
 
 // -----------------------------------------------------------------------
 // Gatt
 // -----------------------------------------------------------------------
 
-LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap) :
-        DefaultBluetoothGatt(device), l2cap(l2cap) {
+LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device) :
+        DefaultBluetoothGatt(device) {
     connect();
 }
 
@@ -151,7 +158,7 @@ LinuxBluetoothGatt::~LinuxBluetoothGatt() {
 }
 
 bool LinuxBluetoothGatt::isConnected() const {
-    return connected;
+    return l2cap >= 0;
 }
 
 void LinuxBluetoothGatt::connect() {
@@ -202,10 +209,10 @@ void LinuxBluetoothGatt::connect() {
 }
 
 void LinuxBluetoothGatt::disconnect() {
-    if (!connected) {
+    if (l2cap < 0) {
         return;
     }
-    LOG_DEBUG("mac = " << device.getMac().str());
+    LOG_DEBUG("Disconnecting from mac = " << device.getMac().str());
     close(l2cap);
 }
 
@@ -439,15 +446,14 @@ LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId, Mac &mac) : Defaul
 }
 
 LinuxBluetoothAdapter::~LinuxBluetoothAdapter() {
-    LOG_DEBUG("Stopping scan on device #" << hciDeviceId);
+    LOG_DEBUG("Closing adapter #" << hciDeviceId);
 
     stopScan();
 
     close(hciSocket);
 
-    for (auto &pair : devices) {
-        delete pair.second;
-    }
+    // Hopefully all devices are disposed at this point.
+    devices.clear();
 }
 
 void LinuxBluetoothAdapter::startScan() {
@@ -504,19 +510,19 @@ void LinuxBluetoothAdapter::stopScan() {
     }
 }
 
-BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
-    map<Mac, LinuxBluetoothDevice *>::iterator it = devices.find(mac);
+shared_ptr<BluetoothDevice> LinuxBluetoothAdapter::getDevice(Mac &mac) {
+    auto it = devices.find(mac);
 
     if (it == devices.end()) {
-        LinuxBluetoothDevice *device = new LinuxBluetoothDevice(*this, mac);
+        auto device = make_shared<LinuxBluetoothDevice>(*this, mac);
         devices[mac] = device;
-        return *device;
+        return device;
     }
 
-    return *it->second;
+    return it->second;
 }
 
-void LinuxBluetoothAdapter::runScan(std::function<void(BluetoothDevice &device)> callback) {
+void LinuxBluetoothAdapter::runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)> callback) {
     fd_set rfds;
     FD_ZERO(&rfds);
 
@@ -559,7 +565,7 @@ void LinuxBluetoothAdapter::runScan(std::function<void(BluetoothDevice &device)>
 
             Mac mac = parseMac(advertisingInfo->bdaddr);
 
-            BluetoothDevice &device = getDevice(mac);
+            auto device = getDevice(mac);
 
             callback(device);
         }
@@ -580,30 +586,28 @@ namespace trygvis {
 namespace bluetooth {
 using namespace trygvis::bluetooth::linux;
 
-map<int, shared_ptr<LinuxBluetoothAdapter>> adapters;
-
-shared_ptr<BluetoothAdapter> getAdapterImpl(int hciDevice) {
-    map<int, shared_ptr<LinuxBluetoothAdapter>>::iterator it = adapters.find(hciDevice);
-
-    if (it == adapters.end()) {
-        struct hci_dev_info di;
-        if (hci_devinfo(hciDevice, &di) < 0) {
-            throw BluetoothException("Could not query device info: " + errnoAsString());
-        }
-        auto mac = parseMac(di.bdaddr);
+shared_ptr<BluetoothAdapter> getAdapterImpl(string name) {
+    int hciDevice;
+    try {
+        hciDevice = std::stoi(name);
+    } catch (const std::invalid_argument &) {
+        throw BluetoothException("Bad device name: " + name);
+    } catch (const std::out_of_range &) {
+        throw BluetoothException("Bad device name: " + name);
+    }
 
-        auto adapter = adapters[hciDevice] = make_shared<LinuxBluetoothAdapter>(hciDevice, mac);
-        return adapter;
+    struct hci_dev_info di;
+    if (hci_devinfo(hciDevice, &di) < 0) {
+        throw BluetoothException("Could not query device info: " + errnoAsString());
     }
+    auto mac = parseMac(di.bdaddr);
 
-    return std::static_pointer_cast<BluetoothAdapter, LinuxBluetoothAdapter>(it->second);
+    auto adapter = make_shared<LinuxBluetoothAdapter>(hciDevice, mac);
+
+    return std::static_pointer_cast<BluetoothAdapter, LinuxBluetoothAdapter>(adapter);
 }
 
 void shutdownImpl() {
-    adapters.clear();
-//    for (auto &pair: adapters) {
-//        delete pair.second;
-//    }
 }
 
 }