feat(OpenGloves): add decoder for FFB

lib/opengloves/opengloves/opengloves.cpp

@@ -1,8 +1,10 @@
 #include "opengloves/opengloves.hpp"
 
+#include <cctype>
 #include <cmath>
 #include <cstdint>
 #include <cstdio>
+#include <string>
 #include <variant>
 
 namespace og {
@@ -161,8 +163,105 @@ namespace og {
         return 0;
     }
 
-    auto AlphaEncoder::parse_output(const char* data, size_t length) const -> Output
+    auto AlphaEncoder::decode_output(const char* data, size_t length) const -> OutputData
     {
-        return {};
+        if (length == 0) {
+            return OutputInvalid{};
+        }
+
+        const auto commands = split_to_map(data, length);
+        if (commands.empty()) {
+            return OutputInvalid{};
+        }
+
+        // We assume all commands are for ffb, if there is any ffb command
+        const auto& thumb_curl = commands.find("A");
+        if (thumb_curl != commands.end()) {
+            OutputForceFeedback ffb{};
+
+            ffb.thumb = std::stof(thumb_curl->second) / MAX_ANALOG_VALUE;
+
+            const auto& index_curl = commands.find("B");
+            if (index_curl != commands.end()) {
+                ffb.index = std::stof(index_curl->second) / MAX_ANALOG_VALUE;
+            }
+
+            const auto& middle_curl = commands.find("C");
+            if (middle_curl != commands.end()) {
+                ffb.middle = std::stof(middle_curl->second) / MAX_ANALOG_VALUE;
+            }
+
+            const auto& ring_curl = commands.find("D");
+            if (ring_curl != commands.end()) {
+                ffb.ring = std::stof(ring_curl->second) / MAX_ANALOG_VALUE;
+            }
+
+            const auto& pinky_curl = commands.find("E");
+            if (pinky_curl != commands.end()) {
+                ffb.pinky = std::stof(pinky_curl->second) / MAX_ANALOG_VALUE;
+            }
+
+            return ffb;
+        }
+
+        // const auto& haptics_frequency = commands.find("F");
+        // if (haptics_frequency != commands.end()) {
+        //     OutputHaptics haptics{};
+        //     return haptics;
+        // }
+
+        return OutputInvalid{};
+    }
+
+    /// Splits the input data into a map of commands and their respective values.
+    ///
+    /// Example: `A100(AB)200B300(BB)400C500\n` -> `{"A": "100", "(AB)": "200", "B": "300", "(BB)": "400", "C": "500"}`
+    auto AlphaEncoder::split_to_map(const char* data, size_t length) const -> std::map<std::string, std::string>
+    {
+        std::map<std::string, std::string> result{};
+
+        // Start at the beginning of the data
+        size_t command_start = 0;
+        for (size_t i = 0; i < length; i++) {
+            const auto& current_char = data[i];
+
+            // Start a new command if the character is non-numeric or an opening parenthesis
+            // and previous character is a numeric character
+            const bool is_command_start = ((isdigit(current_char)) == 0) || current_char == '(';
+            const bool prev_is_digit = isdigit(data[i - 1]) != 0;
+            if (is_command_start && i > 0 && prev_is_digit) {
+                split_command(data, command_start, i, result);
+                command_start = i;
+            }
+        }
+
+        // Add the last command
+        split_command(data, command_start, length, result);
+
+        return result;
+    }
+
+    void AlphaEncoder::split_command(
+      const char* data, size_t start, size_t length, std::map<std::string, std::string>& commands
+    ) const
+    {
+        const std::string current_command = std::string(data + start, length - start);
+
+        if (current_command.empty()) {
+            return;
+        }
+
+        const size_t split_index = current_command.find_first_of("0123456789");
+
+        // If there is no numeric value, the command is empty (likely a binary command)
+        if (split_index == std::string::npos) {
+            commands[current_command] = "";
+            return;
+        }
+
+        const std::string command = current_command.substr(0, split_index);
+        const std::string value = current_command.substr(split_index, current_command.length() - split_index);
+
+        commands[command] = value;
     }
 } // namespace og

lib/opengloves/opengloves/opengloves.hpp

