1 files changed, 70 insertions, 29 deletions

diff --git a/Scripts/DataDecoder.cs b/Scripts/DataDecoder.cs
index 1128e2f..b71f963 100644
--- a/Scripts/DataDecoder.cs
+++ b/Scripts/DataDecoder.cs
@@ -1,3 +1,5 @@
+using System;
+using System.Runtime.InteropServices;
 using System.Collections.Generic;
 using UdonSharp;
 using UnityEngine;
@@ -18,13 +20,17 @@ public class DataDecoder : UdonSharpBehaviour
   private int readWidth;
   private int readHeight;
 
+  // Top-level data types.
+  private const int kT_TimeSyncData = 0;
+
   void Start() {}
 
   void Update()
   {
     if (sourceTexture == null) return;
 
-    int requestWidth = Mathf.Min(tileSize, sourceTexture.width);
+    // TODO only request as much as we need....
+    int requestWidth = sourceTexture.width;
     int requestHeight = sourceTexture.height;
     int pixelCount = requestWidth * requestHeight;
 
@@ -63,6 +69,23 @@ public class DataDecoder : UdonSharpBehaviour
     }
   }
 
+  // Byte parsing logic
+  private bool HasBytesLeft(ref byte[] b, int offset, int bytesLeft) {
+    return b.Length - offset >= bytesLeft;
+  }
+
+  private int GetInt(ref byte[] b, ref int offset) {
+    int ret = BitConverter.ToInt32(b, offset);
+    offset += 4;
+    return ret;
+  }
+
+  private float GetFloat(ref byte[] b, ref int offset) {
+    float ret = BitConverter.ToSingle(b, offset);
+    offset += 4;
+    return ret;
+  }
+
   private void ProcessTiles()
   {
     // Get the tile size.
@@ -81,39 +104,64 @@ public class DataDecoder : UdonSharpBehaviour
     int lengthSubpixels = Parse24BitTile(1);
     int lengthTiles = (int) Mathf.Ceil(lengthSubpixels/3.0f);
 
+    Color32 parsed_first = GetTileRGB(0);
+    Debug.Log($"First tile: {parsed_first.r} {parsed_first.g} {parsed_first.b}");
+    Debug.Log($"Parsed size {tileSize}");
+    Debug.Log($"Parsed length {lengthSubpixels}");
+
     // Collect all nibbles into a flat array. Note that these are still
     // encoded.
-    var nibbles = new List<int>(lengthSubpixels);
+    int[] nibbles = new int[lengthSubpixels];
+    int nibbleCount = 0;
     for (int tile_i = 0; tile_i < lengthTiles; tile_i++) {
-      GetTileRGB(tile_i+2, out int r, out int g, out int b);
-      nibbles.Add(r);
-      if (nibbles.Count < nibbles.Capacity) {
-        nibbles.Add(g);
+      Color32 parsed_i = GetTileRGB(tile_i+2);
+      nibbles[nibbleCount++] = parsed_i.r;
+      if (nibbleCount < lengthSubpixels) {
+        nibbles[nibbleCount++] = parsed_i.g;
       }
-      if (nibbles.Count < nibbles.Capacity) {
-        nibbles.Add(b);
+      if (nibbleCount < lengthSubpixels) {
+        nibbles[nibbleCount++] = parsed_i.b;
       }
     }
 
     // Convert nibbles to bytes.
-    var bytes = nibbles;
-    for (int i = 0; i < nibbles.Count/2; i++) {
+    int byteCount = nibbleCount / 2;
+    byte[] bytes = new byte[byteCount];
+    for (int i = 0; i < byteCount; i++) {
       // See DataEncoder.cs. It puts the upper 4 bits before the lower 4 bits.
-      bytes[i] = (nibbles[2*i] & 0xF0) | ((nibbles[2*i+1] & 0xF0) >> 4);
+      bytes[i] = (byte) ((nibbles[2*i] & 0xF0) | ((nibbles[2*i+1] & 0xF0) >> 4));
+    }
+    Debug.Log($"Parsed {bytes.Length} bytes from {nibbles.Length} subpixels");
+
+    int bOff = 0;
+    while (HasBytesLeft(ref bytes, bOff, 8)) {
+      int type = GetInt(ref bytes, ref bOff);
+      int length = GetInt(ref bytes, ref bOff);
+
+      switch (type) {
+        case kT_TimeSyncData:
+          {
+            if (!HasBytesLeft(ref bytes, bOff, 8)) {
+              break;
+            }
+            float lastSyncTimeMs = GetFloat(ref bytes, ref bOff);
+            float measureTimeUs = GetFloat(ref bytes, ref bOff);
+            Debug.Log($"Parsed time sync data: {lastSyncTimeMs} {measureTimeUs}");
+            break;
+          }
+      }
     }
-    // Remove second half of list.
-    bytes.RemoveRange(nibbles.Count/2, nibbles.Count/2);
 
     int tilesPerColumn = (int) Mathf.Floor(readHeight / tileSize);
   }
 
   private int Parse24BitTile(int tileIdx)
   {
-    GetTileRGB(tileIdx, out int r, out int g, out int b);
+    Color32 parsed = GetTileRGB(tileIdx);
     int data = 0;
-    data |= DecodeNibble(r);
-    data |= DecodeNibble(g) << 4;
-    data |= DecodeNibble(b) << 8;
+    data |= DecodeNibble(parsed.r);
+    data |= DecodeNibble(parsed.g) << 4;
+    data |= DecodeNibble(parsed.b) << 8;
     return data;
   }
 
@@ -121,26 +169,19 @@ public class DataDecoder : UdonSharpBehaviour
     return (subpixel >> 4) & 0x0F;
   }
 
-  private void GetTileRGB(int tileIdx, out int r, out int g, out int b)
+  private Color32 GetTileRGB(int tileIdx)
   {
-    r = 0;
-    g = 0;
-    b = 0;
-
     int tileY = tileIdx * tileSize;
     int centerY = tileY + tileSize / 2;
 
-    if (centerY >= readHeight) return;
+    if (centerY >= readHeight) return new Color32();
 
-    int localX = readWidth / 2;
+    int localX = tileSize / 2;
     int localY = readHeight - 1 - centerY;
     int index = localY * readWidth + localX;
 
-    if (index < 0 || index >= pixelData.Length) return;
+    if (index < 0 || index >= pixelData.Length) return new Color32();
 
-    Color32 c = pixelData[index];
-    r = c.r;
-    g = c.g;
-    b = c.b;
+    return pixelData[index];
   }
 }