add checksum & graycode+triplicate error correction

1 files changed, 100 insertions, 14 deletions

diff --git a/Scripts/DataDecoder.cs b/Scripts/DataDecoder.cs
index ddef18c..96f4a7d 100644
--- a/Scripts/DataDecoder.cs
+++ b/Scripts/DataDecoder.cs
@@ -29,9 +29,17 @@ public class DataDecoder : UdonSharpBehaviour
   {
     if (sourceTexture == null) return;
 
-    // TODO only request as much as we need....
-    int requestWidth = sourceTexture.width;
-    int requestHeight = sourceTexture.height;
+    // Request the rectangular region which leaves either a right-justified
+    // square, or a bottom-justified square.
+    int requestWidth;
+    int requestHeight;
+    if (sourceTexture.width < sourceTexture.height) {
+      requestWidth = sourceTexture.width;
+      requestHeight = sourceTexture.height - sourceTexture.width;
+    } else {
+      requestHeight = sourceTexture.height;
+      requestWidth = sourceTexture.width - sourceTexture.height;
+    }
     int pixelCount = requestWidth * requestHeight;
 
     if (pixelCount <= 0) return;
@@ -70,7 +78,7 @@ public class DataDecoder : UdonSharpBehaviour
   }
 
   // Byte parsing logic
-  private bool HasBytesLeft(ref byte[] b, int offset, int bytesLeft) {
+  private bool HasBytesLeft(byte[] b, int offset, int bytesLeft) {
     return b.Length - offset >= bytesLeft;
   }
 
@@ -86,8 +94,36 @@ public class DataDecoder : UdonSharpBehaviour
     return ret;
   }
 
