From 38fdf9109256aaa64bcead412e0a5bee6850dd68 Mon Sep 17 00:00:00 2001
From: Joel Wetzell <jwetzell@yahoo.com>
Date: Sat, 15 Mar 2025 08:40:50 -0500
Subject: [PATCH] add decoder for tracked point position submodule

---
 src/decoders/index.ts                         |  2 ++
 src/decoders/motion/tracked-point-position.ts | 33 +++++++++++++++++++
 src/models.ts                                 | 10 ++++++
 tests/decode.test.js                          | 17 ++++++++++
 4 files changed, 62 insertions(+)
 create mode 100644 src/decoders/motion/tracked-point-position.ts

diff --git a/src/decoders/index.ts b/src/decoders/index.ts
index 8138cde..fba0161 100644
--- a/src/decoders/index.ts
+++ b/src/decoders/index.ts
@@ -1,11 +1,13 @@
 import CentroidPosition from './motion/centroid-position';
 import OrientationEuler from './motion/orientation-euler';
 import OrientationQuaternion from './motion/orientation-quaternion';
+import TrackedPointPosition from './motion/tracked-point-position';
 import RTTrPHeader from './rttrp';
 
 export const Decoders = {
   RTTrPHeader,
   CentroidPosition,
+  TrackedPointPosition,
   OrientationQuaternion,
   OrientationEuler,
 };
diff --git a/src/decoders/motion/tracked-point-position.ts b/src/decoders/motion/tracked-point-position.ts
new file mode 100644
index 0000000..cc74d64
--- /dev/null
+++ b/src/decoders/motion/tracked-point-position.ts
@@ -0,0 +1,33 @@
+import { TrackedPointPosition } from '../../models';
+
+export default (bytes: Uint8Array, isLittleEndian: boolean = false): TrackedPointPosition => {
+  if (bytes.length !== 30) {
+    throw new Error('Tracked Point Position module must be 30 bytes');
+  }
+
+  const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength);
+
+  const type = view.getUint8(0);
+  if (type !== 0x06) {
+    throw new Error('Tracked Point Position module must have a type of 0x06');
+  }
+
+  const size = view.getUint16(1, isLittleEndian);
+  if (size !== 30) {
+    throw new Error('Tracked Point Position module must be 30 bytes');
+  }
+  const latency = view.getUint16(3, isLittleEndian);
+  const x = view.getFloat64(5, isLittleEndian);
+  const y = view.getFloat64(13, isLittleEndian);
+  const z = view.getFloat64(21, isLittleEndian);
+  const index = view.getUint8(29);
+  return {
+    type,
+    size,
+    latency,
+    x,
+    y,
+    z,
+    index,
+  };
+};
diff --git a/src/models.ts b/src/models.ts
index 6e22e46..e19abdb 100644
--- a/src/models.ts
+++ b/src/models.ts
@@ -19,6 +19,16 @@ export type CentroidPosition = {
 };
 
 
+export type TrackedPointPosition = {
+  type: number;
+  size: number;
+  latency: number;
+  x: number;
+  y: number;
+  z: number;
+  index: number;
+};
+
 export type OrientationQuaternion = {
   type: number;
   size: number;
diff --git a/tests/decode.test.js b/tests/decode.test.js
index e8522a7..4fc98a3 100644
--- a/tests/decode.test.js
+++ b/tests/decode.test.js
@@ -35,6 +35,23 @@ const goodTests = [
     },
     decoder: Decoders.CentroidPosition,
   },
+  {
+    description: 'Tracked Point Position',
+    bytes: new Uint8Array([
+      0x06, 0x00, 0x1e, 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, 0x01,
+    ]),
+    expected: {
+      type: 0x06,
+      size: 0x1e,
+      latency: 16,
+      x: 1.0,
+      y: 2.0,
+      z: 3.0,
+      index: 1,
+    },
+    decoder: Decoders.TrackedPointPosition,
+  },
   {
     description: 'Orientation (Quaternion)',
     bytes: new Uint8Array([