Implementing the data parsing approach

I'm receiving data in a very specific format for each data type.
I parse the bytes correctly and hopefully everything works, yo.

src/main.cpp

@@ -92,14 +92,16 @@ void setup() {
   LOG_INFO(F("* Ready for loop"));
 }
 
+const uint16_t PayloadMax = 2056;
+
 void loop(void) {
   uint64_t cur = millis();
 
   Command cmd = CmdUnknown;
-  uint8_t payload[256] = {0};
+  uint8_t payload[PayloadMax] = {0};
 
   if (Serial.available() > 0) {
-    int16_t resp = WalkieTalkie::RecvStream(&cmd, payload, 256);
+    int16_t resp = WalkieTalkie::RecvStream(&cmd, payload, PayloadMax);
     if (resp < 0) {
       LOG_WARN(F("Failed to receive data from serial"));
       return;
@@ -168,9 +170,35 @@ void loop(void) {
         // Packet will be: Data {
         //   [windowData]
         // }
+        LOG_WARN(F("Data Received: %d bytes\n"), resp);
+        
+        char lineBuffer[32]; // Enough for "XX XX XX XX XX XX XX XX "
+        int pos = 0;
+        
+        for (int k = 0; k < resp; k++) {
+            // Write 2 hex digits and a space to our local buffer
+            // %02X ensures leading zeros (e.g., 0A instead of A)
+            pos += sprintf(lineBuffer + pos, "%02X ", payload[k]);
+        
+            // Every 8 bytes OR if it's the very last byte in the payload
+            if ((k + 1) % 8 == 0 || k == resp - 1) {
+                // Send the completed line to the logger
+                // We use LOG_DEBUG or similar so we don't spam [WARN] on every line
+                Logger::Printf(F("%s\n"), lineBuffer);
+                
+                // Reset buffer position for the next line
+                pos = 0;
+                memset(lineBuffer, 0, sizeof(lineBuffer));
+            }
+        }
+        LOG_WARN(F("\n"));
+
+        Data::Parse(payload, resp);
+
+        break;
       }
       default:
-        LOG_WARN(F("Command: `%s` has not been implemented yet."), CommandToStr(cmd));
+        LOG_WARN(F("Unknown Command: `%d` has not been implemented yet."), cmd);
     }
   }
 }

src/methods/methods.cpp

@@ -159,3 +159,105 @@ namespace Window {
     return win.WinID;
   }
 };
+
+namespace Data {
+  uint8_t Parse(uint8_t *payload, size_t payloadSize) {
+    uint16_t curPos = 0;
+
+    for (; curPos < payloadSize;) {
+      // Get the data type from the current position
+      uint8_t type = payload[curPos];
+    
+      switch (type) {
+        case DataTypeUINT8: {
+          uint8_t value = payload[curPos + 1];
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 2;
+          break;
+        }
+        case DataTypeINT8: {
+          int8_t value = (int8_t)payload[curPos + 1];
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 2;
+          break;
+        }
+        case DataTypeUINT16: {
+          uint16_t value;
+          memcpy(&value, payload + curPos + 1, 2);
+
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 3; // 1 (Type) + 2 (Value)
+          break;
+        }
+        case DataTypeINT16: {
+          int16_t value;
+          memcpy(&value, payload + curPos + 1, 2);
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 3;
+          break;
+        }
+        case DataTypeUINT32: {
+          uint32_t value;
+          memcpy(&value, payload + curPos + 1, 4);
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 5; // 1 (Type) + 4 (Value)
+          break;
+        }
+        case DataTypeINT32: {
+          int32_t value;
+          memcpy(&value, payload + curPos + 1, 4);
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 5;
+          break;
+        }
+        case DataTypeUINT64: {
+          uint64_t value;
+          memcpy(&value, payload + curPos + 1, 8);
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 9; // 1 (Type) + 8 (Value)
+          break;
+        }
+        case DataTypeINT64: {
+          int64_t value;
+          memcpy(&value, payload + curPos + 1, 8);
+          LOG_WARN(F("RECEIVED: %d\n"), value);
+
+          curPos += 9;
+          break;
+        }
+        case DataTypeCHAR: {
+          uint8_t value = payload[curPos + 1];
+          LOG_WARN(F("RECEIVED: %c\n"), value);
+
+          curPos += 2;
+          break;
+        }
+        case DataTypeSTRING: {
+          // Strings have: Type (1), Len (1), Data (Len)
+          uint8_t len = payload[curPos + 1];
+          char strValue[len + 1];
+          memcpy(strValue, payload + curPos + 2, len);
+          strValue[len] = '\0'; // Null terminator
+                                //
+          LOG_WARN(F("RECEIVED [%d]: %s\n"), len, strValue);
+          
+          curPos += (2 + len); 
+          break;
+        }
+        default:
+          // If we hit an unknown type, we are desynced. 
+          // Better to stop than to read garbage.
+          LOG_ERROR(F("Unknown Type 0x%02X at pos %d"), type, curPos);
+      }
+    }
+
+    return 0;
+  }
+}

src/methods/methods.h

@@ -70,4 +70,20 @@ namespace Screen {
   };
 }
 
+namespace Data {
+  typedef uint8_t DataType;
+  const DataType DataTypeUINT8 = 0;
+  const DataType DataTypeINT8 = 1;
+  const DataType DataTypeUINT16 = 2;
+  const DataType DataTypeINT16 = 3;
+  const DataType DataTypeUINT32 = 4;
+  const DataType DataTypeINT32 = 5;
+  const DataType DataTypeUINT64 = 6;
+  const DataType DataTypeINT64 = 7;
+  const DataType DataTypeSTRING = 8;
+  const DataType DataTypeCHAR = 9;
+
+  uint8_t Parse(uint8_t *payload, size_t payloadSize);
+}
+
 #endif