1 files changed, 90 insertions, 35 deletions

diff --git a/apps/sm-get-value.cpp b/apps/sm-get-value.cpp
index 30ef49a..66d35f1 100644
--- a/apps/sm-get-value.cpp
+++ b/apps/sm-get-value.cpp
@@ -8,10 +8,6 @@
 using namespace std;
 using namespace trygvis::bluetooth;
 
-typedef boost::uuids::uuid uuid_t;
-template<class T>
-using o = boost::optional<T>;
-
 #define BLUETOOTH_UUID_INITIALIZER \
     { \
         0x00, 0x00, 0x00, 0x00, \
@@ -21,25 +17,103 @@ using o = boost::optional<T>;
         0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb \
     };
 
-uuid_t trygvis_io_base_uuid = {
+auto trygvis_io_base_uuid = boost::uuids::uuid {
         0x32, 0xd0, 0x00, 0x00,
         0x03, 0x5d,
         0x59, 0xc5,
         0x70, 0xd3,
         0xbc, 0x8e, 0x4a, 0x1f, 0xd8, 0x3f};
 
-uuid_t soil_moisture_service = makeUuid(trygvis_io_base_uuid, 0x00, 0x10);
-uuid_t soil_moisture_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x11);
+auto soil_moisture_service = makeUuid(trygvis_io_base_uuid, 0x00, 0x10);
+auto soil_moisture_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x11);
 
 ByteBuffer &operator<<(ByteBuffer &bytes, const sm_req &req);
 
+bool loop = true;
+
+ByteBuffer writeAndRead(BluetoothGatt &gatt, const BluetoothGattCharacteristic &c, const ByteBuffer &requestBytes) {
+    uint8_t expectedCode = requestBytes.get8(0);
+
+    gatt.writeValue(c, requestBytes);
+
+    auto responseBytes = gatt.readValue(c);
+
+    if (responseBytes.getSize() < 1) {
+        throw runtime_error("Unexpected number of bytes read: " + to_string(requestBytes.getSize()));
+    }
+
+    uint8_t actualCode = responseBytes.read8();
+    if (actualCode != expectedCode) {
+        throw runtime_error("Unexpected response code: " + to_string(actualCode) + ", expected " + to_string(expectedCode));
+    }
+
+    return responseBytes;
+}
+
+uint8_t getSensorCount(BluetoothGatt &gatt, BluetoothGattCharacteristic &c) {
+    sm_req req = {
+            .code = SM_CMD_GET_SENSOR_COUNT,
+    };
+
+    auto requestBytes = ByteBuffer::alloc(sizeof(struct sm_req));
+    requestBytes << req;
+    requestBytes.setCursor(0);
+
+    return writeAndRead(gatt, c, requestBytes).read8();
+}
+
+uint16_t getValue(BluetoothGatt &gatt, BluetoothGattCharacteristic &c, uint8_t sensor) {
+    sm_req req = {
+            .code = SM_CMD_GET_VALUE,
+    };
+    req.get_value = {
+            .sensor = sensor,
+    };
+
+    auto requestBytes = ByteBuffer::alloc(sizeof(struct sm_req));
+    requestBytes << req;
+    requestBytes.setCursor(0);
+
+    return writeAndRead(gatt, c, requestBytes).read16le();
+}
+
+void withConnection(BluetoothGatt &gatt) {
+    gatt.discoverServices();
+
+    auto service = gatt.findService(soil_moisture_service);
+
+    if (!service) {
+        throw runtime_error("The device is missing the soil moisture service");
+    }
+
+    auto c = service->findCharacteristic(soil_moisture_characteristic);
+
+    if (!c) {
+        throw runtime_error("The device is missing the soil moisture characteristic");
+    }
+
+    int sensorCount = getSensorCount(gatt, *c);
+
+    while (loop) {
+        for (uint8_t i = 0; i < sensorCount; i++) {
+            uint16_t value = getValue(gatt, *c, i);
+
+            cout << "sensor=" << to_string(i) << ", value=" << (int) value << endl;
+        }
+
+        sleep(1);
+    }
+}
+
 int main(int argc, char *argv[]) {
     if (argc != 2) {
         cerr << "usage: " << argv[0] << " [mac]" << endl;
         return EXIT_FAILURE;
     }
 
+    __attribute__((unused))
     BluetoothSystem bluetoothSystem;
+
     try {
         Mac mac = Mac::parseMac(argv[1]);
 
@@ -47,37 +121,18 @@ int main(int argc, char *argv[]) {
 
         auto &device = adapter.getDevice(mac);
 
-        cout << "Connecting to device: " << device.getMac().str() << endl;
-
-        auto &gatt = device.connectGatt();
+        while (loop) {
+            cout << "Connecting to device: " << device.getMac().str() << endl;
 
-        gatt.discoverServices();
-
-        auto service = gatt.findService(soil_moisture_service);
-
-        if (!service) {
-            cout << "The device is missing the soil moisture service" << endl;
-            return EXIT_FAILURE;
+            auto &gatt = device.connectGatt();
+            try {
+                withConnection(gatt);
+            } catch (runtime_error &e) {
+                cout << "exception: " << e.what() << endl;
+            }
+            gatt.disconnect();
         }
 
-        auto c = service->findCharacteristic(soil_moisture_characteristic);
-
-        if (!c) {
-            cout << "The device is missing the soil moisture characteristic" << endl;
-            return EXIT_FAILURE;
-        }
-
-        sm_req req = {
-                .code = SM_CMD_GET_SENSOR_COUNT,
-        };
-
-        auto requestBytes = ByteBuffer::alloc(sizeof(struct sm_req));
-        requestBytes << req;
-        gatt.writeValue(*c, requestBytes);
-
-        auto responseBytes = gatt.readValue(*c);
-
-        gatt.disconnect();
         return EXIT_SUCCESS;
     } catch (std::runtime_error ex) {
         W << "std::runtime_error: " << ex.what();