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.

12 files changed, 389 insertions, 110 deletions

diff --git a/apps/SoilMoisture.cpp b/apps/SoilMoisture.cpp
index 1d6888c..88f881a 100644
--- a/apps/SoilMoisture.cpp
+++ b/apps/SoilMoisture.cpp
@@ -1,4 +1,5 @@
 #include "SoilMoisture.h"
+#include <bitset>
 
 namespace trygvis {
 namespace sensor {
@@ -9,22 +10,29 @@ namespace sensor {
 auto trygvis_io_base_uuid =
     boost::uuids::uuid{0x32, 0xd0, 0x00, 0x00, 0x03, 0x5d, 0x59, 0xc5, 0x70, 0xd3, 0xbc, 0x8e, 0x4a, 0x1f, 0xd8, 0x3f};
 
+auto bluetooth_base_uuid =
+    boost::uuids::uuid{0x00, 0x00, 0x18, 0x0f, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb};
+
 const boost::uuids::uuid soil_moisture_service = makeUuid(trygvis_io_base_uuid, 0x00, 0x10);
 const boost::uuids::uuid soil_moisture_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x11);
+const boost::uuids::uuid temperature_characteristic = makeUuid(bluetooth_base_uuid, 0x2a, 0x1e);
 
 using namespace trygvis::bluetooth;
 
+static
 ByteBuffer createGetSensorCount() {
     return ByteBuffer::alloc(100) //
         .write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_GET_SENSOR_COUNT));
 }
 
+static
 ByteBuffer createGetValue(uint8_t sensor) {
     return ByteBuffer::alloc(100) //
         .write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_GET_VALUE)) //
         .write16le(sensor);
 }
 
+static
 ByteBuffer createSetWarningValue(uint8_t sensor, uint16_t warning_value) {
     return ByteBuffer::alloc(100)
         .write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_SET_WARNING_VALUE))
@@ -32,6 +40,7 @@ ByteBuffer createSetWarningValue(uint8_t sensor, uint16_t warning_value) {
         .write16le(warning_value);
 }
 
+static
 ByteBuffer createSetSensorName(uint8_t sensor, string name) {
     return ByteBuffer::alloc(100)
         .write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_SET_SENSOR_NAME))
@@ -39,10 +48,12 @@ ByteBuffer createSetSensorName(uint8_t sensor, string name) {
         .write(reinterpret_cast<const uint8_t *>(name.c_str()), name.length());
 }
 
+static
 ByteBuffer createGetSensorName(uint8_t sensor) {
     return ByteBuffer::alloc(100).write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_GET_SENSOR_NAME)).write8(sensor);
 }
 
+static
 ByteBuffer createSetUpdateInterval(uint8_t sensor, uint8_t interval_in_seconds) {
     return ByteBuffer::alloc(100)
         .write8(static_cast<uint8_t>(sm_cmd_code::SM_CMD_SET_UPDATE_INTERVAL))
@@ -65,13 +76,18 @@ SoilMoisture SoilMoisture::create(shared_ptr<BluetoothGatt> gatt) {
         throw runtime_error("The device is missing the soil moisture characteristic");
     }
 
-    return SoilMoisture(gatt, *service, *c);
+    auto temperature = service->findCharacteristic(temperature_characteristic);
+
+    return SoilMoisture(gatt, *service, c.get(), temperature);
 }
 
-SoilMoisture::SoilMoisture(shared_ptr<BluetoothGatt> gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c)
-    : gatt(std::move(gatt)), s(s), c(c) {}
+SoilMoisture::SoilMoisture(const shared_ptr<BluetoothGatt> &gatt, BluetoothGattService &s,
+                           const BluetoothGattCharacteristic &soilMoistureCharacteristic,
+                           const o<const BluetoothGattCharacteristic &> temperatureCharacteristic)
+    : gatt(gatt), s(s), soilMoistureCharacteristic(soilMoistureCharacteristic),
+      temperatureCharacteristic(temperatureCharacteristic) {}
 
-ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) {
+ByteBuffer SoilMoisture::writeAndRead(const BluetoothGattCharacteristic &c, ByteBuffer &requestBytes) {
     requestBytes.setCursor(0);
 
     uint8_t expectedCode = requestBytes.get8(0);
@@ -81,7 +97,7 @@ ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) {
     auto responseBytes = gatt->readValue(c);
 
     if (responseBytes.getSize() < 1) {
-        throw runtime_error("Unexpected number of bytes read: " + to_string(requestBytes.getSize()));
+        throw runtime_error("Unexpected number of bytes read: " + to_string(responseBytes.getSize()));
     }
 
     uint8_t actualCode = responseBytes.read8();
@@ -95,13 +111,13 @@ ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) {
 
 uint8_t SoilMoisture::getSensorCount() {
     auto buffer = createGetSensorCount();
-    return writeAndRead(buffer).read8();
+    return writeAndRead(soilMoistureCharacteristic, buffer).read8();
 }
 
 uint16_t SoilMoisture::getValue(uint8_t sensor) {
     auto req = createGetValue(sensor);
 
-    auto buffer = writeAndRead(req);
+    auto buffer = writeAndRead(soilMoistureCharacteristic, req);
     buffer.read8(); // sensor index
     return buffer.read16le();
 }
@@ -109,12 +125,44 @@ uint16_t SoilMoisture::getValue(uint8_t sensor) {
 string SoilMoisture::getName(uint8_t sensor) {
     auto req = createGetSensorName(sensor);
 
-    auto buffer = writeAndRead(req);
+    auto buffer = writeAndRead(soilMoistureCharacteristic, req);
     size_t bytesLeft = buffer.getBytesLeft();
     uint8_t bytes[bytesLeft];
     buffer.copy(bytes, bytesLeft);
 
     return string((char *) bytes, (unsigned long) bytesLeft);
 }
+
+bool SoilMoisture::hasTemperatureSensor() {
+    return temperatureCharacteristic.is_initialized();
+}
+
+/**
+ * https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.intermediate_temperature.xml
+ */
+o<double> SoilMoisture::readTemperature() {
+    if (!temperatureCharacteristic) {
+        return o<double>();
+    }
+
+    auto &c = temperatureCharacteristic.get();
+
+    auto responseBytes = gatt->readValue(c);
+
+    if (responseBytes.getSize() < 2) {
+        throw runtime_error("Unexpected number of bytes read: " + to_string(responseBytes.getSize()));
+    }
+
+    bitset<8> b0 = responseBytes.read8();
+
+    bool is_fahrenheit = b0.test(0);
+    bool has_timestamp = b0.test(1);
+    bool has_type = b0.test(2);
+
+    double temperature = responseBytes.readFLOAT();
+
+    return o<double>(temperature);
+}
+
 }
 }
diff --git a/apps/SoilMoisture.h b/apps/SoilMoisture.h
index b5a8749..99b698d 100644
--- a/apps/SoilMoisture.h
+++ b/apps/SoilMoisture.h
@@ -7,6 +7,8 @@ namespace trygvis {
 namespace sensor {
 
 using namespace trygvis::bluetooth;
+template <typename T>
+using o = boost::optional<T>;
 
 enum class sm_cmd_code : uint8_t {
     SM_CMD_GET_SENSOR_COUNT = 1,
@@ -32,14 +34,21 @@ public:
 
     string getName(uint8_t sensor);
 
+    bool hasTemperatureSensor();
+
+    o<double> readTemperature();
+
 private:
-    SoilMoisture(shared_ptr<BluetoothGatt> gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c);
+    SoilMoisture(const shared_ptr<BluetoothGatt> &gatt, BluetoothGattService &s,
+                 const BluetoothGattCharacteristic &soilMoistureCharacteristic,
+                 const o<const BluetoothGattCharacteristic &> temperatureCharacteristic);
 
-    ByteBuffer writeAndRead(ByteBuffer &requestBytes);
+    ByteBuffer writeAndRead(const BluetoothGattCharacteristic &c, ByteBuffer &requestBytes);
 
     shared_ptr<BluetoothGatt> gatt;
     BluetoothGattService &s;
-    BluetoothGattCharacteristic &c;
+    const BluetoothGattCharacteristic &soilMoistureCharacteristic;
+    const o<const BluetoothGattCharacteristic &> temperatureCharacteristic;
 };
 }
 }
diff --git a/apps/ble-inspect-device.cpp b/apps/ble-inspect-device.cpp
index 6330512..2140e03 100644
--- a/apps/ble-inspect-device.cpp
+++ b/apps/ble-inspect-device.cpp
@@ -22,10 +22,10 @@ public:
         options("device", po::value<string>()->required(), "The MAC of the device to inspect");
     }
 
