From cbcb9b02b0fb46d042b1f197a4a32b7d9080c311 Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <stenzek@gmail.com>
Date: Sun, 15 Dec 2019 23:22:53 +1000
Subject: [PATCH] AnalogController: Rumble support

---
 src/core/analog_controller.cpp | 69 ++++++++++++++++++++++++++++++----
 src/core/analog_controller.h   | 11 +++++-
 src/core/controller.cpp        | 10 +++++
 src/core/controller.h          |  6 +++
 4 files changed, 88 insertions(+), 8 deletions(-)

diff --git a/src/core/analog_controller.cpp b/src/core/analog_controller.cpp
index c631f4148..7663f32b0 100644
--- a/src/core/analog_controller.cpp
+++ b/src/core/analog_controller.cpp
@@ -18,6 +18,7 @@ ControllerType AnalogController::GetType() const
 void AnalogController::Reset()
 {
   m_analog_mode = false;
+  m_rumble_unlocked = false;
   m_configuration_mode = false;
   m_command_param = 0;
 }
@@ -28,9 +29,19 @@ bool AnalogController::DoState(StateWrapper& sw)
     return false;
 
   sw.Do(&m_analog_mode);
+  sw.Do(&m_rumble_unlocked);
   sw.Do(&m_configuration_mode);
   sw.Do(&m_command_param);
   sw.Do(&m_state);
+
+  MotorState motor_state = m_motor_state;
+  sw.Do(&motor_state);
+
+  if (sw.IsReading())
+  {
+    for (u8 i = 0; i < NUM_MOTORS; i++)
+      SetMotorState(i, motor_state[i]);
+  }
   return true;
 }
 
@@ -58,8 +69,6 @@ void AnalogController::SetAxisState(s32 axis_code, float value)
 void AnalogController::SetAxisState(Axis axis, u8 value)
 {
   m_axis_state[static_cast<u8>(axis)] = value;
-
-  // TODO: Map to buttons in digital mode
 }
 
 void AnalogController::SetButtonState(Button button, bool pressed)
@@ -78,6 +87,17 @@ void AnalogController::SetButtonState(s32 button_code, bool pressed)
   SetButtonState(static_cast<Button>(button_code), pressed);
 }
 
+u32 AnalogController::GetVibrationMotorCount() const
+{
+  return NUM_MOTORS;
+}
+
+float AnalogController::GetVibrationMotorStrength(u32 motor)
+{
+  DebugAssert(motor < NUM_MOTORS);
+  return static_cast<float>(m_motor_state[motor]) * (1.0f / 255.0f);
+}
+
 void AnalogController::ResetTransferState()
 {
   m_state = State::Idle;
@@ -95,6 +115,21 @@ u16 AnalogController::GetID() const
   return m_analog_mode ? ANALOG_MODE_ID : DIGITAL_MODE_ID;
 }
 
