1 files changed, 72 insertions, 17 deletions

diff --git a/src/input_common/drivers/touch_screen.cpp b/src/input_common/drivers/touch_screen.cpp
index 8acbe4584..1753e0893 100644
--- a/src/input_common/drivers/touch_screen.cpp
+++ b/src/input_common/drivers/touch_screen.cpp
@@ -14,38 +14,93 @@ constexpr PadIdentifier identifier = {
 
 TouchScreen::TouchScreen(std::string input_engine_) : InputEngine(std::move(input_engine_)) {
     PreSetController(identifier);
+    ReleaseAllTouch();
 }
 
-void TouchScreen::TouchMoved(float x, float y, std::size_t finger) {
-    if (finger >= 16) {
+void TouchScreen::TouchMoved(float x, float y, std::size_t finger_id) {
+    const auto index = GetIndexFromFingerId(finger_id);
+    if (!index) {
+        // Touch doesn't exist handle it as a new one
+        TouchPressed(x, y, finger_id);
         return;
     }
-    TouchPressed(x, y, finger);
+    const auto i = index.value();
+    fingers[i].is_active = true;
+    SetButton(identifier, static_cast<int>(i), true);
+    SetAxis(identifier, static_cast<int>(i * 2), x);
+    SetAxis(identifier, static_cast<int>(i * 2 + 1), y);
 }
 
-void TouchScreen::TouchPressed(float x, float y, std::size_t finger) {
-    if (finger >= 16) {
+void TouchScreen::TouchPressed(float x, float y, std::size_t finger_id) {
+    if (GetIndexFromFingerId(finger_id)) {
+        // Touch already exist. Just update the data
+        TouchMoved(x, y, finger_id);
         return;
     }
-    SetButton(identifier, static_cast<int>(finger), true);
-    SetAxis(identifier, static_cast<int>(finger * 2), x);
-    SetAxis(identifier, static_cast<int>(finger * 2 + 1), y);
+    const auto index = GetNextFreeIndex();
+    if (!index) {
+        // No free entries. Ignore input
+        return;
+    }
+    const auto i = index.value();
+    fingers[i].is_enabled = true;
+    fingers[i].finger_id = finger_id;
+    TouchMoved(x, y, finger_id);
 }
 
-void TouchScreen::TouchReleased(std::size_t finger) {
-    if (finger >= 16) {
+void TouchScreen::TouchReleased(std::size_t finger_id) {
+    const auto index = GetIndexFromFingerId(finger_id);
+    if (!index) {
         return;
     }
-    SetButton(identifier, static_cast<int>(finger), false);
-    SetAxis(identifier, static_cast<int>(finger * 2), 0.0f);
-    SetAxis(identifier, static_cast<int>(finger * 2 + 1), 0.0f);
+    const auto i = index.value();
+    fingers[i].is_enabled = false;
+    SetButton(identifier, static_cast<int>(i), false);
+    SetAxis(identifier, static_cast<int>(i * 2), 0.0f);
+    SetAxis(identifier, static_cast<int>(i * 2 + 1), 0.0f);
+}
+
+std::optional<std::size_t> TouchScreen::GetIndexFromFingerId(std::size_t finger_id) const {
+    for (std::size_t index = 0; index < MAX_FINGER_COUNT; ++index) {
+        const auto& finger = fingers[index];
+        if (!finger.is_enabled) {
+            continue;
+        }
+        if (finger.finger_id == finger_id) {
+            return index;
+        }
+    }
+    return std::nullopt;
+}
+
+std::optional<std::size_t> TouchScreen::GetNextFreeIndex() const {
+    for (std::size_t index = 0; index < MAX_FINGER_COUNT; ++index) {
+        if (!fingers[index].is_enabled) {
+            return index;
+        }
+    }
+    return std::nullopt;
+}
+
+void TouchScreen::ClearActiveFlag() {
+    for (auto& finger : fingers) {
+        finger.is_active = false;
+    }
+}
+
+void TouchScreen::ReleaseInactiveTouch() {
+    for (const auto& finger : fingers) {
+        if (!finger.is_active) {
+            TouchReleased(finger.finger_id);
+        }
+    }
 }
 
 void TouchScreen::ReleaseAllTouch() {
-    for (int index = 0; index < 16; ++index) {
-        SetButton(identifier, index, false);
-        SetAxis(identifier, index * 2, 0.0f);
-        SetAxis(identifier, index * 2 + 1, 0.0f);
+    for (const auto& finger : fingers) {
+        if (finger.is_enabled) {
+            TouchReleased(finger.finger_id);
+        }
     }
 }