Fixed issues, added test project

.gitignore

@@ -1 +1,6 @@
 DoxyGen/DoxyWarnLogfile.txt
+.vscode/settings.json
+test/bin
+test/obj
+/bin
+/obj

ControlCore.csproj

@@ -0,0 +1,19 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <GenerateAssemblyInfo>false</GenerateAssemblyInfo>
+    <GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute> 
+    <TargetFramework>net9.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="coverlet.collector" Version="6.0.2" />
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.11.1" />
+    <PackageReference Include="NUnit" Version="4.2.2" />
+    <PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
+    <PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
+  </ItemGroup>
+
+</Project>

LinearAlgebra/Angle.cs

@@ -0,0 +1,5 @@
+class Angle
+{
+    public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
+    public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
+}

LinearAlgebra/Quat32.cs

@@ -7,6 +7,14 @@ namespace Passer.LinearAlgebra
         public float z;
         public float w;
 
+        public Quat32()
+        {
+            this.x = 0;
+            this.y = 0;
+            this.z = 0;
+            this.w = 1;
+        }
+
         public Quat32(float x, float y, float z, float w)
         {
             this.x = x;
@@ -14,5 +22,75 @@ namespace Passer.LinearAlgebra
             this.z = z;
             this.w = w;
         }
+
+        public static Quat32 FromSwingTwist(SwingTwist s)
+        {
+            Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
+            return q32;
+        }
+
+        public static Quat32 Euler(float yaw, float pitch, float roll)
+        {
+            float rollOver2 = roll * Angle.Deg2Rad * 0.5f;
+            float sinRollOver2 = (float)Math.Sin((float)rollOver2);
+            float cosRollOver2 = (float)Math.Cos((float)rollOver2);
+            float pitchOver2 = pitch * 0.5f;
+            float sinPitchOver2 = (float)Math.Sin((float)pitchOver2);
+            float cosPitchOver2 = (float)Math.Cos((float)pitchOver2);
+            float yawOver2 = yaw * 0.5f;
+            float sinYawOver2 = (float)Math.Sin((float)yawOver2);
+            float cosYawOver2 = (float)Math.Cos((float)yawOver2);
+            Quat32 result = new()
+            {
+                w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
+                       sinYawOver2 * sinPitchOver2 * sinRollOver2,
+                x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
+                       cosYawOver2 * sinPitchOver2 * sinRollOver2,
+                y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
+                       sinYawOver2 * cosPitchOver2 * sinRollOver2,
+                z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
+                       sinYawOver2 * sinPitchOver2 * cosRollOver2
+            };
+            return result;
+        }
+
+        public void ToAngles(out float right, out float up, out float forward)
+        {
+            float test = this.x * this.y + this.z * this.w;
+            if (test > 0.499f)
+            {  // singularity at north pole
+                right = 0;
+                up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
+                forward = 90;
+                return;
+                //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90);
+            }
+            else if (test < -0.499f)
+            {  // singularity at south pole
+                right = 0;
+                up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
+                forward = -90;
+                return;
+                //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90);
+            }
+            else
+            {
+                float sqx = this.x * this.x;
+                float sqy = this.y * this.y;
+                float sqz = this.z * this.z;
+
+                right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg;
+                up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg;
+                forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg;
+                return;
+                // return Vector3(
+                //     atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) *
+                //         Rad2Deg,
+                //     atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) *
+                //         Rad2Deg,
+                //     asinf(2 * test) * Angle.Rad2Deg);
+            }
+        }
+
     }
 }

LinearAlgebra/SwingTwist.cs

