From ee4dd77a2b7bd524d1a9c1aafde2dd8d033af1db Mon Sep 17 00:00:00 2001
From: Cassandra de la Cruz-Munoz <me@cass-dlcm.dev>
Date: Wed, 16 Aug 2023 13:52:35 -0400
Subject: [PATCH] create VmcExtHmdPos class for /VMC/Ext/Hmd/Pos messages

---
 Marionette.cs               | 11 ++---
 VmcMessages/VmcExtHmdPos.cs | 81 +++++++++++++++++++++++++++++++++++++
 2 files changed, 84 insertions(+), 8 deletions(-)
 create mode 100644 VmcMessages/VmcExtHmdPos.cs

diff --git a/Marionette.cs b/Marionette.cs
index aab7867..5101181 100644
--- a/Marionette.cs
+++ b/Marionette.cs
@@ -83,14 +83,9 @@ namespace godotVmcSharp
                 case "/VMC/Ext/Midi/CC/Bit":
                     MidiButton(m.Data);
                     break;
-//                case "/VMC/Ext/Hmd/Pos":
-//                    if (m.Data.Count == 8)
-//                    {
-//                        GD.Print(Transform8(m.Data, m.Address.ToString(), new List<string>{"serial", "p.x", "p.y", "p.z", "q.x", "q.y", "q.z", "q.w"}).ToString());
-//                    } else {
-//                        GD.Print($"Invalid number of arguments for \"{m.Address.ToString()}\". Expected 8, received {m.Data.Count}.");
-//                    }
-//                    break;
+                case "/VMC/Ext/Hmd/Pos":
+                    new VmcExtHmdPos(m);
+                    break;
 //                case "/VMC/Ext/Con/Pos":
 //                    if (m.Data.Count == 8)
 //                    {
diff --git a/VmcMessages/VmcExtHmdPos.cs b/VmcMessages/VmcExtHmdPos.cs
new file mode 100644
index 0000000..798193f
--- /dev/null
+++ b/VmcMessages/VmcExtHmdPos.cs
@@ -0,0 +1,81 @@
+/*
+    godotVmcSharp
+    Copyright (C) 2023  Cassandra de la Cruz-Munoz
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Affero General Public License as published
+    by the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+    */
+
+using Godot;
+using godotOscSharp;
+using System.Collections.Generic;
+
+namespace godotVmcSharp
+{
+    public class VmcExtHmdPos : VmcMessage
+    {
+        public string serial { get; }
+        public Godot.Transform3D transform { get; }
+
+        public VmcExtHmdPos(godotOscSharp.OscMessage m) : base(m.Address)
+        {
+            if (m.Data.Count != 8)
+            {
+                GD.Print($"Invalid number of arguments for {base.addr}. Expected 8 or 14, received {m.Data.Count}.");
+                return;
+            }
+            if (m.Data[0].Type != 's')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "name", 's', m.Data[0].Type));
+                return;
+            }
+            if (m.Data[1].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "p.x", 'f', m.Data[1].Type));
+                return;
+            }
+            if (m.Data[2].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "p.y", 'f', m.Data[2].Type));
+                return;
+            }
+            if (m.Data[3].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "p.z", 'f', m.Data[3].Type));
+                return;
+            }
+            if (m.Data[4].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "q.x", 'f', m.Data[4].Type));
+                return;
+            }
+            if (m.Data[5].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "q.y", 'f', m.Data[5].Type));
+                return;
+            }
+            if (m.Data[6].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "q.z", 'f', m.Data[6].Type));
+                return;
+            }
+            if (m.Data[7].Type != 'f')
+            {
+                GD.Print(InvalidArgumentType.GetErrorString(addr, "q.w", 'f', m.Data[7].Type));
+                return;
+            }
+            serial = (string)m.Data[0].Value;
+            transform = new Godot.Transform3D(new Godot.Basis(new Godot.Quaternion((float)m.Data[4].Value, (float)m.Data[5].Value, (float)m.Data[6].Value, (float)m.Data[7].Value)), new Godot.Vector3((float)m.Data[1].Value, (float)m.Data[2].Value, (float)m.Data[3].Value));
+        }
+    }
+}
\ No newline at end of file