-    void scan_callback(BluetoothDevice &device) {
-        cout << "Inspecting device: " << device.getMac().str() << endl;
+    void scan_callback(const shared_ptr<BluetoothDevice> &device) {
+        cout << "Inspecting device: " << device->getMac().str() << endl;
 
-        auto gatt = device.connectGatt();
+        auto gatt = device->connectGatt();
 
         gatt->discoverServices();
 
@@ -53,9 +53,9 @@ public:
         try {
             Mac mac = Mac::parseMac(mac_str);
 
-            shared_ptr<BluetoothAdapter> adapter = getAdapter(0);
+            auto adapter = bluetoothSystem.getAdapter("0");
 
-            BluetoothDevice &device = adapter->getDevice(mac);
+            auto device = adapter->getDevice(mac);
 
             scan_callback(device);
 
diff --git a/apps/ble-scan.cpp b/apps/ble-scan.cpp
index 839a9fa..158af14 100644
--- a/apps/ble-scan.cpp
+++ b/apps/ble-scan.cpp
@@ -26,8 +26,11 @@ public:
 
     ~ble_scan() = default;
 
+    string adapter_name;
+
     void add_options(po::options_description_easy_init &options) override {
-        options("adapter", po::value<int>()->default_value(0), "Which adapter to use.");
+        auto adapter_value = po::value<>(&adapter_name)->default_value("0");
+        options("adapter", adapter_value, "Which adapter to use.");
     }
 
     int main(app_execution &execution) override {
@@ -45,16 +48,14 @@ public:
         sigaction(SIGINT, &sigIntHandler, NULL);
 
         try {
-            auto adapter_index = execution.vm["adapter"].as<int>();
-
-            adapter = getAdapter(adapter_index);
+            adapter = bluetoothSystem.getAdapter(adapter_name);
 
             set<Mac> seen_devices;
 
-            cout << "Scanning with adapter #" << adapter_index << ", mac=" << adapter->getMac().str() << endl;
+            cout << "Scanning with adapter " << adapter_name << ", mac=" << adapter->getMac().str() << endl;
 
-            adapter->runScan([&](BluetoothDevice &device) {
-                auto mac = device.getMac();
+            adapter->runScan([&](auto device) {
+                auto mac = device->getMac();
 
                 cout << "Found: " << mac.str() << endl;
 
diff --git a/apps/sm-get-value.cpp b/apps/sm-get-value.cpp
index 92ef5f9..5d66320 100644
--- a/apps/sm-get-value.cpp
+++ b/apps/sm-get-value.cpp
@@ -50,7 +50,7 @@ public:
     }
 
     int main(app_execution &execution) override {
-        __attribute__((unused)) BluetoothSystem bluetoothSystem;
+        BluetoothSystem bluetoothSystem;
 
         auto desc = execution.desc;
         auto vm = execution.vm;
@@ -66,18 +66,20 @@ public:
 
             Mac mac = Mac::parseMac(mac_string);
 
-            auto adapter = getAdapter(0);
+            auto adapter = bluetoothSystem.getAdapter("0");
 
-            auto &device = adapter->getDevice(mac);
+            auto device = adapter->getDevice(mac);
 
             loop = sleepTime > 0;
 
             do {
                 try {
-                    LOG4CPLUS_INFO(execution.logger, "Connecting to device: " << device.getMac().str());
-                    auto gatt = device.connectGatt();
+                    LOG4CPLUS_INFO(execution.logger, "Connecting to device: " << device->getMac().str());
+                    auto gatt = device->connectGatt();
 
                     withConnection(format, gatt);
+                } catch (BluetoothException &e) {
+                    LOG4CPLUS_ERROR(execution.logger, "Bluetooth error: " << e.what());
                 } catch (runtime_error &e) {
                     LOG4CPLUS_ERROR(execution.logger, "Exception: " << e.what());
                 }
@@ -112,6 +114,11 @@ public:
                           .set(device_key, mac)
                           .set(timestamp_key, std::to_string(timestamp));
 
+        auto tempO = soilMoisture.readTemperature();
+        if (tempO) {
+            sample.set(dict.indexOf("temperature"), std::to_string(tempO.get()));
+        }
+
         int i = 0;
         for (auto s : sensors) {
             auto sensor = s.first;
@@ -122,7 +129,9 @@ public:
             auto sensor_key = dict.indexOf("sensor" + std::to_string(i));
             auto sensor_name_key = dict.indexOf("sensor_name" + std::to_string(i));
 
-            sample.set(sensor_key, std::to_string(value)).set(sensor_name_key, name);
+            sample
+                .set(sensor_key, std::to_string(value))
+                .set(sensor_name_key, name);
 
             i++;
         }
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;
-//    }
 }
 
 }
diff --git a/include/ble/Bluetooth.h b/include/ble/Bluetooth.h
index d238377..8da602f 100644
--- a/include/ble/Bluetooth.h
+++ b/include/ble/Bluetooth.h
@@ -7,6 +7,7 @@
 #include <stdexcept>
 #include <vector>
 #include <cstdint>
+#include <map>
 
 #include "ByteBuffer.h"
 
@@ -115,7 +116,7 @@ public:
 
     virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) = 0;
 
-    virtual const boost::optional<BluetoothGattCharacteristic &> findCharacteristic(boost::uuids::uuid uuid) const = 0;
+    virtual const boost::optional<const BluetoothGattCharacteristic &> findCharacteristic(boost::uuids::uuid uuid) const = 0;
 };
 
 class BluetoothGatt {
@@ -165,9 +166,9 @@ public:
 
     virtual void stopScan() = 0;
 
-    virtual void runScan(std::function<void(BluetoothDevice &device)>) = 0;
+    virtual void runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)>) = 0;
 
-    virtual BluetoothDevice &getDevice(Mac &mac) = 0;
+    virtual shared_ptr <BluetoothDevice> getDevice(Mac &mac) = 0;
 
 protected:
     BluetoothAdapter();
@@ -185,6 +186,11 @@ public:
     BluetoothSystem();
 
     ~BluetoothSystem();
+
+    shared_ptr<BluetoothAdapter> getAdapter(string name);
+
+private:
+    map<string, shared_ptr<BluetoothAdapter>> adapters;
 };
 
 enum AttPduType {
@@ -227,7 +233,7 @@ public:
     static void makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes);
 
 private:
-    static void checkType(ByteBuffer &bytes, AttPduType type);
+//    static void checkType(ByteBuffer &bytes, AttPduType type);
 
     static vector<AttributeData> parse(ByteBuffer &bytes, AttPduType type);
 
@@ -247,10 +253,6 @@ private:
     AttributeData(uint16_t handle, ByteBuffer value);
 };
 
-shared_ptr<BluetoothAdapter> getAdapter(int hciDevice);
-
-void shutdown();
-
 boost::uuids::uuid makeUuid(const boost::uuids::uuid base, uint8_t a, uint8_t b);
 
 }
diff --git a/include/ble/ByteBuffer.h b/include/ble/ByteBuffer.h
index d977c67..1032187 100644
--- a/include/ble/ByteBuffer.h
+++ b/include/ble/ByteBuffer.h
@@ -7,6 +7,29 @@
 #include <iosfwd>
 #include <memory>
 #include <stdexcept>
+#include <math.h>
+
+namespace FLOAT {
+static const uint32_t positive_infinity = 0x007FFFFE;
+static const uint32_t negative_infinity = 0x00800002;
+static const uint32_t NaN = 0x007FFFFF;
+static const uint32_t NRes = 0x00800000;
+static const uint32_t FLAOT_reserved_value = 0x00800001;
+
+// (2^23 - 3) * 10^127
+static const double max = 8.388604999999999e+133;
+static const double min = -max;
+
+static const double mantissa_max = 0x007FFFFD;
+static const double exponent_max = 127;
+static const double exponent_min = -128;
+
+// 10^-128
+static const double epsilon = 1e-128;
+
+// 10^upper(23 * log(2) / log(10))
+static const double precision = 10000000;
+}
 
 class ByteBufferException : public std::runtime_error {
 public:
@@ -53,6 +76,8 @@ public:
 
     ByteBuffer &write16le(uint16_t value);
 
+    ByteBuffer &write32le(uint32_t value);
+
     /**
     * Appends the entire buffer. Make a view if you want to write a part of it.
     */
@@ -60,12 +85,21 @@ public:
 
     ByteBuffer &write(const uint8_t *bytes, size_t len);
 
+    ByteBuffer &writeFLOAT(double d);
+
     uint8_t get8(size_t index) const;
 
     uint8_t read8();
 
     uint16_t read16le();
 
+    uint32_t read32le();
+
+    /**
+     * IEEE-11073 32-bit FLOAT
+     */
+    double readFLOAT();
+
     void copy(uint8_t *bytes, size_t length) const;
 
     /**
diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp
index 5aad376..8659f64 100644
--- a/test/ByteBufferTest.cpp
+++ b/test/ByteBufferTest.cpp
@@ -1,4 +1,5 @@
 #include "ble/ByteBuffer.h"
+#include <iomanip>
 
 #define BOOST_TEST_MODULE "ByteBuffer"
 
@@ -89,3 +90,64 @@ BOOST_AUTO_TEST_CASE(view) {
 
     BOOST_CHECK_EQUAL(view1.read8(), 3);
 }
+
+BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_1_0d) {
+    Bytes b(1000);
+    ByteBuffer buffer(b.bytes, b.capacity);
+
+    buffer.writeFLOAT(1.0);
+    buffer.setCursor(0);
+    BOOST_CHECK_EQUAL(buffer.readFLOAT(), 1.0);
+}
+
+BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_nan) {
+    Bytes b(1000);
+    ByteBuffer buffer(b.bytes, b.capacity);
+
+    BOOST_CHECK_EQUAL(std::isnan(stof("NaN")), true);
+    buffer.writeFLOAT(stof("NaN"));
+    buffer.setCursor(0);
+    BOOST_CHECK_EQUAL(std::isnan(buffer.readFLOAT()), true);
+}
+
+void c(double input, uint32_t hx, double sigma = 0.00001) {
+    Bytes b(1000);
+    ByteBuffer buffer(b.bytes, b.capacity);
+    buffer.writeFLOAT(input);
+    buffer.setCursor(0);
+    double output = buffer.readFLOAT();
+
+    stringstream str;
+    auto actual_hex = ((uint32_t *) b.bytes.get())[0];
+    str << "input=" << setw(20) << setprecision(10) << input <<
+        ", output=" << setw(20) << setprecision(10) << output <<
+        ", diff=" << setw(20) << setprecision(10) << fabs(input - output) <<
+        ", expected=" << setw(8) << setfill('0') << hex << hx <<
+        ", actual=" << setw(8) << setfill('0') << hex << actual_hex;
+
+    if (std::isnan(input) || std::isnan(output)) {
+        BOOST_CHECK_EQUAL(std::isnan(input), std::isnan(output));
+    } else if (input >= FLOAT::max) {
+        BOOST_CHECK_EQUAL(INFINITY, output);
+    } else if (input <= FLOAT::min) {
+        BOOST_CHECK_EQUAL(-INFINITY, output);
+    } else {
+        BOOST_CHECK_MESSAGE(fabs(input - output) < sigma, str.str());
+    }
+}
+
+BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_lots) {
+    c(36.4, 0xFF00016C);
+    c(1, 0xFF000270);
+    c(62.4, 0xFF000270);
+    c(25.8, 0xFF000102);
+    c(-0.2, 0xFFFFFFFE);
+    c(0.03, 0xFE000003);
+    c(15000000.0, 0x0116e360);
+    c(15000000.1, 0x0116e360, 0.11);
+    c(1500000.1, 0x0016e360, 0.11);
+    c(1499999.99, 0x0016e360, 0.02);
+    c(NAN, 0x007FFFFF);
+    c(1e300, 0x007FFFFE);
+    c(-1e300, 0x00800002);
+}