Control Core fixes

CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+if(ESP_PLATFORM)
+    idf_component_register(
+        SRC_DIRS "."
+        INCLUDE_DIRS "."
+    )
+else() 
+    project(ControlCore)
+
+    set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+    add_compile_definitions(GTEST)
+    include(FetchContent)
+    FetchContent_Declare(
+        googletest
+        DOWNLOAD_EXTRACT_TIMESTAMP ON
+        URL https://github.com/google/googletest/archive/refs/heads/main.zip
+    )
+
+    # For Windows: Prevent overriding the parent project's compiler/linker settings
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+    FetchContent_MakeAvailable(googletest)
+
+    include_directories(.)
+    add_library(ControlCore STATIC 
+        "CoreThing.cpp"
+        "LowLevelMessages.cpp"
+        "Messages.cpp" 
+        "Participant.cpp" 
+        "float16.cpp"
+    )
+
+    enable_testing()
+
+    file(GLOB_RECURSE test_srcs test/*_test.cc)
+    add_executable(
+        ControlCoreTest
+        ${test_srcs}
+    )
+    target_link_libraries(
+        ControlCoreTest
+        gtest_main
+        ControlCore
+    )
+
+    if(MSVC)
+    target_compile_options(ControlCoreTest PRIVATE /W4 /WX)
+  else()
+    target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
+  endif()
+  
+
+    include(GoogleTest)
+    gtest_discover_tests(ControlCoreTest)
+endif()
+

CoreThing.cpp

@@ -1,18 +1,25 @@
 #include "CoreThing.h"
 
-CoreThing::CoreThing(
+#include <iostream>
-    // Participant *client,
+
-    unsigned char networkId, unsigned char thingId, unsigned char thingType) {
+CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
-  // this->client = client;
-  this->id = thingId;
   this->type = thingType;
   this->networkId = networkId;
   this->Init();
-  CoreThing::Add(this);
+
+  int thingId = CoreThing::Add(this);
+  if (thingId < 0) {
+    std::cout << "ERROR: Thing store is full\n";
+    this->id = 0; // what to do when we cannot store any more things?
+  } else
+    this->id = thingId;
 }
 
 void CoreThing::Init() {}
 
+void CoreThing::SetName(const char *name) { this->name = name; }
+
+// All things
 CoreThing *CoreThing::allThings[256] = {nullptr};
 
 CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
@@ -26,13 +33,14 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
   return nullptr;
 }
 
-bool CoreThing::Add(CoreThing *newThing) {
+int CoreThing::Add(CoreThing *newThing) {
   for (unsigned char ix = 0; ix < 256; ix++) {
     CoreThing *thing = allThings[ix];
     if (thing == nullptr) {
       allThings[ix] = newThing;
-      return true;
+
+      return ix;
     }
   }
-  return false;
+  return -1;
 }

CoreThing.h

@@ -1,32 +1,57 @@
 #pragma once
 
-namespace Passer::Control {
+namespace Passer {
+namespace Control {
 
 class CoreThing {
 public:
   // Participant *client;
   unsigned char networkId;
+  /// @char The id of the thing
   unsigned char id;
   // CoreThing *parent;
+  /// @brief The type of Thing
   unsigned char type;
-  const char *name;
+  const char *name = nullptr;
-  const char *modelUrl;
+  const char *modelUrl = nullptr;
   // protected Sensor sensor;
 
-  static CoreThing *allThings[];
+  /// @brief Basic Thing types
+  enum class Type {
+    Undetermined,
+    // Sensor,
+    Switch,
+    DistanceSensor,
+    DirectionalSensor,
+    TemperatureSensor,
+    // Motor,
+    ControlledMotor,
+    UncontrolledMotor,
+    Servo,
+    // Other
+    Humanoid,
+    ExternalSensor,
+  };
 
 public:
-  CoreThing(
+  CoreThing(unsigned char networkId = 0,
-      // Participant *client,
+            unsigned char thingType = (unsigned char)Type::Undetermined);
-      unsigned char networkId, unsigned char thingId,
+
-      unsigned char thingType = 0);
+  void SetName(const char *name);
+
+  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
+
+  // All things
+private:
+  static CoreThing *allThings[];
 
   static CoreThing *Get(unsigned char networkId, unsigned char thingId);
-  static bool Add(CoreThing *thing);
+  static int Add(CoreThing *thing);
 
 protected:
   virtual void Init();
 };
 
-} // namespace Passer::Control
+} // namespace Control
+} // namespace Passer
 using namespace Passer::Control;

LowLevelMessages.cpp

@@ -1,6 +1,6 @@
 #include "LowLevelMessages.h"
 
-#include "../float16/float16.h"
+#include "float16.h"
 
 void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
                                   const float angle) {
@@ -32,7 +32,7 @@ float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
   f.setBinary(value);
 
   *startIndex = ix;
-  return f.toDouble();
+  return (float)f.toFloat();
 }
 
 void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,

LowLevelMessages.h

@@ -1,7 +1,8 @@
 #include "../LinearAlgebra/Spherical.h"
 #include "../LinearAlgebra/SwingTwist.h"
 
-namespace Passer::Control {
+namespace Passer {
+namespace Control {
 
 class LowLevelMessages {
 public:
@@ -26,5 +27,6 @@ public:
                                     unsigned char *ix);
 };
 
-} // namespace Passer::Control
+} // namespace Control
+} // namespace Passer
 using namespace Passer::Control;

Messages.cpp

@@ -237,52 +237,33 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
 #pragma region CustomMsg
 
 CustomMsg::CustomMsg(unsigned char *buffer) {
-  unsigned char ix;
+  unsigned char ix = 1;
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
-  this->dataSize = buffer[ix++];
   this->data =
       buffer + ix; // This is only valid because the code ensures the the msg
                    // lifetime is shorter than the buffer lifetime...
 }
 
-CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId,
+CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) {
-                     unsigned char *data, unsigned char dataSize) {
   this->networkId = networkId;
-  this->thingId = thingId;
+  this->thingId = thing->id;
-  this->dataSize = dataSize;
+  this->thing = thing;
-  this->data = data;
 }
 
+#include <iostream>
 unsigned char CustomMsg::Serialize(unsigned char *buffer) {
-  unsigned char ix = 0;
+  unsigned char ix = this->length;
-  buffer[ix++] = this->id;
+  this->thing->SendBytes(buffer, &ix);
-  buffer[ix++] = this->networkId;
+  if (ix <= this->length) // in this case, no data is actually sent
-  buffer[ix++] = this->thingId;
+    return 0;
-  for (int dataIx = 0; dataIx < this->dataSize; dataIx++)
+
-    buffer[ix++] = this->data[dataIx];
+  buffer[0] = this->id;
+  buffer[1] = this->networkId;
+  buffer[2] = this->thingId;
   return ix;
 }
 
-// void CustomMsg::Deserialize(unsigned char *buffer) {
-//   unsigned char ix;
-//   this->networkId = buffer[ix++];
-//   this->thingId = buffer[ix++];
-//   this->dataSize = buffer[ix++];
-//   this->data = buffer + ix; // challenging: point directly into the buffer!
-
-//   // this->data = new unsigned char[this->dataSize]; // memory leak!
-//   // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++)
-//   //   this->data[dataIx] = buffer[ix++];
-// }
-
-// bool CustomMsg::Send(Participant *participant, unsigned char networkId,
-//                      unsigned char thingId, unsigned char *data,
-//                      unsigned char dataSize) {
-//   CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize);
-//   return msg.Send(participant);
-// }
-
 CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
   CustomMsg msg = CustomMsg(buffer);
   return msg;

Messages.h

@@ -2,10 +2,11 @@
 
 #include "../LinearAlgebra/Spherical.h"
 #include "../LinearAlgebra/SwingTwist.h"
-#include "../float16/float16.h"
 #include "CoreThing.h"
+#include "float16.h"
 
-namespace Passer::Control {
+namespace Passer {
+namespace Control {
 
 class Participant;
 
@@ -133,19 +134,20 @@ public:
 
   unsigned char networkId;
   unsigned char thingId;
+  CoreThing *thing;
 
   unsigned char dataSize;
   unsigned char *data;
 
   CustomMsg(unsigned char *buffer);
-  CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data,
+  CustomMsg(unsigned char networkId, CoreThing *thing);
-            unsigned char dataSize);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
 
   static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
 };
 
-} // namespace Passer::Control
+} // namespace Control
+} // namespace Passer
 
 using namespace Passer::Control;

Participant.h

@@ -2,7 +2,8 @@
 
 #include "Messages.h"
 
-namespace Passer::Control {
+namespace Passer {
+namespace Control {
 
 class Participant {
 public:
@@ -21,5 +22,6 @@ protected:
   virtual void ProcessCustomMsg(CustomMsg msg);
 };
 
-} // namespace Passer::Control
+} // namespace Control
+} // namespace Passer
 using namespace Passer::Control;

float16.cpp

@@ -0,0 +1,243 @@
+//
+//    FILE: float16.cpp
+//  AUTHOR: Rob Tillaart
+// VERSION: 0.1.8
+// PURPOSE: library for Float16s for Arduino
+//     URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format
+
+#include "float16.h"
+// #include <limits>
+#include <math.h>
+
+//  CONSTRUCTOR
+float16::float16(float f) { _value = f32tof16(f); }
+
+//  PRINTING
+// size_t float16::printTo(Print& p) const
+// {
+//   double d = this->f16tof32(_value);
+//   return p.print(d, _decimals);
+// }
+
+float float16::toFloat() const { return f16tof32(_value); }
+
+//////////////////////////////////////////////////////////
+//
+//  EQUALITIES
+//
+bool float16::operator==(const float16 &f) { return (_value == f._value); }
+
+bool float16::operator!=(const float16 &f) { return (_value != f._value); }
+
+bool float16::operator>(const float16 &f) {
+  if ((_value & 0x8000) && (f._value & 0x8000))
+    return _value < f._value;
+  if (_value & 0x8000)
+    return false;
+  if (f._value & 0x8000)
+    return true;
+  return _value > f._value;
+}
+
+bool float16::operator>=(const float16 &f) {
+  if ((_value & 0x8000) && (f._value & 0x8000))
+    return _value <= f._value;
+  if (_value & 0x8000)
+    return false;
+  if (f._value & 0x8000)
+    return true;
+  return _value >= f._value;
+}
+
+bool float16::operator<(const float16 &f) {
+  if ((_value & 0x8000) && (f._value & 0x8000))
+    return _value > f._value;
+  if (_value & 0x8000)
+    return true;
+  if (f._value & 0x8000)
+    return false;
+  return _value < f._value;
+}
+
+bool float16::operator<=(const float16 &f) {
+  if ((_value & 0x8000) && (f._value & 0x8000))
+    return _value >= f._value;
+  if (_value & 0x8000)
+    return true;
+  if (f._value & 0x8000)
+    return false;
+  return _value <= f._value;
+}
+
+//////////////////////////////////////////////////////////
+//
+//  NEGATION
+//
+float16 float16::operator-() {
+  float16 f16;
+  f16.setBinary(_value ^ 0x8000);
+  return f16;
+}
+
+//////////////////////////////////////////////////////////
+//
+//  MATH
+//
+float16 float16::operator+(const float16 &f) {
+  return float16(this->toFloat() + f.toFloat());
+}
+
+float16 float16::operator-(const float16 &f) {
+  return float16(this->toFloat() - f.toFloat());
+}
+
+float16 float16::operator*(const float16 &f) {
+  return float16(this->toFloat() * f.toFloat());
+}
+
+float16 float16::operator/(const float16 &f) {
+  return float16(this->toFloat() / f.toFloat());
+}
+
+float16 &float16::operator+=(const float16 &f) {
+  *this = this->toFloat() + f.toFloat();
+  return *this;
+}
+
+float16 &float16::operator-=(const float16 &f) {
+  *this = this->toFloat() - f.toFloat();
+  return *this;
+}
+
+float16 &float16::operator*=(const float16 &f) {
+  *this = this->toFloat() * f.toFloat();
+  return *this;
+}
+
+float16 &float16::operator/=(const float16 &f) {
+  *this = this->toFloat() / f.toFloat();
+  return *this;
+}
+
+//////////////////////////////////////////////////////////
+//
+//  MATH HELPER FUNCTIONS
+//
+int float16::sign() {
+  if (_value & 0x8000)
+    return -1;
+  if (_value & 0xFFFF)
+    return 1;
+  return 0;
+}
+
+bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); }
+
+bool float16::isNaN() {
+  if ((_value & 0x7C00) != 0x7C00)
+    return false;
+  if ((_value & 0x03FF) == 0x0000)
+    return false;
+  return true;
+}
+
+bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); }
+
+//////////////////////////////////////////////////////////
+//
+//  CORE CONVERSION
+//
+float float16::f16tof32(uint16_t _value) const {
+  uint16_t sgn, man;
+  int exp;
+  float f;
+
+  sgn = (_value & 0x8000) > 0;
+  exp = (_value & 0x7C00) >> 10;
+  man = (_value & 0x03FF);
+
+  //  ZERO
+  if ((_value & 0x7FFF) == 0) {
+    return sgn ? -0.0f : 0.0f;
+  }
+  //  NAN & INF
+  if (exp == 0x001F) {
+    if (man == 0)
+      return sgn ? -INFINITY : INFINITY;
+    else
+      return NAN;
+  }
+
+  //  SUBNORMAL/NORMAL
+  if (exp == 0)
+    f = 0;
+  else
+    f = 1;
+
+  //  PROCESS MANTISSE
+  for (int i = 9; i >= 0; i--) {
+    f *= 2;
+    if (man & (1 << i))
+      f = f + 1;
+  }
+  f = f * powf(2.0f, (float)(exp - 25));
+  if (exp == 0) {
+    f = f * powf(2.0f, -13); // 5.96046447754e-8;
+  }
+  return sgn ? -f : f;
+}
+
+uint16_t float16::f32tof16(float f) const {
+  uint32_t t = *(uint32_t *)&f;
+  //  man bits = 10; but we keep 11 for rounding
+  uint16_t man = (t & 0x007FFFFF) >> 12;
+  int16_t exp = (t & 0x7F800000) >> 23;
+  bool sgn = (t & 0x80000000);
+
+  //  handle 0
+  if ((t & 0x7FFFFFFF) == 0) {
+    return sgn ? 0x8000 : 0x0000;
+  }
+  //  denormalized float32 does not fit in float16
+  if (exp == 0x00) {
+    return sgn ? 0x8000 : 0x0000;
+  }
+  //  handle infinity & NAN
+  if (exp == 0x00FF) {
+    if (man)
+      return 0xFE00;              //  NAN
+    return sgn ? 0xFC00 : 0x7C00; //  -INF : INF
+  }
+
+  //  normal numbers
+  exp = exp - 127 + 15;
+  //  overflow does not fit => INF
+  if (exp > 30) {
+    return sgn ? 0xFC00 : 0x7C00; //  -INF : INF
+  }
+  //  subnormal numbers
+  if (exp < -38) {
+    return sgn ? 0x8000 : 0x0000; //  -0 or 0  ?   just 0 ?
+  }
+  if (exp <= 0) //  subnormal
+  {
+    man >>= (exp + 14);
+    //  rounding
+    man++;
+    man >>= 1;
+    if (sgn)
+      return 0x8000 | man;
+    return man;
+  }
+
+  //  normal
+  //  TODO rounding
+  exp <<= 10;
+  man++;
+  man >>= 1;
+  if (sgn)
+    return 0x8000 | exp | man;
+  return exp | man;
+}
+
+//  -- END OF FILE --

float16.h

@@ -0,0 +1,75 @@
+#pragma once
+//
+//    FILE: float16.h
+//  AUTHOR: Rob Tillaart
+// VERSION: 0.1.8
+// PURPOSE: Arduino library to implement float16 data type.
+//          half-precision floating point format,
+//          used for efficient storage and transport.
+//     URL: https://github.com/RobTillaart/float16
+
+// #include "Arduino.h"
+#include <stdint.h>
+
+#define FLOAT16_LIB_VERSION (F("0.1.8"))
+
+typedef uint16_t __fp16;
+
+class float16 {
+public:
+  //  Constructors
+  float16(void) { _value = 0x0000; };
+  float16(float f);
+  float16(const float16 &f) { _value = f._value; };
+
+  //  Conversion
+  float toFloat(void) const;
+  //  access the 2 byte representation.
+  uint16_t getBinary() { return _value; };
+  void setBinary(uint16_t u) { _value = u; };
+
+  //  Printable
+  // size_t printTo(Print &p) const;
+  void setDecimals(uint8_t d) { _decimals = d; };
+  uint8_t getDecimals() { return _decimals; };
+
+  //  equalities
+  bool operator==(const float16 &f);
+  bool operator!=(const float16 &f);
+
+  bool operator>(const float16 &f);
+  bool operator>=(const float16 &f);
+  bool operator<(const float16 &f);
+  bool operator<=(const float16 &f);
+
+  //  negation
+  float16 operator-();
+
+  //  basic math
+  float16 operator+(const float16 &f);
+  float16 operator-(const float16 &f);
+  float16 operator*(const float16 &f);
+  float16 operator/(const float16 &f);
+
+  float16 &operator+=(const float16 &f);
+  float16 &operator-=(const float16 &f);
+  float16 &operator*=(const float16 &f);
+  float16 &operator/=(const float16 &f);
+
+  //  math helper functions
+  int sign(); //  1 = positive   0 = zero   -1 = negative.
+  bool isZero();
+  bool isNaN();
+  bool isInf();
+
+  //  CORE CONVERSION
+  //  should be private but for testing...
+  float f16tof32(uint16_t) const;
+  uint16_t f32tof16(float) const;
+
+private:
+  uint8_t _decimals = 4;
+  __fp16 _value;
+};
+
+//  -- END OF FILE --

test/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+Project(ControlCoreTest)
+
+set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    . 
+    ..
+)
+enable_testing()
+
+add_executable(
+    ControlCoreTest
+    "dummy_test.cc"
+)
+
+target_link_libraries(
+    ControlCoreTest
+    gtest_main
+)
+
+include(GoogleTest)
+gtest_discover_tests(ControlCoreTest)

test/dummy_test.cc

@@ -0,0 +1,9 @@
+#if GTEST
+
+// #include <gmock/gmock.h>
+// not supported using Visual Studio 2022 compiler...
+#include <gtest/gtest.h>
+
+TEST(Dummy, Dummytest) {}
+
+#endif