From db46f9cc33ca26b7bd1ad166a1a368396b0a5383 Mon Sep 17 00:00:00 2001
From: ABeltramo <beltramo.ale@gmail.com>
Date: Thu, 14 Mar 2024 17:11:12 +0000
Subject: [PATCH] fix: DualSense touchpad

---
 include/inputtino/input.hpp               |  4 +++-
 src/uhid/include/uhid/protected_types.hpp |  1 +
 src/uhid/include/uhid/ps5.hpp             |  3 ++-
 src/uhid/joypad_ps5.cpp                   | 24 ++++++++++++-----------
 4 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/include/inputtino/input.hpp b/include/inputtino/input.hpp
index 6d890be..681b8f9 100644
--- a/include/inputtino/input.hpp
+++ b/include/inputtino/input.hpp
@@ -359,7 +359,9 @@ class PS5Joypad : public Joypad {
   void set_stick(STICK_POSITION stick_type, short x, short y) override;
   void set_on_rumble(const std::function<void(int low_freq, int high_freq)> &callback);
 
-  void place_finger(int finger_nr, float x, float y);
+  static constexpr int touchpad_width = 1920;
+  static constexpr int touchpad_height = 1080;
+  void place_finger(int finger_nr, uint16_t x, uint16_t y);
   void release_finger(int finger_nr);
 
   enum MOTION_TYPE : uint8_t {
diff --git a/src/uhid/include/uhid/protected_types.hpp b/src/uhid/include/uhid/protected_types.hpp
index f94e66a..f7dc45e 100644
--- a/src/uhid/include/uhid/protected_types.hpp
+++ b/src/uhid/include/uhid/protected_types.hpp
@@ -9,6 +9,7 @@ struct PS5JoypadState {
   std::shared_ptr<uhid::Device> dev;
 
   uhid::dualsense_input_report_usb current_state;
+  uint8_t touch_points_ids[2] = {0};
 
   std::optional<std::function<void(int, int)>> on_rumble = std::nullopt;
   std::optional<std::function<void(int, int, int)>> on_led = std::nullopt;
diff --git a/src/uhid/include/uhid/ps5.hpp b/src/uhid/include/uhid/ps5.hpp
index 74ac258..3a34b04 100644
--- a/src/uhid/include/uhid/ps5.hpp
+++ b/src/uhid/include/uhid/ps5.hpp
@@ -298,7 +298,8 @@ struct dualsense_touch_point {
    * Contact IDs, with highest bit set are 'inactive'
    * and any associated data is then invalid.
    */
-  uint8_t contact;
+  uint8_t id : 7;
+  uint8_t contact : 1;
   uint8_t x_lo;
   uint8_t x_hi : 4, y_lo : 4;
   uint8_t y_hi;
diff --git a/src/uhid/joypad_ps5.cpp b/src/uhid/joypad_ps5.cpp
index 4af546d..b4393ca 100644
--- a/src/uhid/joypad_ps5.cpp
+++ b/src/uhid/joypad_ps5.cpp
@@ -281,19 +281,16 @@ void PS5Joypad::set_on_led(const std::function<void(int, int, int)> &callback) {
   this->_state->on_led = callback;
 }
 
-void PS5Joypad::place_finger(int finger_nr, float x, float y) {
+void PS5Joypad::place_finger(int finger_nr, uint16_t x, uint16_t y) {
   if (finger_nr <= 1) {
-    this->_state->current_state.points[finger_nr].contact = 0x0;
+    this->_state->current_state.points[finger_nr].contact = 0;
+    this->_state->current_state.points[finger_nr].id = this->_state->touch_points_ids[finger_nr] + 1;
 
-    uint8_t *array;
+    this->_state->current_state.points[finger_nr].x_lo = static_cast<uint8_t>(x & 0x00FF);
+    this->_state->current_state.points[finger_nr].x_hi = static_cast<uint8_t>((x & 0xFF00) >> 8);
 
-    array = reinterpret_cast<uint8_t *>(&x);
-    this->_state->current_state.points[finger_nr].x_lo = array[0];
-    this->_state->current_state.points[finger_nr].x_hi = array[3];
-
-    array = reinterpret_cast<uint8_t *>(&y);
-    this->_state->current_state.points[finger_nr].y_lo = array[0];
-    this->_state->current_state.points[finger_nr].y_hi = array[3];
+    this->_state->current_state.points[finger_nr].y_lo = static_cast<uint8_t>(y & 0x00FF);
+    this->_state->current_state.points[finger_nr].y_hi = static_cast<uint8_t>((y & 0xFF00) >> 4);
 
     send_report(*this->_state);
   }
@@ -301,7 +298,12 @@ void PS5Joypad::place_finger(int finger_nr, float x, float y) {
 
 void PS5Joypad::release_finger(int finger_nr) {
   if (finger_nr <= 1) {
-    this->_state->current_state.points[finger_nr].contact = 0xFF;
+    this->_state->touch_points_ids[finger_nr]++;
+    // if it goes above 0x7F we should reset it to 0
+    if (this->_state->touch_points_ids[finger_nr] > 0x7F) {
+      this->_state->touch_points_ids[finger_nr] = 0;
+    }
+    this->_state->current_state.points[finger_nr].contact = 1;
     send_report(*this->_state);
   }
 }