2 files changed, 75 insertions, 55 deletions

diff --git a/src/core/hle/service/hid/controllers/touchscreen.cpp b/src/core/hle/service/hid/controllers/touchscreen.cpp
index 13f75b48a..dc0d2c712 100644
--- a/src/core/hle/service/hid/controllers/touchscreen.cpp
+++ b/src/core/hle/service/hid/controllers/touchscreen.cpp
@@ -2,6 +2,7 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include <algorithm>
 #include <cstring>
 #include "common/common_types.h"
 #include "core/core_timing.h"
@@ -16,7 +17,13 @@ constexpr std::size_t SHARED_MEMORY_OFFSET = 0x400;
 Controller_Touchscreen::Controller_Touchscreen(Core::System& system) : ControllerBase(system) {}
 Controller_Touchscreen::~Controller_Touchscreen() = default;
 
-void Controller_Touchscreen::OnInit() {}
+void Controller_Touchscreen::OnInit() {
+    for (size_t id = 0; id < MAX_FINGERS; id++) {
+        mouse_finger_id[id] = MAX_FINGERS;
+        keyboard_finger_id[id] = MAX_FINGERS;
+        udp_finger_id[id] = MAX_FINGERS;
+    }
+}
 
 void Controller_Touchscreen::OnRelease() {}
 
