From 85a19faeb4ec342e68dc4824042ac7f78baf29c4 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 11 Feb 2025 16:02:28 +0100
Subject: [PATCH] First step to ControlCore

---
 Sensors/DistanceSensor.cs | 20 ++++++++++++++++++
 SiteServer.cs             |  6 +++++-
 Unity/DebugConsole.cs     | 25 +++++++++++++++++++++++
 Unity/SiteServer.cs       | 43 +++++++++++++++++++++++++++++++++++++++
 Unity/Thing.cs            | 37 +++++++++++++++++++++++++++++++++
 test/UnitTest1.cs         |  4 +++-
 6 files changed, 133 insertions(+), 2 deletions(-)
 create mode 100644 Sensors/DistanceSensor.cs
 create mode 100644 Unity/DebugConsole.cs
 create mode 100644 Unity/SiteServer.cs
 create mode 100644 Unity/Thing.cs

diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs
new file mode 100644
index 0000000..f347a4f
--- /dev/null
+++ b/Sensors/DistanceSensor.cs
@@ -0,0 +1,20 @@
+using System;
+
+namespace Passer.Control.Core {
+
+    public class DistanceSensor : Thing {
+        public float distance = 0;
+
+        public DistanceSensor() : base() { }
+
+        public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
+        }
+
+        public override void ProcessBinary(byte[] bytes) {
+            byte ix = 0;
+            this.distance = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
+        }
+
+    }
+
+}
\ No newline at end of file
diff --git a/SiteServer.cs b/SiteServer.cs
index 4172a8e..f44916b 100644
--- a/SiteServer.cs
+++ b/SiteServer.cs
@@ -13,7 +13,7 @@ namespace Passer.Control.Core {
             this.ipAddress = "0.0.0.0";
             this.port = port;
 
-            this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
+            //this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
 
             Console.Write($"Prepare receive on port {port}");
             this.udpClient = new UdpClient(port); // for receiving
@@ -22,6 +22,10 @@ namespace Passer.Control.Core {
                 new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
         }
 
+        public void Close() {
+            this.udpClient.Close();
+        }
+
         public override void Publish() {            
         }
 
diff --git a/Unity/DebugConsole.cs b/Unity/DebugConsole.cs
new file mode 100644
index 0000000..cb4de46
--- /dev/null
+++ b/Unity/DebugConsole.cs
@@ -0,0 +1,25 @@
+#if UNITY_5_3_OR_NEWER
+using System.IO;
+using System.Text;
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+    public class UnityLogWriter : TextWriter {
+        public override void Write(char value) {
+            Debug.Log(value);
+        }
+
+        public override void Write(string value) {
+            Debug.Log(value);
+        }
+
+        public override void WriteLine(string value) {
+            Debug.Log(value);
+        }
+
+        public override Encoding Encoding => Encoding.UTF8;
+    }
+    
+}
+#endif
\ No newline at end of file
diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs
new file mode 100644
index 0000000..cbc987e
--- /dev/null
+++ b/Unity/SiteServer.cs
@@ -0,0 +1,43 @@
+#if UNITY_5_3_OR_NEWER
+using System;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+    public class SiteServer : MonoBehaviour {
+        public Core.SiteServer site;
+
+        public Queue<Core.Thing> thingQueue = new();
+
+        protected virtual void Awake() {
+            Console.SetOut(new UnityLogWriter());
+
+            site = new(7681);
+            Core.Thing.OnNewThing += HandleNewThing;
+        }
+
+        void OnApplicationQuit() {
+            site.Close();
+        }
+
+        public void HandleNewThing(Core.Thing thing) {
+            // if (thing.participant == null) {
+            //     Console.WriteLine($"new thing without participant [{thing.networkId}/{thing.id}] {thing.name}");
+            //     this.site.Add(thing, false);
+            // }
+
+            thingQueue.Enqueue(thing);
+        }
+
+        protected virtual void Update() {
+            site.Update((ulong)(Time.time * 1000));
+            if (thingQueue.TryDequeue(out Core.Thing thing)) {
+                thing.CreateComponent();
+            }
+        }
+
+    }
+
+}
+#endif
\ No newline at end of file
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
new file mode 100644
index 0000000..1fd9b0e
--- /dev/null
+++ b/Unity/Thing.cs
@@ -0,0 +1,37 @@
+#if UNITY_5_3_OR_NEWER
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+    public class Thing : MonoBehaviour {
+
+        protected Core.Thing core;
+
+        protected void CreateThing(Core.Thing thing) {
+            SiteServer siteServer = FindAnyObjectByType<SiteServer>();
+            if (siteServer == null) {
+                Debug.LogWarning("No site server found");
+                return;
+            }
+
+            core = thing;
+            siteServer.site.Add(thing);
+        }
+
+        protected virtual void Update() {
+            if (core == null)
+                return;
+
+            if (core.linearVelocity != null) {
+                Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
+                this.transform.Translate(direction * core.linearVelocity.distance * Time.deltaTime);
+            }
+            if (core.angularVelocity != null) {
+                Vector3 axis = Quaternion.AngleAxis(core.angularVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
+                this.transform.Rotate(axis, core.angularVelocity.distance * Time.deltaTime);
+            }
+        }
+    }
+
+}
+#endif
\ No newline at end of file
diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs
index adeb0c0..00fd906 100644
--- a/test/UnitTest1.cs
+++ b/test/UnitTest1.cs
@@ -1,4 +1,5 @@
-using System;
+#if !UNITY_5_3_OR_NEWER
+using System;
 using System.Threading;
 using NUnit.Framework;
 
@@ -83,3 +84,4 @@ namespace ControlCore.test {
         }
     }
 }
+#endif
\ No newline at end of file