From 122b546ba21077f2f10a252fdf06a0e028536cfd Mon Sep 17 00:00:00 2001
From: Joel Wetzell <jwetzell@yahoo.com>
Date: Sat, 15 Mar 2025 08:39:27 -0500
Subject: [PATCH] add decoder for orientation submodules

---
 src/decoders/index.ts                         |  4 +++
 src/decoders/motion/orientation-euler.ts      | 33 ++++++++++++++++++
 src/decoders/motion/orientation-quaternion.ts | 33 ++++++++++++++++++
 src/models.ts                                 | 19 +++++++++++
 tests/decode.test.js                          | 34 +++++++++++++++++++
 5 files changed, 123 insertions(+)
 create mode 100644 src/decoders/motion/orientation-euler.ts
 create mode 100644 src/decoders/motion/orientation-quaternion.ts

diff --git a/src/decoders/index.ts b/src/decoders/index.ts
index 9f6d651..8138cde 100644
--- a/src/decoders/index.ts
+++ b/src/decoders/index.ts
@@ -1,7 +1,11 @@
 import CentroidPosition from './motion/centroid-position';
+import OrientationEuler from './motion/orientation-euler';
+import OrientationQuaternion from './motion/orientation-quaternion';
 import RTTrPHeader from './rttrp';
 
 export const Decoders = {
   RTTrPHeader,
   CentroidPosition,
+  OrientationQuaternion,
+  OrientationEuler,
 };
diff --git a/src/decoders/motion/orientation-euler.ts b/src/decoders/motion/orientation-euler.ts
new file mode 100644
index 0000000..bd67dc9
--- /dev/null
+++ b/src/decoders/motion/orientation-euler.ts
@@ -0,0 +1,33 @@
+import { OrientationEuler } from '../../models';
+
+export default (bytes: Uint8Array, isLittleEndian: boolean = false): OrientationEuler => {
+  if (bytes.length !== 31) {
+    throw new Error('Orientation (Euler) module must be 31 bytes');
+  }
+
+  const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength);
+
+  const type = view.getUint8(0);
+  if (type !== 0x04) {
+    throw new Error('Orientation (Euler) module must have a type of 0x04');
+  }
+
+  const size = view.getUint16(1, isLittleEndian);
+  if (size !== 31) {
+    throw new Error('Orientation (Euler) module must be 31 bytes');
+  }
+  const latency = view.getUint16(3, isLittleEndian);
+  const order = view.getUint16(5, isLittleEndian);
+  const r1 = view.getFloat64(7, isLittleEndian);
+  const r2 = view.getFloat64(15, isLittleEndian);
+  const r3 = view.getFloat64(23, isLittleEndian);
+  return {
+    type,
+    size,
+    latency,
+    order,
+    r1,
+    r2,
+    r3,
+  };
+};
diff --git a/src/decoders/motion/orientation-quaternion.ts b/src/decoders/motion/orientation-quaternion.ts
new file mode 100644
index 0000000..c5c88e3
--- /dev/null
+++ b/src/decoders/motion/orientation-quaternion.ts
@@ -0,0 +1,33 @@
+import { OrientationQuaternion } from '../../models';
+
+export default (bytes: Uint8Array, isLittleEndian: boolean = false): OrientationQuaternion => {
+  if (bytes.length !== 37) {
+    throw new Error('Orientation (Quaternion) module must be 37 bytes');
+  }
+
+  const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength);
+
+  const type = view.getUint8(0);
+  if (type !== 0x03) {
+    throw new Error('Orientation (Quaternion) module must have a type of 0x03');
+  }
+
+  const size = view.getUint16(1, isLittleEndian);
+  if (size !== 37) {
+    throw new Error('Orientation (Quaternion) module must be 37 bytes');
+  }
+  const latency = view.getUint16(3, isLittleEndian);
+  const Qx = view.getFloat64(5, isLittleEndian);
+  const Qy = view.getFloat64(13, isLittleEndian);
+  const Qz = view.getFloat64(21, isLittleEndian);
+  const Qw = view.getFloat64(29, isLittleEndian);
+  return {
+    type,
+    size,
+    latency,
+    Qx,
+    Qy,
+    Qz,
+    Qw,
+  };
+};
diff --git a/src/models.ts b/src/models.ts
index d6b2a3b..6e22e46 100644
--- a/src/models.ts
+++ b/src/models.ts
@@ -18,4 +18,23 @@ export type CentroidPosition = {
   z: number;
 };
 
+
+export type OrientationQuaternion = {
+  type: number;
+  size: number;
+  latency: number;
+  Qx: number;
+  Qy: number;
+  Qz: number;
+  Qw: number;
+};
+
+export type OrientationEuler = {
+  type: number;
+  size: number;
+  latency: number;
+  order: number;
+  r1: number;
+  r2: number;
+  r3: number;
 };
diff --git a/tests/decode.test.js b/tests/decode.test.js
index 0d3beb3..e8522a7 100644
--- a/tests/decode.test.js
+++ b/tests/decode.test.js
@@ -35,6 +35,40 @@ const goodTests = [
     },
     decoder: Decoders.CentroidPosition,
   },
+  {
+    description: 'Orientation (Quaternion)',
+    bytes: new Uint8Array([
+      0x03, 0x00, 37, 0x00, 0x10, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    ]),
+    expected: {
+      type: 0x03,
+      size: 37,
+      latency: 16,
+      Qx: 1.0,
+      Qy: 2.0,
+      Qz: 3.0,
+      Qw: 4.0,
+    },
+    decoder: Decoders.OrientationQuaternion,
+  },
+  {
+    description: 'Orientation (Euler)',
+    bytes: new Uint8Array([
+      0x04, 0x00, 31, 0x00, 0x10, 0x01, 0x23, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    ]),
+    expected: {
+      type: 0x04,
+      size: 31,
+      latency: 16,
+      order: 0x0123,
+      r1: 1.0,
+      r2: 2.0,
+      r3: 3.0,
+    },
+    decoder: Decoders.OrientationEuler,
+  },
 ];
 
 describe('RTTrP Bytes Decoding', () => {