@@ -1,20 +1,32 @@
-namespace Passer.LinearAlgebra {
+namespace Passer.LinearAlgebra
+{
 
-    public class SwingTwist {
+    public class SwingTwist
+    {
         public Direction swing;
         public float twist;
 
         public static readonly SwingTwist zero = new(0, 0, 0);
 
-        public SwingTwist(Direction swing, float twist) {
+        public SwingTwist(Direction swing, float twist)
+        {
             this.swing = swing;
             this.twist = twist;
         }
-        public SwingTwist(float horizontalSwing, float verticalSwing, float twist) {
+        public SwingTwist(float horizontalSwing, float verticalSwing, float twist)
+        {
             this.swing = new Direction(horizontalSwing, verticalSwing);
             this.swing.Normalize();
             this.twist = twist;
         }
+        public static SwingTwist FromQuat32(Quat32 q32)
+        {
+            // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
+            // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
+            q32.ToAngles(out float right, out float up, out float forward);
+            SwingTwist r = new(up, right, forward);
+            return r;
+        }
     }
 
 }

LowLevelMessages.cs

@@ -1,14 +1,19 @@
 using Passer.LinearAlgebra;
 
-public class LowLevelMessages {
+namespace Passer.Control.Core
+{
+    public class LowLevelMessages
+    {
 
-    public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) {
+        public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
+        {
             SendFloat16(buffer, ref ix, new float16(v.distance));
             SendAngle8(buffer, ref ix, v.direction.horizontal);
             SendAngle8(buffer, ref ix, v.direction.horizontal);
         }
 
-    public static Spherical ReceiveSpherical(byte[] data, ref byte ix) {
+        public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
+        {
             float distance = ReceiveFloat16(data, ref ix);
             float horizontal = ReceiveAngle8(data, ref ix);
             float vertical = ReceiveAngle8(data, ref ix);
@@ -16,18 +21,20 @@ public class LowLevelMessages {
             return v;
         }
 
-    public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
+        public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
-        UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
+        {
-        Quat32 q32 = new(q.x, q.y, q.z, q.w);
+            Quat32 q32 = Quat32.FromSwingTwist(s);
             SendQuat32(buffer, ref ix, q32);
 
         }
-    public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) {
+        public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
+        {
             int qx = (int)(q.x * 127 + 128);
             int qy = (int)(q.y * 127 + 128);
             int qz = (int)(q.z * 127 + 128);
             int qw = (int)(q.w * 255);
-        if (q.w < 0) {
+            if (q.w < 0)
+            {
                 qx = -qx;
                 qy = -qy;
                 qz = -qz;
@@ -39,7 +46,8 @@ public class LowLevelMessages {
             buffer[ix++] = (byte)qz;
             buffer[ix++] = (byte)qw;
         }
-    public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) {
+        public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
+        {
             Quat32 q = new(
                 (data[ix++] - 128.0F) / 127.0F,
                 (data[ix++] - 128.0F) / 127.0F,
@@ -48,14 +56,18 @@ public class LowLevelMessages {
             return q;
         }
 
-    public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
+        public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix)
+        {
             Quat32 q32 = ReceiveQuat32(data, ref ix);
-        UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
+            // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
-        SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
+            // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
+
+            SwingTwist r = SwingTwist.FromQuat32(q32);
             return r;
         }
 
-    public static void SendAngle8(byte[] buffer, ref byte ix, float angle) {
+        public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
+        {
             // Normalize angle
             while (angle >= 180)
                 angle -= 360;
@@ -64,28 +76,34 @@ public class LowLevelMessages {
             buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
         }
 
-    public static float ReceiveAngle8(byte[] data, ref byte ix) {
+        public static float ReceiveAngle8(byte[] data, ref byte ix)
+        {
             float value = (data[ix++] * 180) / 128.0F;
             return value;
         }
 
-    public static void SendFloat16(byte[] data, ref byte ix, float f) {
+        public static void SendFloat16(byte[] data, ref byte ix, float f)
+        {
             float16 f16 = new(f);
             ushort binary = f16.GetBinary();
             data[ix++] = (byte)(binary >> 8);
             data[ix++] = (byte)(binary & 255);
         }
-    public static void SendFloat16(byte[] data, ref byte ix, float16 f) {
+        public static void SendFloat16(byte[] data, ref byte ix, float16 f)
+        {
             ushort binary = f.GetBinary();
             data[ix++] = (byte)(binary >> 8);
             data[ix++] = (byte)(binary & 255);
         }
 
-    public static float ReceiveFloat16(byte[] data, ref byte ix) {
+        public static float ReceiveFloat16(byte[] data, ref byte ix)
+        {
             ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
             float16 f16 = new();
             f16.SetBinary(value);
             float f = f16.toFloat();
             return f;
         }
+    }
+
 }

Participant.cs

@@ -37,7 +37,8 @@ namespace Passer.Control.Core
 
         #region Init
 
-        public Participant() {
+        public Participant()
+        {
             this.dataStream = new EchoStream();
             others.Add(this);
         }
@@ -59,14 +60,17 @@ namespace Passer.Control.Core
         private float nextPublishMe = 0;
 
 
-        public virtual void Update(float currentTime) {
+        public virtual void Update(float currentTime)
-            if (currentTime > this.nextPublishMe) {
+        {
+            if (currentTime > this.nextPublishMe)
+            {
                 this.PublishBuffer(ClientMsg.Serialized(this.buffer, this.networkId));
                 ClientMsg.Publish(this, this.networkId);
                 this.nextPublishMe = currentTime + Participant.publishInterval;
             }
 
-            for (int ix = 0; ix < this.others.Count; ix++) {
+            for (int ix = 0; ix < this.others.Count; ix++)
+            {
                 Participant client = this.others[ix];
                 if (client == null)
                     continue;
@@ -85,7 +89,7 @@ namespace Passer.Control.Core
             //if (this.ipAddress == null)
             //    return false;
 
-            UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
+            // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
             //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
             this.udpClient.Send(this.buffer, bufferSize, this.endPoint);
             return true;
@@ -104,8 +108,10 @@ namespace Passer.Control.Core
 
         #region Receive
 
-        public async Task ReceiveData() {
+        public async Task ReceiveData()
-            while (true) {
+        {
+            while (true)
+            {
                 byte packetSize = (byte)this.dataStream.ReadByte();
                 if (packetSize != 0xFF)
                     await ReceiveData(this.dataStream, this, packetSize);
@@ -113,16 +119,19 @@ namespace Passer.Control.Core
             }
         }
 
-        public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) {
+        public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize)
+        {
             byte msgId = (byte)dataStream.ReadByte();
-            if (msgId == 0xFF) {
+            if (msgId == 0xFF)
+            {
                 // Timeout
                 return;
             }
 
             //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}");
             bool result = false;
-            switch (msgId) {
+            switch (msgId)
+            {
                 case ClientMsg.Id: // 0xA0 / 160
                     result = await ClientMsg.Receive(dataStream, client, packetSize);
                     break;
@@ -156,7 +165,8 @@ namespace Passer.Control.Core
                 default:
                     break;
             }
-            if (result == false) {
+            if (result == false)
+            {
                 packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte
                 await ReceiveData(dataStream, client, packetSize);
             }

Thing.cs

@@ -1,7 +1,8 @@
 using System.Collections.Generic;
 using Passer.LinearAlgebra;
 
-namespace Passer.Control.Core {
+namespace Passer.Control.Core
+{
 
     /// <summary>
     /// A thing is the basic building block
@@ -18,29 +19,36 @@ namespace Passer.Control.Core {
 
         public event ChangeHandler OnParentChanged;
         private Thing _parent;
-        public Thing parent {
+        public Thing parent
+        {
             get => _parent;
-            set {
+            set
+            {
                 if (_parent == value)
                     return;
 
-                if (value == null) {
+                if (value == null)
+                {
                     _parent.RemoveChild(this);
                     _parent = null;
-                } else {
+                }
+                else
+                {
                     value.AddChild(this);
                     OnParentChanged?.Invoke();
                 }
             }
         }
 
-        public void AddChild(Thing child) {
+        public void AddChild(Thing child)
+        {
             if (children.Find(thing => thing == child) != null)
                 return;
             child._parent = this;
             children.Add(child);
         }
-        public void RemoveChild(Thing child) {
+        public void RemoveChild(Thing child)
+        {
             children.Remove(child);
         }
 
@@ -50,10 +58,13 @@ namespace Passer.Control.Core {
 
         public event ChangeHandler OnNameChanged;
         private string _name;
-        public string name {
+        public string name
+        {
             get => _name;
-            set {
+            set
-                if (_name != value) {
+            {
+                if (_name != value)
+                {
                     _name = value;
                     OnNameChanged?.Invoke();
                 }
@@ -65,10 +76,13 @@ namespace Passer.Control.Core {
         public byte poseUpdated = 0x00;
         public event ChangeHandler OnPositionChanged;
         private Spherical _position;
-        public Spherical position {
+        public Spherical position
+        {
             get { return _position; }
-            set {
+            set
-                if (_position != value) {
+            {
+                if (_position != value)
+                {
                     _position = value;
                     OnPositionChanged?.Invoke();
                 }
@@ -77,10 +91,13 @@ namespace Passer.Control.Core {
 
         public event ChangeHandler OnOrientationChanged;
         private SwingTwist _orientation;
-        public SwingTwist orientation {
+        public SwingTwist orientation
+        {
             get { return _orientation; }
-            set {
+            set
-                if (_orientation != value) {
+            {
+                if (_orientation != value)
+                {
                     _orientation = value;
                     OnOrientationChanged?.Invoke();
                 }
@@ -89,10 +106,13 @@ namespace Passer.Control.Core {
 
         public event SphericalHandler OnLinearVelocityChanged;
         private Spherical _linearVelocity;
-        public Spherical linearVelocity {
+        public Spherical linearVelocity
+        {
             get => _linearVelocity;
-            set {
+            set
-                if (_linearVelocity != value) {
+            {
+                if (_linearVelocity != value)
+                {
                     _linearVelocity = value;
                     OnLinearVelocityChanged?.Invoke(_linearVelocity);
                 }
@@ -100,17 +120,21 @@ namespace Passer.Control.Core {
         }
         public Spherical angularVelocity;
 
-        public virtual void Init(bool invokeEvent = true) {
+        public virtual void Init(bool invokeEvent = true)
+        {
             Thing.Add(this, invokeEvent);
         }
 
-        public Thing(bool initialize = true) {
+        public Thing(bool initialize = true)
-            if (initialize) {
+        {
+            if (initialize)
+            {
                 //this.Init();
                 Thing.Add(this);
             }
         }
-        public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) {
+        public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0)
+        {
             this.participant = client;
             this.id = thingId;
             this.type = thingType;
@@ -119,29 +143,31 @@ namespace Passer.Control.Core {
             Thing.Add(this);
         }
 
-        public virtual void Update(float currentTime) {
+        public virtual void Update(float currentTime)
+        {
             // should recurse over children...
         }
 
-        public virtual void ProcessBytes(byte[] bytes) {
+        public virtual void ProcessBytes(byte[] bytes)
+        {
             //if (sensor != null)
             //    sensor.ProcessBytes(bytes);
         }
 
         // Experimental
 
-        public float stressLevel = 0;
+        // public float stressLevel = 0;
 
-        protected delegate void ReceptorFunc(Sensor sensor);
+        // protected delegate void ReceptorFunc(Sensor sensor);
-        protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) {
+        // protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) {
-            sensor.Signaller += (sensor => Receptor(receptor, sensor));
+        //     sensor.Signaller += (sensor => Receptor(receptor, sensor));
-        }
+        // }
-        protected void Receptor(ReceptorFunc receptor, Sensor sensor) {
+        // protected void Receptor(ReceptorFunc receptor, Sensor sensor) {
-            if (sensor.signalStrength <= stressLevel)
+        //     if (sensor.signalStrength <= stressLevel)
-                return;
+        //         return;
 
-            receptor(sensor);
+        //     receptor(sensor);
-        }
+        // }
 
         //---------- All Things
 
@@ -150,40 +176,48 @@ namespace Passer.Control.Core {
         public delegate void ThingHandler(Thing t);
         public static event ThingHandler OnNewThing;
 
-        public static void Add(Thing thing, bool invokeEvent = true) {
+        public static void Add(Thing thing, bool invokeEvent = true)
+        {
             Thing foundThing = Get(thing.networkId, thing.id);
 
-            if (foundThing == null) {
+            if (foundThing == null)
+            {
                 if (thing.id == 0)
                     thing.id = (byte)(allThings.Count + 1);
                 allThings.Add(thing);
                 if (invokeEvent)
                     OnNewThing?.Invoke(thing);
-                UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
+                // UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
             }
         }
 
-        public static Thing Get(byte networkId, byte thingId) {
+        public static Thing Get(byte networkId, byte thingId)
+        {
             Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
             return thing;
         }
 
-        private static bool IsThing(Thing thing, byte networkId, byte thingId) {
+        private static bool IsThing(Thing thing, byte networkId, byte thingId)
+        {
             if (thing == null)
                 return false;
-            return (thing.networkId == networkId ) && (thing.id == thingId);
+            return (thing.networkId == networkId) && (thing.id == thingId);
         }
 
-        public static void Remove(byte networkId, byte thingId) {
+        public static void Remove(byte networkId, byte thingId)
+        {
             allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
         }
 
-        public static Thing[] GetAllThings() {
+        public static Thing[] GetAllThings()
+        {
             return allThings.ToArray();
         }
 
-        public static void UpdateAll(float currentTime) {
+        public static void UpdateAll(float currentTime)
-            foreach (Thing thing in allThings) {
+        {
+            foreach (Thing thing in allThings)
+            {
                 if (thing.parent == null) // update only root things
                     thing.Update(currentTime);
             }

float16.cs

@@ -0,0 +1,272 @@
+namespace Passer.LinearAlgebra
+{
+
+    public class float16
+    {
+        //
+        //    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
+
+        ushort _value;
+
+        public float16() { _value = 0; }
+
+        public float16(float f)
+        {
+            _value = f32tof16(f);
+        }
+
+        public float toFloat()
+        {
+            return f16tof32(_value);
+        }
+
+        public ushort GetBinary() { return _value; }
+        public void SetBinary(ushort value) { _value = 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->toDouble() + f.toDouble());
+    }
+
+    float16 float16::operator -(const float16 &f) {
+        return float16(this->toDouble() - f.toDouble());
+    }
+
+    float16 float16::operator *(const float16 &f) {
+        return float16(this->toDouble() * f.toDouble());
+    }
+
+    float16 float16::operator /(const float16 &f) {
+        return float16(this->toDouble() / f.toDouble());
+    }
+
+    float16 & float16::operator+=(const float16 &f) {
+        *this = this->toDouble() + f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator-=(const float16 &f) {
+        *this = this->toDouble() - f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator*=(const float16 &f) {
+        *this = this->toDouble() * f.toDouble();
+        return *this;
+    }
+
+    float16 & float16::operator/=(const float16 &f) {
+        *this = this->toDouble() / f.toDouble();
+        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 f16tof32(ushort _value)
+        {
+            //ushort sgn;
+            ushort man;
+            int exp;
+            float f;
+
+            //Debug.Log($"{_value}");
+
+            bool sgn = (_value & 0x8000) > 0;
+            exp = (_value & 0x7C00) >> 10;
+            man = (ushort)(_value & 0x03FF);
+
+            //Debug.Log($"{sgn} {exp} {man}");
+
+            //  ZERO
+            if ((_value & 0x7FFF) == 0)
+            {
+                return sgn ? -0 : 0;
+            }
+            //  NAN & INF
+            if (exp == 0x001F)
+            {
+                if (man == 0)
+                    return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY;
+                else
+                    return float.NaN; // 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)) != 0)
+                    f = f + 1;
+            }
+            //Debug.Log($"{f}");
+            f = f * (float)Math.Pow(2.0f, exp - 25);
+            if (exp == 0)
+            {
+                f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8;
+            }
+            //Debug.Log($"{f}");
+            return sgn ? -f : f;
+        }
+
+        ushort f32tof16(float f)
+        {
+            //uint t = *(uint*)&f;
+            uint t = (uint)BitConverter.SingleToInt32Bits(f);
+            //  man bits = 10; but we keep 11 for rounding
+            ushort man = (ushort)((t & 0x007FFFFF) >> 12);
+            short exp = (short)((t & 0x7F800000) >> 23);
+            bool sgn = (t & 0x80000000) != 0;
+
+            //  handle 0
+            if ((t & 0x7FFFFFFF) == 0)
+            {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            //  denormalized float32 does not fit in float16
+            if (exp == 0x00)
+            {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000;
+            }
+            //  handle infinity & NAN
+            if (exp == 0x00FF)
+            {
+                if (man != 0)
+                    return 0xFE00;              //  NAN
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; //  -INF : INF
+            }
+
+            //  normal numbers
+            exp = (short)(exp - 127 + 15);
+            //  overflow does not fit => INF
+            if (exp > 30)
+            {
+                return sgn ? (ushort)0xFC00 : (ushort)0x7C00; //  -INF : INF
+            }
+            //  subnormal numbers
+            if (exp < -38)
+            {
+                return sgn ? (ushort)0x8000 : (ushort)0x0000; //  -0 or 0  ?   just 0 ?
+            }
+            if (exp <= 0) //  subnormal
+            {
+                man >>= (exp + 14);
+                //  rounding
+                man++;
+                man >>= 1;
+                if (sgn)
+                    return (ushort)(0x8000 | man);
+                return man;
+            }
+
+            //  normal
+            //  TODO rounding
+            exp <<= 10;
+            man++;
+            man >>= 1;
+            if (sgn)
+                return (ushort)(0x8000 | exp | man);
+            return (ushort)(exp | man);
+        }
+
+        //  -- END OF FILE --
+    }
+
+}

test.sln

@@ -0,0 +1,36 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.0.31903.59
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ControlCore", "ControlCore.csproj", "{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "test", "test\test.csproj", "{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Release|Any CPU.Build.0 = Release|Any CPU
+		{DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Release|Any CPU.Build.0 = Release|Any CPU
+		{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Release|Any CPU.Build.0 = Release|Any CPU
+		{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+EndGlobal

test/UnitTest1.cs

@@ -0,0 +1,17 @@
+namespace test;
+
+using Passer.Control.Core;
+
+public class Tests
+{
+    [SetUp]
+    public void Setup()
+    {
+    }
+
+    [Test]
+    public void Test1()
+    {
+        Assert.Pass();
+    }
+}

test/test.csproj

@@ -0,0 +1,28 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net9.0</TargetFramework>
+    <LangVersion>latest</LangVersion>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+    <IsPackable>false</IsPackable>
+    <IsTestProject>true</IsTestProject>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="coverlet.collector" Version="6.0.2" />
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.11.1" />
+    <PackageReference Include="NUnit" Version="4.2.2" />
+    <PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
+    <PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Using Include="NUnit.Framework" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\ControlCore.csproj" />
+  </ItemGroup>
+
+</Project>