+void AnalogController::SetAnalogMode(bool enabled)
+{
+  if (m_analog_mode == enabled)
+    return;
+
+  Log_InfoPrintf("Controller switched to %s mode", enabled ? "analog" : "digital");
+  m_analog_mode = enabled;
+}
+
+void AnalogController::SetMotorState(u8 motor, u8 value)
+{
+  DebugAssert(motor < NUM_MOTORS);
+  m_motor_state[motor] = value;
+}
+
 bool AnalogController::Transfer(const u8 data_in, u8* data_out)
 {
   bool ack;
@@ -185,9 +220,29 @@ bool AnalogController::Transfer(const u8 data_in, u8* data_out)
     break;
 
       ID_STATE_MSB(State::GetStateIDMSB, State::GetStateButtonsLSB);
-      FIXED_REPLY_STATE(State::GetStateButtonsLSB, Truncate8(m_button_state), true, State::GetStateButtonsMSB);
-      FIXED_REPLY_STATE(State::GetStateButtonsMSB, Truncate8(m_button_state >> 8), m_analog_mode,
-                        m_analog_mode ? State::GetStateRightAxisX : State::Idle);
+
+    case State::GetStateButtonsLSB:
+    {
+      if (m_rumble_unlocked)
+        SetMotorState(0, data_in);
+
+      *data_out = Truncate8(m_button_state);
+      m_state = State::GetStateButtonsMSB;
+      ack = true;
+    }
+    break;
+
+    case State::GetStateButtonsMSB:
+    {
+      if (m_rumble_unlocked)
+        SetMotorState(1, data_in);
+
+      *data_out = Truncate8(m_button_state >> 8);
+      m_state = m_analog_mode ? State::GetStateRightAxisX : State::Idle;
+      ack = m_analog_mode;
+    }
+    break;
+
       FIXED_REPLY_STATE(State::GetStateRightAxisX, Truncate8(m_axis_state[static_cast<u8>(Axis::RightX)]), true,
                         State::GetStateRightAxisY);
       FIXED_REPLY_STATE(State::GetStateRightAxisY, Truncate8(m_axis_state[static_cast<u8>(Axis::RightY)]), true,
@@ -225,7 +280,7 @@ bool AnalogController::Transfer(const u8 data_in, u8* data_out)
     {
       Log_DebugPrintf("analog mode sel 0x%02x", data_in);
       if (data_in != 0x00)
-        m_analog_mode = (m_command_param == 0x01);
+        SetAnalogMode(m_command_param == 0x01);
 
       *data_out = 0x00;
       m_state = State::Pad4Bytes;
@@ -271,7 +326,7 @@ bool AnalogController::Transfer(const u8 data_in, u8* data_out)
 
     case State::Command4CMode:
     {
-      m_analog_mode = (data_in != 0x00);
+      SetAnalogMode(data_in != 0x00);
       Log_DebugPrintf("analog mode %s by 0x4c", m_analog_mode ? "enabled" : "disabled");
       *data_out = 0x00;
       m_state = State::Command4C1;
diff --git a/src/core/analog_controller.h b/src/core/analog_controller.h
index da627eec9..1ea78f955 100644
--- a/src/core/analog_controller.h
+++ b/src/core/analog_controller.h
@@ -38,6 +38,8 @@ public:
     Count
   };
 
+  static constexpr u8 NUM_MOTORS = 2;
+
   AnalogController();
   ~AnalogController() override;
 
@@ -61,7 +63,12 @@ public:
   void SetAxisState(Axis axis, u8 value);
   void SetButtonState(Button button, bool pressed);
 
+  u32 GetVibrationMotorCount() const override;
+  float GetVibrationMotorStrength(u32 motor) override;
+
 private:
+  using MotorState = std::array<u8, NUM_MOTORS>;
+
   enum class State : u8
   {
     Idle,
@@ -115,6 +122,8 @@ private:
   };
 
   u16 GetID() const;
+  void SetAnalogMode(bool enabled);
+  void SetMotorState(u8 motor, u8 value);
 
   bool m_analog_mode = false;
   bool m_rumble_unlocked = false;
@@ -126,7 +135,7 @@ private:
   // buttons are active low
   u16 m_button_state = UINT16_C(0xFFFF);
 
-  std::array<u8, 2> m_motor_state{};
+  MotorState m_motor_state{};
 
   State m_state = State::Idle;
 };
diff --git a/src/core/controller.cpp b/src/core/controller.cpp
index 666a440d8..ff23bce0a 100644
--- a/src/core/controller.cpp
+++ b/src/core/controller.cpp
@@ -26,6 +26,16 @@ void Controller::SetAxisState(s32 axis_code, float value) {}
 
 void Controller::SetButtonState(s32 button_code, bool pressed) {}
 
+u32 Controller::GetVibrationMotorCount() const
+{
+  return 0;
+}
+
+float Controller::GetVibrationMotorStrength(u32 motor)
+{
+  return 0.0f;
+}
+
 std::unique_ptr<Controller> Controller::Create(ControllerType type)
 {
   switch (type)
diff --git a/src/core/controller.h b/src/core/controller.h
index 5aea0c3dc..df73c1e6b 100644
--- a/src/core/controller.h
+++ b/src/core/controller.h
@@ -36,6 +36,12 @@ public:
   /// Changes the specified button state.
   virtual void SetButtonState(s32 button_code, bool pressed);
 
+  /// Returns the number of vibration motors.
+  virtual u32 GetVibrationMotorCount() const;
+
+  /// Queries the state of the specified vibration motor. Values are normalized from 0..1.
+  virtual float GetVibrationMotorStrength(u32 motor);
+
   /// Creates a new controller of the specified type.
   static std::unique_ptr<Controller> Create(ControllerType type);