From 50fc14d92d7517c55e2e86961a86b4c23c61a61b Mon Sep 17 00:00:00 2001
From: Sam Hos <sam.hos@hva.nl>
Date: Sat, 5 Oct 2024 15:44:28 +0200
Subject: [PATCH] start parser cleanup

---
 src/C++/Driver/src/KobukiParser.cpp | 105 ++++++++++++++++++++++++++++
 src/C++/Driver/src/KobukiParser.h   |  51 ++++++++++++++
 2 files changed, 156 insertions(+)
 create mode 100644 src/C++/Driver/src/KobukiParser.cpp
 create mode 100644 src/C++/Driver/src/KobukiParser.h

diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp
new file mode 100644
index 0000000..9d7e4eb
--- /dev/null
+++ b/src/C++/Driver/src/KobukiParser.cpp
@@ -0,0 +1,105 @@
+#include "KobukiParser.h"
+
+int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
+    int rtrnvalue = checkChecksum(data);
+    if (rtrnvalue != 0)
+        return -2;
+
+    int checkedValue = 1;
+    while (checkedValue < data[0]) {
+        unsigned char header = data[checkedValue];
+        unsigned char length = data[checkedValue + 1];
+
+        switch (header) {
+            case 0x01:
+                if (length == 0x0F) {
+                    parseBasicData(output, data, checkedValue);
+                }
+                break;
+            case 0x03:
+                if (length == 0x03) {
+                    parseIRSensorData(output, data, checkedValue);
+                }
+                break;
+            case 0x04:
+                if (length == 0x07) {
+                    parseGyroData(output, data, checkedValue);
+                }
+                break;
+            case 0x05:
+                if (length == 0x06) {
+                    parseCliffSensorData(output, data, checkedValue);
+                }
+                break;
+            case 0x06:
+                if (length == 0x02) {
+                    parseWheelCurrentData(output, data, checkedValue);
+                }
+                break;
+            case 0x0A:
+                if (length == 0x04) {
+                    parseHardwareVersionData(output, data, checkedValue);
+                }
+                break;
+            case 0x0B:
+                if (length == 0x04) {
+                    parseFirmwareVersionData(output, data, checkedValue);
+                }
+                break;
+            case 0x0D:
+                if (length == 0x0D) {
+                    parseXYZData(output, data, checkedValue);
+                }
+                break;
+            case 0x10:
+                if (length == 0x10) {
+                    parseDigitalAnalogInputData(output, data, checkedValue);
+                }
+                break;
+            case 0x13:
+                if (length == 0x0C) {
+                    parseUDIDData(output, data, checkedValue);
+                }
+                break;
+            default:
+                checkedValue += length + 2; // Skip unknown data block
+                break;
+        }
+    }
+    return 0;
+}
+
+int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue) {
+    checkedValue += 2; // Skip header and length
+    output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue += 2;
+    output.BumperCenter = data[checkedValue] & 0x02;
+    output.BumperLeft = data[checkedValue] & 0x04;
+    output.BumperRight = data[checkedValue] & 0x01;
+    checkedValue++;
+    output.WheelDropLeft = data[checkedValue] & 0x02;
+    output.WheelDropRight = data[checkedValue] & 0x01;
+    checkedValue++;
+    output.CliffCenter = data[checkedValue] & 0x02;
+    output.CliffLeft = data[checkedValue] & 0x04;
+    output.CliffRight = data[checkedValue] & 0x01;
+    checkedValue++;
+    output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue += 2;
+    output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue += 2;
+    output.PWMleft = data[checkedValue];
+    checkedValue++;
+    output.PWMright = data[checkedValue];
+    checkedValue++;
+    output.ButtonPress = data[checkedValue];
+    checkedValue++;
+    output.Charger = data[checkedValue];
+    checkedValue++;
+    output.Battery = data[checkedValue];
+    checkedValue++;
+    output.overCurrent = data[checkedValue];
+    checkedValue++;
+    return 0;
+}
+
diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiParser.h
new file mode 100644
index 0000000..ed78466
--- /dev/null
+++ b/src/C++/Driver/src/KobukiParser.h
@@ -0,0 +1,51 @@
+#ifndef KOBUKIPARSER_H
+#define KOBUKIPARSER_H
+
+#include <vector>
+
+struct TRawGyroData {
+    int x, y, z;
+};
+
+struct TExtraInfo {
+    int HardwareVersionPatch, HardwareVersionMinor, HardwareVersionMajor;
+    int FirmwareVersionPatch, FirmwareVersionMinor, FirmwareVersionMajor;
+    int UDID0, UDID1, UDID2;
+};
+
+struct TKobukiData {
+    int timestamp, BumperCenter, BumperLeft, BumperRight;
+    int WheelDropLeft, WheelDropRight;
+    int CliffCenter, CliffLeft, CliffRight;
+    int EncoderLeft, EncoderRight;
+    int PWMleft, PWMright;
+    int ButtonPress, Charger, Battery, overCurrent;
+    int IRSensorRight, IRSensorCenter, IRSensorLeft;
+    int GyroAngle, GyroAngleRate;
+    int CliffSensorRight, CliffSensorCenter, CliffSensorLeft;
+    int wheelCurrentLeft, wheelCurrentRight;
+    int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3;
+    int frameId;
+    std::vector<TRawGyroData> gyroData;
+    TExtraInfo extraInfo;
+};
+
+class KobukiParser {
+public:
+    int parseKobukiMessage(TKobukiData &output, unsigned char *data);
+
+private:
+    int checkChecksum(unsigned char *data);
+    int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
+};
+
+#endif // KOBUKIPARSER_H
\ No newline at end of file