@@ -131,7 +131,39 @@ namespace og {
 
     using InputData = std::variant<InputInfoData, InputPeripheralData>;
 
-    class Output {};
+    template<typename Tf = float, typename Tb = bool>
+    struct OutputForceFeedback {
+        Tf thumb;
+        Tf index;
+        Tf middle;
+        Tf ring;
+        Tf pinky;
+
+        auto operator==(const OutputForceFeedback& other) const -> bool
+        {
+            return thumb == other.thumb && index == other.index && middle == other.middle && ring == other.ring
+                   && pinky == other.pinky;
+        }
+    };
+    using OutputForceFeedbackData = OutputForceFeedback<float, bool>;
+
+    template<typename Tf = float, typename Tb = bool>
+    struct OutputHaptics {
+        Tf frequency;
+        Tf duration;
+        Tf amplitude;
+
+        auto operator==(const OutputHaptics& other) const -> bool
+        {
+            return frequency == other.frequency && duration == other.duration && amplitude == other.amplitude;
+        }
+    };
+    using OutputHapticsData = OutputHaptics<float, bool>;
+
+    class OutputInvalid {
+        auto operator==(const OutputInvalid&) const -> bool { return true; }
+    };
+    using OutputData = std::variant<OutputInvalid, OutputForceFeedbackData, OutputHapticsData>;
 
     class IEncoder {
       public:
@@ -149,22 +181,22 @@ namespace og {
             return buffer;
         }
 
-        [[nodiscard]] virtual auto parse_output(const char* data, size_t length) const -> Output = 0;
+        [[nodiscard]] virtual auto decode_output(const char* data, size_t length) const -> OutputData = 0;
 
-        [[nodiscard]] inline auto parse_output(const std::vector<char>& data) const -> Output
+        [[nodiscard]] inline auto decode_output(const std::vector<char>& data) const -> OutputData
         {
-            return this->parse_output(data.data(), data.size());
+            return this->decode_output(data.data(), data.size());
         }
 
-        [[nodiscard]] inline auto parse_output(const std::string& data) const -> Output
+        [[nodiscard]] inline auto decode_output(const std::string& data) const -> OutputData
         {
-            return this->parse_output(data.data(), data.length());
+            return this->decode_output(data.data(), data.length());
         }
 
 #ifdef ARDUINO
-        [[nodiscard]] inline auto parse_output(const String& data) const -> Output
+        [[nodiscard]] inline auto decode_output(const String& data) const -> OutputData
         {
-            return this->parse_output(data.c_str(), data.length());
+            return this->decode_output(data.c_str(), data.length());
         }
 #endif // ARDUINO
     };
@@ -377,6 +409,11 @@ namespace og {
 
         [[nodiscard]] auto encode_input(const InputData& input, char* buffer, size_t length) const -> size_t override;
 
-        [[nodiscard]] auto parse_output(const char* data, size_t length) const -> Output override;
+        [[nodiscard]] auto decode_output(const char* data, size_t length) const -> OutputData override;
+
+      protected:
+        [[nodiscard]] auto split_to_map(const char* data, size_t length) const -> std::map<std::string, std::string>;
+        void split_command(const char* data, size_t start, size_t length, std::map<std::string, std::string>& commands)
+          const;
     };
 } // namespace og

test/test_opengloves_alpha_encoding/main.cpp

@@ -173,11 +173,66 @@ void test_encode_input_peripherals(void)
     }
 }
 
+void test_decode_output_ffb(void)
+{
+    const IEncoder* encoder = new AlphaEncoder();
+
+    std::map<std::string, OutputForceFeedbackData> cases = {
+        {
+          "A0B0C0D0E0\n",
+          OutputForceFeedbackData{
+            .thumb = 0.0f,
+            .index = 0.0f,
+            .middle = 0.0f,
+            .ring = 0.0f,
+            .pinky = 0.0f,
+          },
+        },
+        {
+          "A0\n",
+          OutputForceFeedbackData{
+            .thumb = 0.0f,
+            .index = 0.0f,
+            .middle = 0.0f,
+            .ring = 0.0f,
+            .pinky = 0.0f,
+          },
+        },
+        {
+          "A819B1638C2457D3276E4095\n",
+          OutputForceFeedbackData{
+            .thumb = 0.2f,
+            .index = 0.4f,
+            .middle = 0.6f,
+            .ring = 0.8f,
+            .pinky = 1.0f,
+          },
+        },
+        {
+          "A4095B4095C4095D4095E4095\n",
+          OutputForceFeedbackData{
+            .thumb = 1.0f,
+            .index = 1.0f,
+            .middle = 1.0f,
+            .ring = 1.0f,
+            .pinky = 1.0f,
+          },
+        },
+    };
+
+    for (const auto& [data, expected] : cases) {
+        const auto decoded = encoder->decode_output(data.c_str(), data.size());
+        TEST_ASSERT_TRUE(std::holds_alternative<OutputForceFeedbackData>(decoded));
+        TEST_ASSERT_TRUE(std::get<OutputForceFeedbackData>(decoded) == expected);
+    }
+}
+
 int process(void)
 {
     UNITY_BEGIN();
 
     RUN_TEST(test_encode_input_peripherals);
+    RUN_TEST(test_decode_output_ffb);
 
     return UNITY_END();
 }