1 files changed, 108 insertions, 3 deletions

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));
     }
 }