From ec7055b001daadc233f081a07da854b4d29f2574 Mon Sep 17 00:00:00 2001
From: shinyquagsire23 <mtinc2@gmail.com>
Date: Thu, 22 Feb 2018 22:18:28 -0700
Subject: [PATCH] hid: Improve heuristics for P1_AUTO to switch between all
 available controllers

---
 nx/include/switch/services/hid.h |  2 +-
 nx/source/services/hid.c         | 50 +++++++++++++++++++++++++++++---
 2 files changed, 47 insertions(+), 5 deletions(-)

diff --git a/nx/include/switch/services/hid.h b/nx/include/switch/services/hid.h
index 2c0edde7..22beb2d1 100644
--- a/nx/include/switch/services/hid.h
+++ b/nx/include/switch/services/hid.h
@@ -310,7 +310,7 @@ typedef enum
     CONTROLLER_PLAYER_8 = 7,
     CONTROLLER_HANDHELD = 8,
     CONTROLLER_UNKNOWN  = 9,
-    CONTROLLER_P1_AUTO = 10, /// Not an actual HID-sysmodule ID. Only for hidKeys*(). Automatically uses CONTROLLER_PLAYER_1 when connected, otherwise uses CONTROLLER_HANDHELD.
+    CONTROLLER_P1_AUTO = 10, /// Not an actual HID-sysmodule ID. Only for hidKeys*(). Switches the commanding controller based on activity.
 } HidControllerID;
 
 typedef struct touchPosition
diff --git a/nx/source/services/hid.c b/nx/source/services/hid.c
index cad32029..327c0032 100644
--- a/nx/source/services/hid.c
+++ b/nx/source/services/hid.c
@@ -21,11 +21,13 @@ static u64 g_mouseOld, g_mouseHeld, g_mouseDown, g_mouseUp;
 static u64 g_keyboardModOld, g_keyboardModHeld, g_keyboardModDown, g_keyboardModUp;
 static u32 g_keyboardOld[8], g_keyboardHeld[8], g_keyboardDown[8], g_keyboardUp[8];
 static u64 g_controllerOld[10], g_controllerHeld[10], g_controllerDown[10], g_controllerUp[10];
+static u64 g_controllerHeldStartTime[10], g_controllerStickHeldStartTime[10];
 
 static HidControllerLayoutType g_controllerLayout[10];
 static u64 g_touchTimestamp, g_mouseTimestamp, g_keyboardTimestamp, g_controllerTimestamps[10];
 
 static HidControllerID g_controllerP1AutoID;
+static HidControllerID g_controllerP1StickAutoID;
 
 static RwLock g_hidLock;
 
@@ -92,6 +94,9 @@ void hidReset(void)
     for (int i = 0; i < 10; i++)
         g_controllerOld[i] = g_controllerHeld[i] = g_controllerDown[i] = g_controllerUp[i] = 0;
 
+    memset(g_controllerHeldStartTime, 0, sizeof(g_controllerHeldStartTime));
+    memset(g_controllerStickHeldStartTime, 0, sizeof(g_controllerStickHeldStartTime));
+
     for (int i = 0; i < 10; i++)
         g_controllerLayout[i] = LAYOUT_DEFAULT;
 
@@ -100,6 +105,7 @@ void hidReset(void)
         g_controllerTimestamps[i] = 0;
 
     g_controllerP1AutoID = CONTROLLER_HANDHELD;
+    g_controllerP1StickAutoID = CONTROLLER_HANDHELD;
 
     rwlockWriteUnlock(&g_hidLock);
 }
@@ -197,15 +203,51 @@ void hidScanInput(void) {
             g_controllerTimestamps[i] = newInputEntry->timestamp;
 
             g_controllerHeld[i] |= g_controllerEntries[i].buttons;
+
+            if (g_controllerDown[i]) {
+                g_controllerHeldStartTime[i] = g_controllerTimestamps[i];
+            } else if (g_controllerUp[i]) {
+                g_controllerHeldStartTime[i] = 0;
+            }
+
+            bool sticksActive = (g_controllerEntries[i].joysticks[JOYSTICK_LEFT].dx
+                                 || g_controllerEntries[i].joysticks[JOYSTICK_RIGHT].dx
+                                 || g_controllerEntries[i].joysticks[JOYSTICK_LEFT].dy
+                                 || g_controllerEntries[i].joysticks[JOYSTICK_RIGHT].dy);
+            if (sticksActive && !g_controllerStickHeldStartTime[i]) {
+                g_controllerStickHeldStartTime[i] = g_controllerTimestamps[i];
+            } else if (!sticksActive && g_controllerStickHeldStartTime[i]){
+                g_controllerStickHeldStartTime[i] = 0;
+            }
         }
 
         g_controllerDown[i] = (~g_controllerOld[i]) & g_controllerHeld[i];
         g_controllerUp[i] = g_controllerOld[i] & (~g_controllerHeld[i]);
     }
 
-    g_controllerP1AutoID = CONTROLLER_HANDHELD;
-    if (g_controllerEntries[CONTROLLER_PLAYER_1].connectionState & CONTROLLER_STATE_CONNECTED)
-       g_controllerP1AutoID = CONTROLLER_PLAYER_1;
+    // For P1_AUTO, newer inputs > older inputs
+    for (int i = 0; i < 10; i++)
+    {
+        if (g_controllerHeldStartTime[i] > g_controllerHeldStartTime[g_controllerP1AutoID])
+            g_controllerP1AutoID = i;
+    }
+
+    // and older joystick values > newer joystick values
+    bool noCommandingJoystick = true;
+    for (int i = 0; i < 10; i++)
+    {
+        if (!g_controllerStickHeldStartTime[i]) continue;
+        noCommandingJoystick = false;
+
+        if (g_controllerStickHeldStartTime[i] < g_controllerStickHeldStartTime[g_controllerP1StickAutoID]
+            || !g_controllerStickHeldStartTime[g_controllerP1StickAutoID])
+            g_controllerP1StickAutoID = i;
+    }
+
+    // Fall back to the commanding controller if no joysticks are held
+    // Note: The commanding stick controller is what actually gets shown
+    //       in the home menu, album, etc in the lower left corner.
+    if (noCommandingJoystick) g_controllerP1StickAutoID = g_controllerP1AutoID;
 
     rwlockWriteUnlock(&g_hidLock);
 }
@@ -341,7 +383,7 @@ void hidTouchRead(touchPosition *pos, u32 point_id) {
 }
 
 void hidJoystickRead(JoystickPosition *pos, HidControllerID id, HidControllerJoystick stick) {
-    if (id == CONTROLLER_P1_AUTO) return hidJoystickRead(pos, g_controllerP1AutoID, stick);
+    if (id == CONTROLLER_P1_AUTO) return hidJoystickRead(pos, g_controllerP1StickAutoID, stick);
 
     if (pos) {
         if (id < 0 || id > 9 || stick >= JOYSTICK_NUM_STICKS) {