+  // Convert a two's complement number in the lower 4 bits of a byte to a
+  // gray code in the lower 4 bits.
+  private byte ToGrayNibble(byte twosComplementNibble)
+  {
+    int lowerN = twosComplementNibble & 0x0F;
+    int lowerG = lowerN ^ (lowerN >> 1);
+    return (byte)((twosComplementNibble & 0xF0) | lowerG);
+  }
+
+  // Convert a gray code in the lower 4 bits of a byte to a two's complement
+  // number in the lower 4 bits.
+  private byte ToTwosComplementNibble(byte grayNibble)
+  {
+    int temp = grayNibble & 0x0F;
+    temp ^= (temp >> 1);
+    temp ^= (temp >> 2);
+    temp &= 0x0F;
+    return (byte)((grayNibble & 0xF0) | temp);
+  }
+
   private void ProcessTiles()
   {
+    // Three reserved tiles:
+    //  1. Size. The size of the tiles, in pixels.
+    //  2. Length. The length of the payload, in subpixels.
+    //  3. Checksum. The checksum of the size, length, and first third of the
+    //      payload. The payload is sent in triplicate to allow for some
+    //      forward error correction.
+    const int kNumReservedTiles = 3;
+
     // Get the tile size.
     {
       int oldTileSize = tileSize;
@@ -104,17 +140,23 @@ public class DataDecoder : UdonSharpBehaviour
     int lengthSubpixels = Parse24BitTile(1);
     int lengthTiles = (int) Mathf.Ceil(lengthSubpixels/3.0f);
 
+    // Get the checksum. This covers the size tile, length tile, and first
+    // third of the payload.
+    int checksumRemote = Parse24BitTile(2);
+    int checksumLocal = tileSize + lengthSubpixels;
+
     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}");
+    Debug.Log($"Parsed checksum {checksumRemote}");
 
     // Collect all nibbles into a flat array. Note that these are still
     // encoded.
     int[] nibbles = new int[lengthSubpixels];
     int nibbleCount = 0;
     for (int tile_i = 0; tile_i < lengthTiles; tile_i++) {
-      Color32 parsed_i = GetTileRGB(tile_i+2);
+      Color32 parsed_i = GetTileRGB(tile_i+kNumReservedTiles);
       nibbles[nibbleCount++] = parsed_i.r;
       if (nibbleCount < lengthSubpixels) {
         nibbles[nibbleCount++] = parsed_i.g;
@@ -124,8 +166,54 @@ public class DataDecoder : UdonSharpBehaviour
       }
     }
 
+    // Compute checksum of nibbles. Match behavior in OperatorEncoding ::
+    // Checksum. Note that we only look at the first third of our data,
+    // since data is sent in triplicate.
+    for (int i = 0; i < nibbleCount / 3; i++) {
+      checksumLocal += (nibbles[i] >> 4) & 0x0F;
+    }
+
+    Debug.Log($"Local checksum {checksumLocal}");
+
+    if (checksumLocal != checksumRemote) {
+      Debug.LogWarning($"Checksums don't match. Attempting error recovery.");
+
+      // Data is submitted in triplicate. Perform a bitwise majority vote
+      // with `(a & b) | (a & c) | (b & c)`.
+      int nc3 = nibbleCount / 3;
+      checksumLocal = tileSize + lengthSubpixels;
+      for (int i = 0; i < nibbleCount / 3; i++) {
+        int copy0 = nibbles[i];
+        int copy1 = nibbles[i+nc3];
+        int copy2 = nibbles[i+nc3*2];
+
+        // Convert to gray codes before error correction to (hopefully)
+        // minimize the number of bit flips which must be corrected.
+        byte copy0Gray = ToGrayNibble((byte)((copy0 >> 4) & 0x0F));
+        byte copy1Gray = ToGrayNibble((byte)((copy1 >> 4) & 0x0F));
+        byte copy2Gray = ToGrayNibble((byte)((copy2 >> 4) & 0x0F));
+
+        int resolvedGray =
+          (copy0Gray & copy1Gray) |
+          (copy0Gray & copy2Gray) |
+          (copy1Gray & copy2Gray);
+
+        byte resolved = ToTwosComplementNibble((byte)resolvedGray);
+        nibbles[i] = (resolved << 4) & 0xF0;
+
+        checksumLocal += (nibbles[i] >> 4) & 0x0F;
+      }
+
+      // Check result
+      if (checksumLocal != checksumRemote) {
+        Debug.LogError($"Checksums still don't match after recovery: " +
+            $"{checksumRemote} vs {checksumLocal}. Bailing out.");
+        return;
+      }
+    }
+
     // Convert nibbles to bytes.
-    int byteCount = nibbleCount / 2;
+    int byteCount = nibbleCount / 6;
     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.
@@ -134,16 +222,16 @@ public class DataDecoder : UdonSharpBehaviour
     Debug.Log($"Parsed {bytes.Length} bytes from {nibbles.Length} subpixels");
 
     int bOff = 0;
-    while (HasBytesLeft(ref bytes, bOff, 8)) {
+    while (HasBytesLeft(bytes, bOff, 8)) {
       int type = GetInt(ref bytes, ref bOff);
       int length = GetInt(ref bytes, ref bOff);
-
+      // Can't descend into value if there's not enough length....
+      if (!HasBytesLeft(bytes, bOff, length)) {
+        break;
+      }
       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}");
@@ -151,8 +239,6 @@ public class DataDecoder : UdonSharpBehaviour
           }
       }
     }
-
-    int tilesPerColumn = (int) Mathf.Floor(readHeight / tileSize);
   }
 
   private int Parse24BitTile(int tileIdx)
@@ -173,7 +259,7 @@ public class DataDecoder : UdonSharpBehaviour
   {
     // Calculate which column and position within column this tile is in
     int tilesPerColumn = readHeight / tileSize;
-    int column = tileIdx / tilesPerColumn;
+    int column = tileIdx / Math.Max(1, tilesPerColumn);
     int tileInColumn = tileIdx % tilesPerColumn;
 
     // Calculate Y position (vertical position within column)