@@ -40,32 +47,35 @@ void Controller_Touchscreen::OnUpdate(const Core::Timing::CoreTiming& core_timin
     cur_entry.sampling_number = last_entry.sampling_number + 1;
     cur_entry.sampling_number2 = cur_entry.sampling_number;
 
-    updateTouchInputEvent(touch_mouse_device->GetStatus(), mouse_finger_id);
-    updateTouchInputEvent(touch_btn_device->GetStatus(), keyboard_finger_id);
-    updateTouchInputEvent(touch_udp_device->GetStatus(), udp_finger_id);
-
-    std::array<Finger, 16> sorted_fingers;
-    size_t active_fingers = 0;
-    for (Finger finger : fingers) {
-        if (finger.pressed) {
-            sorted_fingers[active_fingers++] = finger;
-        }
+    const Input::TouchStatus& mouse_status = touch_mouse_device->GetStatus();
+    const Input::TouchStatus& keyboard_status = touch_btn_device->GetStatus();
+    const Input::TouchStatus& udp_status = touch_udp_device->GetStatus();
+    for (size_t id = 0; id < mouse_status.size(); id++) {
+        mouse_finger_id[id] = UpdateTouchInputEvent(mouse_status[id], mouse_finger_id[id]);
+        keyboard_finger_id[id] = UpdateTouchInputEvent(keyboard_status[id], keyboard_finger_id[id]);
+        udp_finger_id[id] = UpdateTouchInputEvent(udp_status[id], udp_finger_id[id]);
     }
 
+    std::array<Finger, 16> active_fingers;
+    const auto end_iter = std::copy_if(fingers.begin(), fingers.end(), active_fingers.begin(),
+                                       [](const auto& finger) { return finger.pressed; });
+    const auto active_fingers_count =
+        static_cast<size_t>(std::distance(active_fingers.begin(), end_iter));
+
     const u64 tick = core_timing.GetCPUTicks();
-    cur_entry.entry_count = static_cast<s32_le>(active_fingers);
+    cur_entry.entry_count = static_cast<s32_le>(active_fingers_count);
     for (size_t id = 0; id < MAX_FINGERS; id++) {
         auto& touch_entry = cur_entry.states[id];
-        if (id < active_fingers) {
-            touch_entry.x = static_cast<u16>(sorted_fingers[id].x * Layout::ScreenUndocked::Width);
-            touch_entry.y = static_cast<u16>(sorted_fingers[id].y * Layout::ScreenUndocked::Height);
+        if (id < active_fingers_count) {
+            touch_entry.x = static_cast<u16>(active_fingers[id].x * Layout::ScreenUndocked::Width);
+            touch_entry.y = static_cast<u16>(active_fingers[id].y * Layout::ScreenUndocked::Height);
             touch_entry.diameter_x = Settings::values.touchscreen.diameter_x;
             touch_entry.diameter_y = Settings::values.touchscreen.diameter_y;
             touch_entry.rotation_angle = Settings::values.touchscreen.rotation_angle;
-            touch_entry.delta_time = tick - sorted_fingers[id].last_touch;
-            sorted_fingers[id].last_touch = tick;
-            touch_entry.finger = sorted_fingers[id].id;
-            touch_entry.attribute.raw = sorted_fingers[id].attribute.raw;
+            touch_entry.delta_time = tick - active_fingers[id].last_touch;
+            active_fingers[id].last_touch = tick;
+            touch_entry.finger = active_fingers[id].id;
+            touch_entry.attribute.raw = active_fingers[id].attribute.raw;
         } else {
             // Clear touch entry
             touch_entry.attribute.raw = 0;
@@ -91,44 +101,50 @@ void Controller_Touchscreen::OnLoadInputDevices() {
     }
 }
 
-void Controller_Touchscreen::updateTouchInputEvent(
-    const std::tuple<float, float, bool>& touch_input, size_t& finger_id) {
-    bool pressed = false;
-    float x, y;
-    std::tie(x, y, pressed) = touch_input;
+std::optional<size_t> Controller_Touchscreen::GetUnusedFingerID() const {
+    size_t first_free_id = 0;
+    while (first_free_id < MAX_FINGERS) {
+        if (!fingers[first_free_id].pressed) {
+            return first_free_id;
+        } else {
+            first_free_id++;
+        }
+    }
+    return std::nullopt;
+}
+
+size_t Controller_Touchscreen::UpdateTouchInputEvent(
+    const std::tuple<float, float, bool>& touch_input, size_t finger_id) {
+    const auto& [x, y, pressed] = touch_input;
     if (pressed) {
-        if (finger_id == -1) {
-            int first_free_id = 0;
-            int found = false;
-            while (!found && first_free_id < MAX_FINGERS) {
-                if (!fingers[first_free_id].pressed) {
-                    found = true;
-                } else {
-                    first_free_id++;
-                }
+        Attributes attribute = {};
+        if (finger_id == MAX_FINGERS) {
+            const auto first_free_id = GetUnusedFingerID();
+            if (!first_free_id) {
+                // Invalid finger id do nothing
+                return MAX_FINGERS;
             }
-            if (found) {
-                finger_id = first_free_id;
-                fingers[finger_id].x = x;
-                fingers[finger_id].y = y;
-                fingers[finger_id].pressed = true;
-                fingers[finger_id].id = static_cast<u32_le>(finger_id);
-                fingers[finger_id].attribute.start_touch.Assign(1);
-            }
-        } else {
-            fingers[finger_id].x = x;
-            fingers[finger_id].y = y;
-            fingers[finger_id].attribute.raw = 0;
+            finger_id = first_free_id.value();
+            fingers[finger_id].pressed = true;
+            fingers[finger_id].id = static_cast<u32_le>(finger_id);
+            attribute.start_touch.Assign(1);
         }
-    } else if (finger_id != -1) {
+        fingers[finger_id].x = x;
+        fingers[finger_id].y = y;
+        fingers[finger_id].attribute = attribute;
+        return finger_id;
+    }
+
+    if (finger_id != MAX_FINGERS) {
         if (!fingers[finger_id].attribute.end_touch) {
             fingers[finger_id].attribute.end_touch.Assign(1);
             fingers[finger_id].attribute.start_touch.Assign(0);
-        } else {
-            fingers[finger_id].pressed = false;
-            finger_id = -1;
+            return finger_id;
         }
+        fingers[finger_id].pressed = false;
     }
+
+    return MAX_FINGERS;
 }
 
 } // namespace Service::HID
diff --git a/src/core/hle/service/hid/controllers/touchscreen.h b/src/core/hle/service/hid/controllers/touchscreen.h
index 03f399344..e39ab89ee 100644
--- a/src/core/hle/service/hid/controllers/touchscreen.h
+++ b/src/core/hle/service/hid/controllers/touchscreen.h
@@ -30,13 +30,17 @@ public:
     void OnLoadInputDevices() override;
 
 private:
+    static constexpr size_t MAX_FINGERS = 16;
+
+    // Returns an unused finger id, if there is no fingers available std::nullopt will be returned
+    std::optional<size_t> GetUnusedFingerID() const;
+
     // If the touch is new it tries to assing a new finger id, if there is no fingers avaliable no
     // changes will be made. Updates the coordinates if the finger id it's already set. If the touch
     // ends delays the output by one frame to set the end_touch flag before finally freeing the
     // finger id
-    void updateTouchInputEvent(const std::tuple<float, float, bool>& touch_input,
-                               size_t& finger_id);
-    static const size_t MAX_FINGERS = 16;
+    size_t UpdateTouchInputEvent(const std::tuple<float, float, bool>& touch_input,
+                                 size_t finger_id);
 
     struct Attributes {
         union {
@@ -88,9 +92,9 @@ private:
     std::unique_ptr<Input::TouchDevice> touch_mouse_device;
     std::unique_ptr<Input::TouchDevice> touch_udp_device;
     std::unique_ptr<Input::TouchDevice> touch_btn_device;
-    size_t mouse_finger_id{-1};
-    size_t keyboard_finger_id{-1};
-    size_t udp_finger_id{-1};
+    std::array<size_t, MAX_FINGERS> mouse_finger_id;
+    std::array<size_t, MAX_FINGERS> keyboard_finger_id;
+    std::array<size_t, MAX_FINGERS> udp_finger_id;
     std::array<Finger, MAX_FINGERS> fingers;
 };
 } // namespace Service::HID