From a48ae12fc2f6d4a99119c128e78bf4b103e607c3 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 6 Dec 2024 16:30:24 +0100
Subject: [PATCH] ControlCore mostly works (but I don't see a model on the site
 server yet)

---
 LowLevelMessages.cs | 41 +++++++++++++++++++++++++++++++++++++++--
 Messages.cs         | 45 +++++++++++++++++++++++++++++++++++++++++----
 2 files changed, 80 insertions(+), 6 deletions(-)

diff --git a/LowLevelMessages.cs b/LowLevelMessages.cs
index a976ddf..f279dbc 100644
--- a/LowLevelMessages.cs
+++ b/LowLevelMessages.cs
@@ -1,6 +1,13 @@
 using Passer;
 
 public class LowLevelMessages {
+
+    public static void SendSpherical(byte[] buffer, ref uint ix, Spherical v) {
+        SendFloat16(buffer, ref ix, new float16(v.distance));
+        SendAngle8(buffer, ref ix, v.horizontal);
+        SendAngle8(buffer, ref ix, v.vertical);
+    }
+
     public static Spherical ReceiveSpherical(byte[] data, ref uint ix) {
         float horizontal = ReceiveAngle8(data, ref ix);
         float vertical = ReceiveAngle8(data, ref ix);
@@ -9,6 +16,23 @@ public class LowLevelMessages {
         return v;
     }
 
+    public static void SendQuat32(byte[] buffer, ref uint 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) {
+            qx = -qx;
+            qy = -qy;
+            qz = -qz;
+            qw = -qw;
+        }
+
+        buffer[ix++] = (byte)qx;
+        buffer[ix++] = (byte)qy;
+        buffer[ix++] = (byte)qz;
+        buffer[ix++] = (byte)qw;
+    }
     public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) {
         Quat32 q = new(
             (data[ix++] - 128.0F) / 127.0F,
@@ -18,11 +42,26 @@ public class LowLevelMessages {
         return q;
     }
 
+    public static void SendAngle8(byte[] buffer, ref uint ix, float angle) {
+        // Normalize angle
+        while (angle >= 180)
+            angle -= 360;
+        while (angle < -180)
+            angle += 360;
+        buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
+    }
+
     public static float ReceiveAngle8(byte[] data, ref uint ix) {
         float value = (data[ix++] * 180) / 128.0F;
         return value;
     }
 
+    public static void SendFloat16(byte[] data, ref uint ix, float16 f) {
+        ushort binary = f.GetBinary();
+        data[ix++] = (byte)(binary >> 8);
+        data[ix++] = (byte)(binary & 255);
+    }
+
     public static float ReceiveFloat16(byte[] data, ref uint ix) {
         ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
         float16 f16 = new();
@@ -30,6 +69,4 @@ public class LowLevelMessages {
         float f = f16.toFloat();
         return f;
     }
-
-
 }
diff --git a/Messages.cs b/Messages.cs
index acbecad..b9c7e2b 100644
--- a/Messages.cs
+++ b/Messages.cs
@@ -62,6 +62,18 @@ namespace Passer.Control {
 			client.udpClient.Send(data, data.Length, client.ipAddress, client.port);
 			return true;
 		}
+
+        public static bool PublishMsg(Client client, IMessage msg) {
+            return PublishMsg(client, msg.Serialize());
+        }
+        public static bool PublishMsg(Client client, byte[] data) {
+			if (client == null)
+				return false;
+
+            client.udpClient.Send(data, data.Length, "127.0.0.1", client.port);
+			return true;
+        }
+
 		public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
 			byte[] buffer = new byte[packetSize - 1]; //  without msgId
 			int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
@@ -86,7 +98,13 @@ namespace Passer.Control {
 		}
 		public ClientMsg(byte[] data) : base(data) { }
 
-		public override void Deserialize(byte[] data) {
+        public override byte[] Serialize() {
+			byte[] buffer = new byte[ClientMsg.length];
+			buffer[0] = ClientMsg.Id;
+			buffer[1] = networkId;
+			return buffer;
+        }
+        public override void Deserialize(byte[] data) {
 			base.Deserialize(data);
 			uint ix = 0;
 			networkId = data[ix];
@@ -96,6 +114,10 @@ namespace Passer.Control {
 			ClientMsg msg = new(networkId);
 			return SendMsg(client, msg);
 		}
+		public static bool Publish(Client client, byte networkId) {
+			ClientMsg msg = new(networkId);
+			return PublishMsg(client, msg);
+		}
 		public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
 			if (packetSize != length)
 				return false;
@@ -212,7 +234,7 @@ namespace Passer.Control {
     #region Thing
 
     public class ThingMsg : IMessage {
-		public const byte length = 4;
+		public const byte length = 5;
 		public const byte Id = 0x80;
 		public byte objectId;
 		public byte objectType;
@@ -227,7 +249,7 @@ namespace Passer.Control {
 		}
 
 		public static bool Send(Client client, byte thingId, byte thingType, byte parentId) {
-			byte[] data = new byte[4];
+			byte[] data = new byte[ThingMsg.length];
 			data[0] = ThingMsg.Id;
 			data[1] = client.networkId;
 			data[2] = thingId;
@@ -370,12 +392,27 @@ namespace Passer.Control {
             this.poseType = 0;
 			if (this.position != null)
 				this.poseType |= Pose_Position;
+			else
+				this.position = new Spherical(0, 0, 0);
 			if (this.orientation != null)
 				this.poseType |= Pose_Orientation;
+			else
+				this.orientation = new Quat32(0, 0, 0, 1);
         }
         public PoseMsg(byte[] data) : base(data) { }
 
-		public override void Deserialize(byte[] data) {
+        public override byte[] Serialize() {
+			byte[] buffer = new byte[PoseMsg.length];
+			uint ix = 0;
+			buffer[ix++] = PoseMsg.Id;
+			buffer[ix++] = this.thingId;
+			buffer[ix++] = this.poseType;
+
+			LowLevelMessages.SendSpherical(buffer, ref ix, position);
+			LowLevelMessages.SendQuat32(buffer, ref ix, orientation);
+			return buffer;
+        }
+        public override void Deserialize(byte[] data) {
 			uint ix = 0;
 			thingId = data[ix++];
 			poseType = data[ix++];