ControllerInterface/Wiimote/Wiimote.cpp

// Copyright 2020 Dolphin Emulator Project
// Licensed under GPLv2+
// Refer to the license.txt file included.

#include "InputCommon/ControllerInterface/Wiimote/Wiimote.h"

#include "Common/BitUtils.h"
#include "Common/Logging/Log.h"
#include "Common/MathUtil.h"
#include "Core/Config/SYSCONFSettings.h"
#include "Core/HW/WiimoteEmu/ExtensionPort.h"
#include "Core/HW/WiimoteEmu/WiimoteEmu.h"
#include "InputCommon/ControllerEmu/ControllerEmu.h"
#include "InputCommon/ControllerInterface/ControllerInterface.h"

namespace ciface::Wiimote
{
static constexpr char SOURCE_NAME[] = "Bluetooth";

static constexpr size_t IR_SENSITIVITY_LEVEL_COUNT = 5;

template <typename T>
class Button final : public Core::Device::Input
{
public:
  Button(const T* value, std::common_type_t<T> mask, std::string name)
      : m_value(*value), m_mask(mask), m_name(std::move(name))
  {
  }

  std::string GetName() const override { return m_name; }

  ControlState GetState() const override { return (m_value & m_mask) != 0; }

private:
  const T& m_value;
  const T m_mask;
  const std::string m_name;
};

// GetState returns value divided by supplied "extent".
template <typename T, bool Detectable>
class GenericInput : public Core::Device::Input
{
public:
  GenericInput(const T* value, std::string name, ControlState extent)
      : m_value(*value), m_name(std::move(name)), m_extent(extent)
  {
  }

  bool IsDetectable() const override { return Detectable; }

  std::string GetName() const override { return m_name; }

  ControlState GetState() const final override { return ControlState(m_value) / m_extent; }

protected:
  const T& m_value;
  const std::string m_name;
  const ControlState m_extent;
};

template <typename T>
using AnalogInput = GenericInput<T, true>;

template <typename T>
using UndetectableAnalogInput = GenericInput<T, false>;

// GetName() is appended with '-' or '+' based on sign of "extent" value.
template <bool Detectable>
class SignedInput final : public GenericInput<float, Detectable>
{
public:
  using GenericInput<float, Detectable>::GenericInput;

  std::string GetName() const override { return this->m_name + (this->m_extent < 0 ? '-' : '+'); }
};

using SignedAnalogInput = SignedInput<true>;
using UndetectableSignedAnalogInput = SignedInput<false>;

class Motor final : public Core::Device::Output
{
public:
  Motor(ControlState* value) : m_value(*value) {}

  std::string GetName() const override { return "Motor"; }

  void SetState(ControlState state) override { m_value = state; }

private:
  ControlState& m_value;
};

template <typename T>
void Device::QueueReport(T&& report, std::function<void(ErrorCode)> ack_callback)
{
  // Maintain proper rumble state.
  report.rumble = m_rumble;

  m_wiimote->QueueReport(report.REPORT_ID, &report, sizeof(report));

  if (ack_callback)
    AddReportHandler(MakeAckHandler(report.REPORT_ID, std::move(ack_callback)));
}

void AddDevice(std::unique_ptr<WiimoteReal::Wiimote> wiimote)
{
  // Our real wiimote class requires an index.
  // Within the pool it's only going to be used for logging purposes.
  static constexpr int CIFACE_WIIMOTE_INDEX = 55;

  if (!wiimote->Connect(CIFACE_WIIMOTE_INDEX))
  {
    WARN_LOG(WIIMOTE, "WiiRemote: Failed to connect.");
    return;
  }

  wiimote->Prepare();
  wiimote->EventLinked();

  g_controller_interface.AddDevice(std::make_shared<Device>(std::move(wiimote)));
}

void ReleaseDevices(std::optional<u32> count)
{
  u32 removed_devices = 0;

  // Remove up to "count" remotes (or all of them if nullopt).
  // Real wiimotes will be added to the pool.
  g_controller_interface.RemoveDevice([&](const Core::Device* device) {
    if (device->GetSource() != SOURCE_NAME || count == removed_devices)
      return false;

    ++removed_devices;
    return true;
  });
}

Device::Device(std::unique_ptr<WiimoteReal::Wiimote> wiimote) : m_wiimote(std::move(wiimote))
{
  using EmuWiimote = WiimoteEmu::Wiimote;

  // Buttons.
  static constexpr std::pair<u16, const char*> button_masks[] = {
      {EmuWiimote::BUTTON_A, "A"},       {EmuWiimote::BUTTON_B, "B"},
      {EmuWiimote::BUTTON_ONE, "1"},     {EmuWiimote::BUTTON_TWO, "2"},
      {EmuWiimote::BUTTON_MINUS, "-"},   {EmuWiimote::BUTTON_PLUS, "+"},
      {EmuWiimote::BUTTON_HOME, "HOME"},
  };

  for (auto& button : button_masks)
    AddInput(new Button<u16>(&m_core_data.hex, button.first, button.second));

  static constexpr u16 dpad_masks[] = {
      EmuWiimote::PAD_UP,
      EmuWiimote::PAD_DOWN,
      EmuWiimote::PAD_LEFT,
      EmuWiimote::PAD_RIGHT,
  };

  // Friendly orientation inputs.
  static constexpr const char* const rotation_names[] = {"Pitch", "Roll", "Yaw"};
  for (std::size_t i = 0; i != std::size(rotation_names); ++i)
  {
    AddInput(
        new UndetectableSignedAnalogInput(&m_rotation_inputs.data[i], rotation_names[i], -1.f));
    AddInput(new UndetectableSignedAnalogInput(&m_rotation_inputs.data[i], rotation_names[i], 1.f));
  }

  // Raw accelerometer.
  for (std::size_t i = 0; i != std::size(dpad_masks); ++i)
    AddInput(new Button<u16>(&m_core_data.hex, dpad_masks[i], named_directions[i]));

  static constexpr std::array<std::array<const char*, 2>, 3> accel_names = {{
      {"Accel Left", "Accel Right"},
      {"Accel Backward", "Accel Forward"},
      {"Accel Up", "Accel Down"},
  }};

  for (std::size_t i = 0; i != m_accel_data.data.size(); ++i)
  {
    AddInput(new UndetectableAnalogInput<float>(&m_accel_data.data[i], accel_names[i][0], 1));
    AddInput(new UndetectableAnalogInput<float>(&m_accel_data.data[i], accel_names[i][1], -1));
  }

  // IR data.
  static constexpr const char* const ir_names[] = {"IR Center X", "IR Center Y"};
  for (std::size_t i = 0; i != std::size(ir_names); ++i)
  {
    AddInput(
        new UndetectableSignedAnalogInput(&m_ir_state.center_position.data[i], ir_names[i], -1.f));
    AddInput(
        new UndetectableSignedAnalogInput(&m_ir_state.center_position.data[i], ir_names[i], 1.f));
  }

  AddInput(new UndetectableAnalogInput<bool>(&m_ir_state.is_hidden, "IR Hidden", 1));

  AddInput(new UndetectableAnalogInput<float>(&m_ir_state.distance, "IR Distance", 1));

  // Raw gyroscope.
  static constexpr std::array<std::array<const char*, 2>, 3> gyro_names = {{
      {"Gyro Pitch Down", "Gyro Pitch Up"},
      {"Gyro Roll Left", "Gyro Roll Right"},
      {"Gyro Yaw Left", "Gyro Yaw Right"},
  }};

  for (std::size_t i = 0; i != m_accel_data.data.size(); ++i)
  {
    AddInput(
        new UndetectableAnalogInput<float>(&m_mplus_state.gyro_data.data[i], gyro_names[i][0], 1));
    AddInput(
        new UndetectableAnalogInput<float>(&m_mplus_state.gyro_data.data[i], gyro_names[i][1], -1));
  }

  using WiimoteEmu::Nunchuk;
  const std::string nunchuk_prefix = "Nunchuk ";

  // Buttons.
  AddInput(new Button<u8>(&m_nunchuk_state.buttons, Nunchuk::BUTTON_C, nunchuk_prefix + "C"));
  AddInput(new Button<u8>(&m_nunchuk_state.buttons, Nunchuk::BUTTON_Z, nunchuk_prefix + "Z"));

  // Stick.
  static constexpr const char* const nunchuk_stick_names[] = {"X", "Y"};
  for (std::size_t i = 0; i != std::size(nunchuk_stick_names); ++i)
  {
    AddInput(new SignedAnalogInput(&m_nunchuk_state.stick.data[i],
                                   nunchuk_prefix + nunchuk_stick_names[i], -1.f));
    AddInput(new SignedAnalogInput(&m_nunchuk_state.stick.data[i],
                                   nunchuk_prefix + nunchuk_stick_names[i], 1.f));
  }

  // Raw accelerometer.
  for (std::size_t i = 0; i != m_accel_data.data.size(); ++i)
  {
    AddInput(new UndetectableAnalogInput<float>(&m_nunchuk_state.accel.data[i],
                                                nunchuk_prefix + accel_names[i][0], 1));
    AddInput(new UndetectableAnalogInput<float>(&m_nunchuk_state.accel.data[i],
                                                nunchuk_prefix + accel_names[i][1], -1));
  }

  using WiimoteEmu::Classic;
  const std::string classic_prefix = "Classic ";

  // Buttons.
  static constexpr u16 classic_dpad_masks[] = {
      Classic::PAD_UP,
      Classic::PAD_DOWN,
      Classic::PAD_LEFT,
      Classic::PAD_RIGHT,
  };

  for (std::size_t i = 0; i != std::size(classic_dpad_masks); ++i)
    AddInput(new Button<u16>(&m_classic_state.buttons, classic_dpad_masks[i],
                             classic_prefix + named_directions[i]));

  static constexpr u16 classic_button_masks[] = {
      Classic::BUTTON_A,     Classic::BUTTON_B,    Classic::BUTTON_X,    Classic::BUTTON_Y,
      Classic::TRIGGER_L,    Classic::TRIGGER_R,   Classic::BUTTON_ZL,   Classic::BUTTON_ZR,
      Classic::BUTTON_MINUS, Classic::BUTTON_PLUS, Classic::BUTTON_HOME,
  };

  static constexpr const char* const classic_button_names[] = {
      "A", "B", "X", "Y", "L", "R", "ZL", "ZR", "-", "+", "HOME",
  };

  for (std::size_t i = 0; i != std::size(classic_button_masks); ++i)
    AddInput(new Button<u16>(&m_classic_state.buttons, classic_button_masks[i],
                             classic_prefix + classic_button_names[i]));

  // Sticks.
  static constexpr const char* const classic_stick_names[][2] = {{"Left X", "Left Y"},
                                                                 {"Right X", "Right Y"}};

  for (std::size_t s = 0; s != std::size(m_classic_state.sticks); ++s)
  {
    for (std::size_t i = 0; i != std::size(m_classic_state.sticks[0].data); ++i)
    {
      AddInput(new SignedAnalogInput(&m_classic_state.sticks[s].data[i],
                                     classic_prefix + classic_stick_names[s][i], -1.f));
      AddInput(new SignedAnalogInput(&m_classic_state.sticks[s].data[i],
                                     classic_prefix + classic_stick_names[s][i], 1.f));
    }
  }

  // Triggers.
  AddInput(new AnalogInput<float>(&m_classic_state.triggers[0], classic_prefix + "L-Analog", 1.f));
  AddInput(new AnalogInput<float>(&m_classic_state.triggers[1], classic_prefix + "R-Analog", 1.f));

  // Specialty inputs:
  AddInput(new UndetectableAnalogInput<float>(&m_battery, "Battery", 1.f));
  AddInput(new UndetectableAnalogInput<WiimoteEmu::ExtensionNumber>(
      &m_extension_number_input, "Attached Extension", WiimoteEmu::ExtensionNumber(1)));
  AddInput(new UndetectableAnalogInput<bool>(&m_mplus_attached_input, "Attached MotionPlus", 1));

  AddOutput(new Motor(&m_rumble_level));
}

Device::~Device()
{
  if (!m_wiimote->IsConnected())
    return;

  m_wiimote->EmuStop();

  INFO_LOG(WIIMOTE, "WiiRemote: Returning remote to pool.");
  WiimoteReal::AddWiimoteToPool(std::move(m_wiimote));
}

std::string Device::GetName() const
{
  return "Wii Remote";
}

std::string Device::GetSource() const
{
  return SOURCE_NAME;
}

void Device::RunTasks()
{
  if (IsPerformingTask())
    return;

  // Request status.
  if (Clock::now() >= m_status_outdated_time)
  {
    QueueReport(OutputReportRequestStatus());

    AddReportHandler(std::function<void(const InputReportStatus& status)>(
        [this](const InputReportStatus& status) {
          DEBUG_LOG(WIIMOTE, "WiiRemote: Received requested status.");
          ProcessStatusReport(status);
        }));

    return;
  }

  // Set LEDs.
  const auto desired_leds = GetDesiredLEDValue();
  if (m_leds != desired_leds)
  {
    OutputReportLeds rpt = {};
    rpt.ack = 1;
    rpt.leds = desired_leds;
    QueueReport(rpt, [this, desired_leds](ErrorCode result) {
      if (result != ErrorCode::Success)
      {
        WARN_LOG(WIIMOTE, "WiiRemote: Failed to set LEDs.");
        return;
      }

      DEBUG_LOG(WIIMOTE, "WiiRemote: Set LEDs.");

      m_leds = desired_leds;
    });

    return;
  }

  // Set reporting mode to one that supports every component.
  static constexpr auto desired_reporting_mode = InputReportID::ReportCoreAccelIR10Ext6;
  if (m_reporting_mode != desired_reporting_mode)
  {
    OutputReportMode mode = {};
    mode.ack = 1;
    mode.mode = desired_reporting_mode;
    QueueReport(mode, [this](ErrorCode error) {
      if (error != ErrorCode::Success)
      {
        WARN_LOG(WIIMOTE, "WiiRemote: Failed to set reporting mode.");
        return;
      }

      m_reporting_mode = desired_reporting_mode;

      DEBUG_LOG(WIIMOTE, "WiiRemote: Set reporting mode.");
    });

    return;
  }

  // Read accelerometer calibration.
  if (!m_accel_calibration.has_value())
  {
    static constexpr u16 ACCEL_CALIBRATION_ADDR = 0x16;

    ReadData(AddressSpace::EEPROM, 0, ACCEL_CALIBRATION_ADDR, sizeof(AccelCalibrationData),
             [this](ReadResponse response) {
               if (!response)
               {
                 WARN_LOG(WIIMOTE, "WiiRemote: Failed to read accelerometer calibration.");
                 return;
               }

               DEBUG_LOG(WIIMOTE, "WiiRemote: Read accelerometer calibration.");

               auto& calibration_data = *response;

               const AccelCalibrationData accel_calibration =
                   Common::BitCastPtr<AccelCalibrationData>(calibration_data.data());
               m_accel_calibration = accel_calibration.GetCalibration();

               WiimoteEmu::UpdateCalibrationDataChecksum(calibration_data, 1);

               // We could potentially try the second block at 0x26 if the checksum is bad.
               if (accel_calibration.checksum != calibration_data.back())
                 WARN_LOG(WIIMOTE, "WiiRemote: Bad accelerometer calibration checksum.");
             });

    return;
  }

  if (!m_ir_state.IsFullyConfigured())
  {
    ConfigureIRCamera();

    return;
  }

  if (!m_speaker_configured)
  {
    ConfigureSpeaker();

    return;
  }

  // Perform the following tasks only after M+ is settled.
  if (IsWaitingForMotionPlus())
    return;

  // Read the "active" extension ID. (This also gives us the current M+ mode)
  // This will fail on an un-intialized other extension.
  // But extension initialization is the same as M+ de-activation so we must try this first.
  if (m_extension_port == true &&
      (!IsMotionPlusStateKnown() || (!IsMotionPlusActive() && !m_extension_id.has_value())))
  {
    static constexpr u16 ENCRYPTION_ADDR = 0xfb;
    static constexpr u8 ENCRYPTION_VALUE = 0x00;

    // First disable encryption. Note this is a no-op when performed on the M+.
    WriteData(AddressSpace::I2CBus, WiimoteEmu::ExtensionPort::REPORT_I2C_SLAVE, ENCRYPTION_ADDR,
              {ENCRYPTION_VALUE}, [this](ErrorCode error) {
                if (error != ErrorCode::Success)
                  return;

                ReadActiveExtensionID();
              });

    return;
  }

  static constexpr u16 INIT_ADDR = 0xf0;
  static constexpr u8 INIT_VALUE = 0x55;

  // Initialize "active" extension if ID was not recognized.
  // Note this is done before M+ setup to determine the required passthrough mode.
  if (m_extension_id == ExtensionID::Unsupported)
  {
    // Note that this signal also DE-activates a M+.
    WriteData(AddressSpace::I2CBus, WiimoteEmu::ExtensionPort::REPORT_I2C_SLAVE, INIT_ADDR,
              {INIT_VALUE}, [this](ErrorCode result) {
                DEBUG_LOG(WIIMOTE, "WiiRemote: Initialized extension: %d.", int(result));

                m_extension_id = std::nullopt;
              });

    return;
  }

  // The following tasks require a known M+ state.
  if (!IsMotionPlusStateKnown())
    return;

  // We now know the status of the M+.
  // Updating it too frequently results off/on flashes on mode change.
  m_mplus_attached_input = IsMotionPlusActive();

  // Extension removal status is known here. Attachment status is updated after the ID is read.
  if (m_extension_port != true)
    m_extension_number_input = WiimoteEmu::ExtensionNumber::NONE;

  // Periodically try to initialize and activate an inactive M+.
  if (!IsMotionPlusActive() && m_mplus_desired_mode.has_value() &&
      m_mplus_state.current_mode != m_mplus_desired_mode)
  {
    static constexpr u16 MPLUS_POLL_ADDR = WiimoteEmu::MotionPlus::PASSTHROUGH_MODE_OFFSET;
    ReadData(AddressSpace::I2CBus, WiimoteEmu::MotionPlus::INACTIVE_DEVICE_ADDR, MPLUS_POLL_ADDR, 1,
             [this](ReadResponse response) {
               if (!response)
               {
                 DEBUG_LOG(WIIMOTE, "WiiRemote: M+ poll failed.");
                 HandleMotionPlusNonResponse();
                 return;
               }

               WriteData(AddressSpace::I2CBus, WiimoteEmu::MotionPlus::INACTIVE_DEVICE_ADDR,
                         INIT_ADDR, {INIT_VALUE}, [this](ErrorCode result) {
                           DEBUG_LOG(WIIMOTE, "WiiRemote: M+ initialization: %d.", int(result));
                           if (result != ErrorCode::Success)
                           {
                             HandleMotionPlusNonResponse();
                             return;
                           }

                           TriggerMotionPlusModeChange();
                         });
             });

    return;
  }

  // Change active M+ passthrough mode.
  if (IsMotionPlusActive() && m_mplus_desired_mode.has_value() &&
      m_mplus_state.current_mode != m_mplus_desired_mode)
  {
    TriggerMotionPlusModeChange();

    return;
  }

  // Read passthrough extension ID.
  // This will also give us a desired M+ passthrough mode.
  if (IsMotionPlusActive() && m_mplus_state.passthrough_port == true && !m_extension_id.has_value())
  {
    // The M+ reads the passthrough ext ID and stores it at 0xf6,f8,f9.
    static constexpr u16 PASSTHROUGH_EXT_ID_ADDR = 0xf6;

    ReadData(AddressSpace::I2CBus, WiimoteEmu::MotionPlus::ACTIVE_DEVICE_ADDR,
             PASSTHROUGH_EXT_ID_ADDR, 4, [this](ReadResponse response) {
               if (!response)
                 return;

               // Port status may have changed since the read was sent.
               // In which case this data read would succeed but be useless.
               if (m_mplus_state.passthrough_port != true)
                 return;

               auto& identifier = *response;

               ProcessExtensionID(identifier[2], identifier[0], identifier[3]);
             });

    return;
  }

  // The following tasks require M+ configuration to be done.
  if (!IsMotionPlusInDesiredMode())
    return;

  // Now that M+ config has settled we can update the extension number.
  // Updating it too frequently results off/on flashes on M+ mode change.
  UpdateExtensionNumberInput();

  static constexpr u16 NORMAL_CALIBRATION_ADDR = 0x20;

  // Read M+ calibration.
  if (IsMotionPlusActive() && !m_mplus_state.calibration.has_value())
  {
    ReadData(AddressSpace::I2CBus, WiimoteEmu::MotionPlus::ACTIVE_DEVICE_ADDR,
             NORMAL_CALIBRATION_ADDR, sizeof(WiimoteEmu::MotionPlus::CalibrationData),
             [this](ReadResponse response) {
               if (!response)
                 return;

               DEBUG_LOG(WIIMOTE, "WiiRemote: Read M+ calibration.");

               WiimoteEmu::MotionPlus::CalibrationData calibration =
                   Common::BitCastPtr<WiimoteEmu::MotionPlus::CalibrationData>(response->data());

               const auto read_checksum = std::pair(calibration.crc32_lsb, calibration.crc32_msb);

               calibration.UpdateChecksum();

               m_mplus_state.SetCalibrationData(calibration);

               if (read_checksum != std::pair(calibration.crc32_lsb, calibration.crc32_msb))
               {
                 // We could potentially try another read or call the M+ unusable.
                 WARN_LOG(WIIMOTE, "WiiRemote: Bad M+ calibration checksum.");
               }
             });

    return;
  }

  // Read normal extension calibration.
  if ((m_extension_id == ExtensionID::Nunchuk && !m_nunchuk_state.calibration) ||
      (m_extension_id == ExtensionID::Classic && !m_classic_state.calibration))
  {
    // Extension calibration is normally at 0x20 but M+ reads and stores it at 0x40.
    static constexpr u16 PASSTHROUGH_CALIBRATION_ADDR = 0x40;

    const u16 calibration_addr =
        IsMotionPlusActive() ? PASSTHROUGH_CALIBRATION_ADDR : NORMAL_CALIBRATION_ADDR;
    static constexpr u16 CALIBRATION_SIZE = 0x10;

    ReadData(
        AddressSpace::I2CBus, WiimoteEmu::ExtensionPort::REPORT_I2C_SLAVE, calibration_addr,
        CALIBRATION_SIZE, [this](ReadResponse response) {
          if (!response)
            return;

          DEBUG_LOG(WIIMOTE, "WiiRemote: Read extension calibration.");

          auto& calibration_data = *response;

          const auto read_checksum = std::pair(calibration_data[CALIBRATION_SIZE - 2],
                                               calibration_data[CALIBRATION_SIZE - 1]);

          WiimoteEmu::UpdateCalibrationDataChecksum(calibration_data, 2);

          Checksum checksum = Checksum::Good;

          if (read_checksum != std::pair(calibration_data[CALIBRATION_SIZE - 2],
                                         calibration_data[CALIBRATION_SIZE - 1]))
          {
            // We could potentially try another block or call the extension unusable.
            WARN_LOG(WIIMOTE, "WiiRemote: Bad extension calibration checksum.");
            checksum = Checksum::Bad;
          }

          if (m_extension_id == ExtensionID::Nunchuk)
          {
            m_nunchuk_state.SetCalibrationData(
                Common::BitCastPtr<WiimoteEmu::Nunchuk::CalibrationData>(calibration_data.data()),
                checksum);
          }
          else if (m_extension_id == ExtensionID::Classic)
          {
            m_classic_state.SetCalibrationData(
                Common::BitCastPtr<WiimoteEmu::Classic::CalibrationData>(calibration_data.data()),
                checksum);
          }
        });

    return;
  }
}

void Device::HandleMotionPlusNonResponse()
{
  // No need for additional checks if an extension is attached.
  // (not possible for M+ to become attached)
  if (m_extension_port == true)
    m_mplus_desired_mode = MotionPlusState::PassthroughMode{};
  else
    WaitForMotionPlus();
}

// Produce LED bitmask for remotes.
// Remotes 1-4 are normal. Additional remotes LED labels will add up to their assigned ID.
u8 Device::GetDesiredLEDValue() const
{
  const auto index = GetId();

  // Normal LED behavior for remotes 1-4.
  if (index < 4)
    return 1 << index;

  // Light LED 4 and LEDs 1 through 3 for remotes 5-7. (Add up the numbers on the remote)
  if (index < 7)
    return 1 << (index - 4) | 8;

  // Light LED 4+3 and LEDs 1 or 2 for remotes 8 or 9. (Add up the numbers on the remote)
  if (index < 9)
    return 1 << (index - 7) | 8 | 4;

  // For remotes 10 and up just light all LEDs.
  return 0xf;
}

void Device::UpdateExtensionNumberInput()
{
  switch (m_extension_id.value_or(ExtensionID::Unsupported))
  {
  case ExtensionID::Nunchuk:
    m_extension_number_input = WiimoteEmu::ExtensionNumber::NUNCHUK;
    break;
  case ExtensionID::Classic:
    m_extension_number_input = WiimoteEmu::ExtensionNumber::CLASSIC;
    break;
  case ExtensionID::Unsupported:
  default:
    m_extension_number_input = WiimoteEmu::ExtensionNumber::NONE;
    break;
  }
}

void Device::ProcessExtensionEvent(bool connected)
{
  // Reset extension state.
  m_nunchuk_state = {};
  m_classic_state = {};

  m_extension_id = std::nullopt;

  // We won't know the desired mode until we get the extension ID.
  if (connected)
    m_mplus_desired_mode = std::nullopt;
}

void Device::ProcessExtensionID(u8 id_0, u8 id_4, u8 id_5)
{
  if (id_4 == 0x00 && id_5 == 0x00)
  {
    INFO_LOG(WIIMOTE, "WiiRemote: Nunchuk is attached.");
    m_extension_id = ExtensionID::Nunchuk;

    m_mplus_desired_mode = MotionPlusState::PassthroughMode::Nunchuk;
  }
  else if (id_4 == 0x01 && id_5 == 0x01)
  {
    INFO_LOG(WIIMOTE, "WiiRemote: Classic Controller is attached.");
    m_extension_id = ExtensionID::Classic;

    m_mplus_desired_mode = MotionPlusState::PassthroughMode::Classic;
  }
  else
  {
    // This is a normal occurance before extension initialization.
    DEBUG_LOG(WIIMOTE, "WiiRemote: Unknown extension: %d %d %d.", id_0, id_4, id_5);
    m_extension_id = ExtensionID::Unsupported;
  }
}

void Device::MotionPlusState::SetCalibrationData(
    const WiimoteEmu::MotionPlus::CalibrationData& data)
{
  DEBUG_LOG(WIIMOTE, "WiiRemote: Set M+ calibration.");

  calibration.emplace();

  calibration->fast = data.fast;
  calibration->slow = data.slow;
}

Device::NunchukState::Calibration::Calibration() : accel{}, stick{}
{
  accel.zero.data.fill(1 << (accel.BITS_OF_PRECISION - 1));
  // Approximate 1G value per WiiBrew:
  accel.max.data.fill(740);

  stick.zero.data.fill(1 << (stick.BITS_OF_PRECISION - 1));
  stick.max.data.fill((1 << stick.BITS_OF_PRECISION) - 1);
}

void Device::NunchukState::SetCalibrationData(const WiimoteEmu::Nunchuk::CalibrationData& data,
                                              Checksum checksum)
{
  DEBUG_LOG(WIIMOTE, "WiiRemote: Set Nunchuk calibration.");

  calibration.emplace();

  if (checksum == Checksum::Bad)
    return;

  // Genuine Nunchuks have been observed with "min" and "max" values of zero.
  // We catch that here and fall back to "full range" calibration.
  const auto stick_calibration = data.GetStick();
  if (stick_calibration.IsSane())
    calibration->stick = stick_calibration;
  else
    WARN_LOG(WIIMOTE, "WiiRemote: Nunchuk stick calibration is not sane. Using fallback values.");

  // No known reports of bad accelerometer calibration but we'll handle it just in case.
  const auto accel_calibration = data.GetAccel();
  if (accel_calibration.IsSane())
    calibration->accel = accel_calibration;
  else
    WARN_LOG(WIIMOTE, "WiiRemote: Nunchuk accel calibration is not sane. Using fallback values.");
}

Device::ClassicState::Calibration::Calibration()
    : left_stick{}, right_stick{}, left_trigger{}, right_trigger{}
{
  left_stick.zero.data.fill(1 << (left_stick.BITS_OF_PRECISION - 1));
  left_stick.max.data.fill((1 << left_stick.BITS_OF_PRECISION) - 1);

  right_stick.zero.data.fill(1 << (right_stick.BITS_OF_PRECISION - 1));
  right_stick.max.data.fill((1 << right_stick.BITS_OF_PRECISION) - 1);

  left_trigger.max = (1 << left_trigger.BITS_OF_PRECISION) - 1;
  right_trigger.max = (1 << right_trigger.BITS_OF_PRECISION) - 1;
}

void Device::ClassicState::SetCalibrationData(const WiimoteEmu::Classic::CalibrationData& data,
                                              Checksum checksum)
{
  DEBUG_LOG(WIIMOTE, "WiiRemote: Set Classic Controller calibration.");

  calibration.emplace();

  if (checksum == Checksum::Bad)
    return;

  const auto left_stick_calibration = data.GetLeftStick();
  if (left_stick_calibration.IsSane())
    calibration->left_stick = left_stick_calibration;
  else
    WARN_LOG(WIIMOTE, "WiiRemote: CC left stick calibration is not sane. Using fallback values.");

  const auto right_stick_calibration = data.GetRightStick();
  if (right_stick_calibration.IsSane())
    calibration->right_stick = right_stick_calibration;
  else
    WARN_LOG(WIIMOTE, "WiiRemote: CC right stick calibration is not sane. Using fallback values.");

  calibration->left_trigger = data.GetLeftTrigger();
  calibration->right_trigger = data.GetRightTrigger();
}

void Device::ReadActiveExtensionID()
{
  static constexpr u16 EXT_ID_ADDR = 0xfa;
  static constexpr u16 EXT_ID_SIZE = 6;

  ReadData(AddressSpace::I2CBus, WiimoteEmu::ExtensionPort::REPORT_I2C_SLAVE, EXT_ID_ADDR,
           EXT_ID_SIZE, [this](ReadResponse response) {
             if (!response)
               return;

             auto& identifier = *response;

             // Check for M+ ID.
             if (identifier[5] == 0x05)
             {
               const auto passthrough_mode = MotionPlusState::PassthroughMode(identifier[4]);

               m_mplus_state.current_mode = passthrough_mode;

               INFO_LOG(WIIMOTE, "WiiRemote: M+ is active in mode: %d.", int(passthrough_mode));
             }
             else
             {
               m_mplus_state.current_mode = MotionPlusState::PassthroughMode{};

               ProcessExtensionID(identifier[0], identifier[4], identifier[5]);
             }
           });
}

bool Device::IRState::IsFullyConfigured() const
{
  return enabled && mode_set && current_sensitivity == GetDesiredIRSensitivity();
}

u32 Device::IRState::GetDesiredIRSensitivity()
{
  // Wii stores values from 1 to 5. (subtract 1)
  const u32 configured_level = Config::Get(Config::SYSCONF_SENSOR_BAR_SENSITIVITY) - 1;

  if (configured_level < IR_SENSITIVITY_LEVEL_COUNT)
    return configured_level;

  // Default to middle level on bad value.
  return 2;
}

void Device::SetIRSensitivity(u32 level)
{
  struct IRSensitivityConfig
  {
    std::array<u8, 9> block1;
    std::array<u8, 2> block2;
  };

  // Data for Wii levels 1 to 5.
  static constexpr std::array<IRSensitivityConfig, IR_SENSITIVITY_LEVEL_COUNT> sensitivity_configs =
      {{
          {{0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x64, 0x00, 0xfe}, {0xfd, 0x05}},
          {{0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x96, 0x00, 0xb4}, {0xb3, 0x04}},
          {{0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xaa, 0x00, 0x64}, {0x63, 0x03}},
          {{0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xc8, 0x00, 0x36}, {0x35, 0x03}},
          {{0x07, 0x00, 0x00, 0x71, 0x01, 0x00, 0x72, 0x00, 0x20}, {0x1f, 0x03}},
      }};

  static constexpr u16 BLOCK1_ADDR = 0x00;
  static constexpr u16 BLOCK2_ADDR = 0x1a;

  DEBUG_LOG(WIIMOTE, "WiiRemote: Setting IR sensitivity: %d.", level + 1);

  const auto& sensitivity_config = sensitivity_configs[level];

  WriteData(AddressSpace::I2CBus, WiimoteEmu::CameraLogic::I2C_ADDR, BLOCK1_ADDR,
            sensitivity_config.block1, [&sensitivity_config, level, this](ErrorCode block_result) {
              if (block_result != ErrorCode::Success)
              {
                WARN_LOG(WIIMOTE, "WiiRemote: Failed to write IR block 1.");
                return;
              }

              WriteData(AddressSpace::I2CBus, WiimoteEmu::CameraLogic::I2C_ADDR, BLOCK2_ADDR,
                        sensitivity_config.block2, [&, level, this](ErrorCode block2_result) {
                          if (block2_result != ErrorCode::Success)
                          {
                            WARN_LOG(WIIMOTE, "WiiRemote: Failed to write IR block 2.");
                            return;
                          }

                          DEBUG_LOG(WIIMOTE, "WiiRemote: IR sensitivity set.");

                          m_ir_state.current_sensitivity = level;
                        });
            });
}

void Device::ConfigureIRCamera()
{
  if (!m_ir_state.enabled)
  {
    OutputReportIRLogicEnable2 ir_logic2 = {};
    ir_logic2.ack = 1;
    ir_logic2.enable = 1;
    QueueReport(ir_logic2, [this](ErrorCode result) {
      if (result != ErrorCode::Success)
      {
        WARN_LOG(WIIMOTE, "WiiRemote: Failed to enable IR.");
        return;
      }

      OutputReportIRLogicEnable ir_logic = {};
      ir_logic.ack = 1;
      ir_logic.enable = 1;
      QueueReport(ir_logic, [this](ErrorCode ir_result) {
        if (ir_result != ErrorCode::Success)
        {
          WARN_LOG(WIIMOTE, "WiiRemote: Failed to enable IR.");
          return;
        }

        DEBUG_LOG(WIIMOTE, "WiiRemote: IR enabled.");

        m_ir_state.enabled = true;
      });
    });

    return;
  }

  if (const u32 desired_level = IRState::GetDesiredIRSensitivity();
      desired_level != m_ir_state.current_sensitivity)
  {
    SetIRSensitivity(desired_level);

    return;
  }

  if (!m_ir_state.mode_set)
  {
    static constexpr u16 MODE_ADDR = 0x33;

    // We only support "Basic" mode (it's all that fits in ReportCoreAccelIR10Ext6).
    WriteData(AddressSpace::I2CBus, WiimoteEmu::CameraLogic::I2C_ADDR, MODE_ADDR,
              {WiimoteEmu::CameraLogic::IR_MODE_BASIC}, [this](ErrorCode mode_result) {
                if (mode_result != ErrorCode::Success)
                {
                  WARN_LOG(WIIMOTE, "WiiRemote: Failed to set IR mode.");
                  return;
                }

                // This seems to enable object tracking.
                static constexpr u16 ENABLE_ADDR = 0x30;
                static constexpr u8 ENABLE_VALUE = 0x08;

                WriteData(AddressSpace::I2CBus, WiimoteEmu::CameraLogic::I2C_ADDR, ENABLE_ADDR,
                          {ENABLE_VALUE}, [this](ErrorCode result) {
                            if (result != ErrorCode::Success)
                            {
                              WARN_LOG(WIIMOTE, "WiiRemote: Failed to enable object tracking.");
                              return;
                            }

                            DEBUG_LOG(WIIMOTE, "WiiRemote: IR mode set.");

                            m_ir_state.mode_set = true;
                          });
              });
  }
}

void Device::ConfigureSpeaker()
{
  OutputReportSpeakerMute mute = {};
  mute.enable = 1;
  mute.ack = 1;
  QueueReport(mute, [this](ErrorCode mute_result) {
    if (mute_result != ErrorCode::Success)
    {
      WARN_LOG(WIIMOTE, "WiiRemote: Failed to mute speaker.");
      return;
    }

    OutputReportSpeakerEnable spkr = {};
    spkr.enable = 0;
    spkr.ack = 1;
    QueueReport(spkr, [this](ErrorCode enable_result) {
      if (enable_result != ErrorCode::Success)
      {
        WARN_LOG(WIIMOTE, "WiiRemote: Failed to disable speaker.");
        return;
      }

      DEBUG_LOG(WIIMOTE, "WiiRemote: Speaker muted and disabled.");

      m_speaker_configured = true;
    });
  });
}

void Device::TriggerMotionPlusModeChange()
{
  if (!m_mplus_desired_mode.has_value())
    return;

  const u8 passthrough_mode = u8(*m_mplus_desired_mode);

  const u8 device_addr = IsMotionPlusActive() ? WiimoteEmu::MotionPlus::ACTIVE_DEVICE_ADDR :
                                                WiimoteEmu::MotionPlus::INACTIVE_DEVICE_ADDR;

  WriteData(AddressSpace::I2CBus, device_addr, WiimoteEmu::MotionPlus::PASSTHROUGH_MODE_OFFSET,
            {passthrough_mode}, [this](ErrorCode activation_result) {
              DEBUG_LOG(WIIMOTE, "WiiRemote: M+ activation: %d.", int(activation_result));

              WaitForMotionPlus();

              // Normally M+ will be seen performing a reset here. (extension port events)
              // But sometimes (rarely) M+ activation does not cause an extension port event.
              // We'll consider the mode unknown. It will be read back after some time.
              m_mplus_state.current_mode = std::nullopt;
            });
}

void Device::TriggerMotionPlusCalibration()
{
  static constexpr u16 CALIBRATION_TRIGGER_ADDR = 0xf2;
  static constexpr u8 CALIBRATION_TRIGGER_VALUE = 0x00;

  // This triggers a hardware "zero" calibration.
  // The effect is notiecable but output still strays from calibration data.
  // It seems we're better off just manually determining "zero".
  WriteData(AddressSpace::I2CBus, WiimoteEmu::MotionPlus::ACTIVE_DEVICE_ADDR,
            CALIBRATION_TRIGGER_ADDR, {CALIBRATION_TRIGGER_VALUE}, [](ErrorCode result) {
              DEBUG_LOG(WIIMOTE, "WiiRemote: M+ calibration trigger done: %d.", int(result));
            });
}

bool Device::IsMotionPlusStateKnown() const
{
  return m_mplus_state.current_mode.has_value();
}

bool Device::IsMotionPlusActive() const
{
  return m_mplus_state.current_mode != MotionPlusState::PassthroughMode{};
}

bool Device::IsMotionPlusInDesiredMode() const
{
  return m_mplus_state.current_mode.has_value() &&
         (m_mplus_state.current_mode == m_mplus_desired_mode);
}

void Device::ProcessInputReport(WiimoteReal::Report& report)
{
  if (report.size() < WiimoteReal::REPORT_HID_HEADER_SIZE)
  {
    WARN_LOG(WIIMOTE, "WiiRemote: Bad report size.");
    return;
  }

  auto report_id = InputReportID(report[WiimoteReal::REPORT_HID_HEADER_SIZE]);

  for (auto it = m_report_handlers.begin(); true;)
  {
    if (it == m_report_handlers.end())
    {
      if (report_id == InputReportID::Status)
      {
        if (report.size() - WiimoteReal::REPORT_HID_HEADER_SIZE <
            sizeof(TypedInputData<InputReportStatus>))
        {
          WARN_LOG(WIIMOTE, "WiiRemote: Bad report size.");
        }
        else
        {
          ProcessStatusReport(Common::BitCastPtr<InputReportStatus>(report.data() + 2));
        }
      }
      else if (report_id < InputReportID::ReportCore)
      {
        WARN_LOG(WIIMOTE, "WiiRemote: Unhandled input report: %s.",
                 ArrayToString(report.data(), u32(report.size())).c_str());
      }

      break;
    }

    if (it->IsExpired())
    {
      WARN_LOG(WIIMOTE, "WiiRemote: Removing expired handler.");
      it = m_report_handlers.erase(it);
      continue;
    }

    if (const auto result = it->TryToHandleReport(report);
        result == ReportHandler::HandlerResult::Handled)
    {
      it = m_report_handlers.erase(it);
      break;
    }

    ++it;
  }

  if (report_id < InputReportID::ReportCore)
  {
    // Normal input reports can be processed as "ReportCore".
    report_id = InputReportID::ReportCore;
  }
  else
  {
    // We can assume the last received input report is the current reporting mode.
    // FYI: This logic fails to properly handle the (never used) "interleaved" reports.
    m_reporting_mode = InputReportID(report_id);
  }

  auto manipulator = MakeDataReportManipulator(
      report_id, report.data() + WiimoteReal::REPORT_HID_HEADER_SIZE + sizeof(InputReportID));

  if (manipulator->GetDataSize() + WiimoteReal::REPORT_HID_HEADER_SIZE > report.size())
  {
    WARN_LOG(WIIMOTE, "WiiRemote: Bad report size.");
    return;
  }

  // Read buttons.
  manipulator->GetCoreData(&m_core_data);

  // Process accel data.
  if (manipulator->HasAccel() && m_accel_calibration.has_value())
  {
    // FYI: This logic fails to properly handle the (never used) "interleaved" reports.
    AccelData accel_data = {};
    manipulator->GetAccelData(&accel_data);

    m_accel_data =
        accel_data.GetNormalizedValue(*m_accel_calibration) * float(MathUtil::GRAVITY_ACCELERATION);
  }

  // Process IR data.
  if (manipulator->HasIR() && m_ir_state.IsFullyConfigured())
  {
    m_ir_state.ProcessData(
        Common::BitCastPtr<std::array<WiimoteEmu::IRBasic, 2>>(manipulator->GetIRDataPtr()));
  }

  // Process extension data.
  if (IsMotionPlusStateKnown())
  {
    const auto ext_data = manipulator->GetExtDataPtr();
    const auto ext_size = manipulator->GetExtDataSize();

    if (IsMotionPlusActive())
      ProcessMotionPlusExtensionData(ext_data, ext_size);
    else
      ProcessNormalExtensionData(ext_data, ext_size);
  }

  UpdateOrientation();
}

void Device::UpdateOrientation()
{
  const auto current_report_time = Clock::now();
  const auto elapsed_time = std::chrono::duration_cast<std::chrono::duration<float>>(
      current_report_time - m_last_report_time);
  m_last_report_time = current_report_time;

  // Apply M+ gyro data to our orientation.
  m_orientation =
      WiimoteEmu::GetMatrixFromGyroscope(m_mplus_state.gyro_data * -1 * elapsed_time.count()) *
      m_orientation;

  // When M+ data is not available give accel/ir data more weight.
  // ComplementaryFilter will then just smooth out our data a bit.
  const bool is_mplus_active = IsMotionPlusStateKnown() && IsMotionPlusActive();

  // With non-zero acceleration data we can perform pitch and roll correction.
  if (m_accel_data.LengthSquared())
  {
    const auto accel_weight = is_mplus_active ? 0.04 : 0.5f;

    m_orientation = WiimoteEmu::ComplementaryFilter(m_orientation, m_accel_data, accel_weight);
  }

  // If IR objects are visible we can perform yaw and pitch correction.
  if (!m_ir_state.is_hidden)
  {
    // FYI: We could do some roll correction from multiple IR objects.

    const auto ir_rotation =
        Common::Vec3(m_ir_state.center_position.y * WiimoteEmu::CameraLogic::CAMERA_FOV_Y, 0,
                     m_ir_state.center_position.x * WiimoteEmu::CameraLogic::CAMERA_FOV_X) /
        2;
    const auto ir_normal = Common::Vec3(0, 1, 0);
    const auto ir_vector = WiimoteEmu::GetMatrixFromGyroscope(-ir_rotation) * ir_normal;

    // Pitch correction will be slightly wrong based on sensorbar height.
    // Keep weight below accelerometer weight for that reason.
    // Correction will only happen near pitch zero when the sensorbar is actually in view.
    const auto ir_weight = is_mplus_active ? 0.035 : 0.45f;

    m_orientation = WiimoteEmu::ComplementaryFilter(m_orientation, ir_vector, ir_weight, ir_normal);
  }

  // Update our (pitch, roll, yaw) inputs now that orientation has been adjusted.
  m_rotation_inputs =
      Common::Vec3{WiimoteEmu::GetPitch(m_orientation), WiimoteEmu::GetRoll(m_orientation),
                   WiimoteEmu::GetYaw(m_orientation)} /
      float(MathUtil::PI);
}

void Device::IRState::ProcessData(const std::array<WiimoteEmu::IRBasic, 2>& data)
{
  // A better implementation might extrapolate points when they fall out of camera view.
  // But just averaging visible points actually seems to work very well.

  using IRObject = WiimoteEmu::IRBasic::IRObject;

  MathUtil::RunningVariance<Common::Vec2> points;

  const auto camera_max = IRObject(WiimoteEmu::CameraLogic::CAMERA_RES_X - 1,
                                   WiimoteEmu::CameraLogic::CAMERA_RES_Y - 1);

  const auto add_point = [&](IRObject point) {
    // Non-visible points are 0xFF-filled.
    if (point.y > camera_max.y)
      return;

    points.Push(Common::Vec2(point));
  };

  for (auto& block : data)
  {
    add_point(block.GetObject1());
    add_point(block.GetObject2());
  }

  is_hidden = !points.Count();

  if (points.Count() >= 2)
  {
    const auto variance = points.PopulationVariance();
    // Adjusts Y coorinate to match horizontal FOV.
    const auto separation =
        Common::Vec2(std::sqrt(variance.x), std::sqrt(variance.y)) /
        Common::Vec2(WiimoteEmu::CameraLogic::CAMERA_RES_X,
                     WiimoteEmu::CameraLogic::CAMERA_RES_Y * WiimoteEmu::CameraLogic::CAMERA_AR) *
        2;

    distance = WiimoteEmu::CameraLogic::SENSOR_BAR_LED_SEPARATION / separation.Length() / 2 /
               std::tan(WiimoteEmu::CameraLogic::CAMERA_FOV_X / 2);
  }

  if (points.Count())
  {
    center_position = points.Mean() / Common::Vec2(camera_max) * 2.f - Common::Vec2(1, 1);
  }
  else
  {
    center_position = {};
  }
}

void Device::ProcessMotionPlusExtensionData(const u8* ext_data, u32 ext_size)
{
  if (ext_size < sizeof(WiimoteEmu::MotionPlus::DataFormat))
    return;

  const WiimoteEmu::MotionPlus::DataFormat mplus_data =
      Common::BitCastPtr<WiimoteEmu::MotionPlus::DataFormat>(ext_data);

  const bool is_ext_connected = mplus_data.extension_connected;

  // Handle passthrough extension change.
  if (is_ext_connected != m_mplus_state.passthrough_port)
  {
    m_mplus_state.passthrough_port = is_ext_connected;

    DEBUG_LOG(WIIMOTE, "WiiRemote: M+ passthrough port event: %d.", is_ext_connected);

    // With no passthrough extension we'll be happy with the current mode.
    if (!is_ext_connected)
      m_mplus_desired_mode = m_mplus_state.current_mode;

    ProcessExtensionEvent(is_ext_connected);
  }

  if (mplus_data.is_mp_data)
  {
    m_mplus_state.ProcessData(mplus_data);
    return;
  }

  if (!IsMotionPlusInDesiredMode())
  {
    DEBUG_LOG(WIIMOTE, "WiiRemote: Ignoring unwanted passthrough data.");
    return;
  }

  std::array<u8, sizeof(WiimoteEmu::Nunchuk::DataFormat)> data;
  std::copy_n(ext_data, ext_size, data.begin());

  // Undo bit-hacks of M+ passthrough.
  WiimoteEmu::MotionPlus::ReversePassthroughModifications(*m_mplus_state.current_mode, data.data());

  ProcessNormalExtensionData(data.data(), u32(data.size()));
}

void Device::ProcessNormalExtensionData(const u8* ext_data, u32 ext_size)
{
  if (m_extension_id == ExtensionID::Nunchuk)
  {
    if (ext_size < sizeof(WiimoteEmu::MotionPlus::DataFormat))
      return;

    const WiimoteEmu::Nunchuk::DataFormat nunchuk_data =
        Common::BitCastPtr<WiimoteEmu::Nunchuk::DataFormat>(ext_data);

    m_nunchuk_state.ProcessData(nunchuk_data);
  }
  else if (m_extension_id == ExtensionID::Classic)
  {
    if (ext_size < sizeof(WiimoteEmu::Classic::DataFormat))
      return;

    const WiimoteEmu::Classic::DataFormat cc_data =
        Common::BitCastPtr<WiimoteEmu::Classic::DataFormat>(ext_data);

    m_classic_state.ProcessData(cc_data);
  }
}

void Device::UpdateRumble()
{
  static constexpr auto rumble_period = std::chrono::milliseconds(100);

  const auto on_time = std::chrono::duration_cast<Clock::duration>(rumble_period * m_rumble_level);
  const auto off_time = rumble_period - on_time;

  const auto now = Clock::now();

  if (m_rumble && (now < m_last_rumble_change + on_time || !off_time.count()))
    return;

  if (!m_rumble && (now < m_last_rumble_change + off_time || !on_time.count()))
    return;

  m_last_rumble_change = now;
  m_rumble ^= true;

  // Rumble flag will be set within QueueReport.
  QueueReport(OutputReportRumble{});
}

void Device::UpdateInput()
{
  if (!m_wiimote->IsConnected())
  {
    g_controller_interface.RemoveDevice(
        [this](const Core::Device* device) { return device == this; });
    return;
  }

  UpdateRumble();
  RunTasks();

  WiimoteReal::Report report;
  while (m_wiimote->GetNextReport(&report))
  {
    ProcessInputReport(report);
    RunTasks();
  }
}

void Device::MotionPlusState::ProcessData(const WiimoteEmu::MotionPlus::DataFormat& data)
{
  // We need the calibration block read to know the sensor orientations.
  if (!calibration.has_value())
    return;

  gyro_data = data.GetData().GetAngularVelocity(*calibration);
}

bool Device::IsWaitingForMotionPlus() const
{
  return Clock::now() < m_mplus_wait_time;
}

void Device::WaitForMotionPlus()
{
  DEBUG_LOG(WIIMOTE, "WiiRemote: Wait for M+.");
  m_mplus_wait_time = Clock::now() + std::chrono::seconds{2};
}

void Device::NunchukState::ProcessData(const WiimoteEmu::Nunchuk::DataFormat& data)
{
  buttons = data.GetButtons();

  // Stick/accel require calibration data.
  if (!calibration.has_value())
    return;

  stick = data.GetStick().GetNormalizedValue(calibration->stick);
  accel = data.GetAccel().GetNormalizedValue(calibration->accel) *
          float(MathUtil::GRAVITY_ACCELERATION);
}

void Device::ClassicState::ProcessData(const WiimoteEmu::Classic::DataFormat& data)
{
  buttons = data.GetButtons();

  // Sticks/triggers require calibration data.
  if (!calibration.has_value())
    return;

  sticks[0] = data.GetLeftStick().GetNormalizedValue(calibration->left_stick);
  sticks[1] = data.GetRightStick().GetNormalizedValue(calibration->right_stick);
  triggers[0] = data.GetLeftTrigger().GetNormalizedValue(calibration->left_trigger);
  triggers[1] = data.GetRightTrigger().GetNormalizedValue(calibration->right_trigger);
}

void Device::ReadData(AddressSpace space, u8 slave, u16 address, u16 size,
                      std::function<void(ReadResponse)> callback)
{
  OutputReportReadData read_data{};
  read_data.space = u8(space);
  read_data.slave_address = slave;
  read_data.address[0] = u8(address >> 8);
  read_data.address[1] = u8(address);
  read_data.size[0] = u8(size >> 8);
  read_data.size[1] = u8(size);
  QueueReport(read_data);

  AddReadDataReplyHandler(space, slave, address, size, {}, std::move(callback));
}

void Device::AddReadDataReplyHandler(AddressSpace space, u8 slave, u16 address, u16 size,
                                     std::vector<u8> starting_data,
                                     std::function<void(ReadResponse)> callback)
{
  // Data read may return a busy ack.
  auto ack_handler = MakeAckHandler(OutputReportID::ReadData, [callback](ErrorCode result) {
    DEBUG_LOG(WIIMOTE, "WiiRemote: Read ack error: %d.", int(result));
    callback(ReadResponse{});
  });

  // Or more normally a "ReadDataReply".
  auto read_handler = [this, space, slave, address, size, data = std::move(starting_data),
                       callback =
                           std::move(callback)](const InputReportReadDataReply& reply) mutable {
    if (Common::swap16(reply.address) != address)
      return ReportHandler::HandlerResult::NotHandled;

    if (reply.error != u8(ErrorCode::Success))
    {
      DEBUG_LOG(WIIMOTE, "WiiRemote: Read reply error: %d.", int(reply.error));
      callback(ReadResponse{});

      return ReportHandler::HandlerResult::Handled;
    }

    const auto read_count = reply.size_minus_one + 1;

    data.insert(data.end(), reply.data, reply.data + read_count);

    if (read_count < size)
    {
      // We have more data to acquire.
      AddReadDataReplyHandler(space, slave, address + read_count, size - read_count,
                              std::move(data), std::move(callback));
    }
    else
    {
      // We have all the data.
      callback(std::move(data));
    }

    return ReportHandler::HandlerResult::Handled;
  };

  AddReportHandler(
      std::function<ReportHandler::HandlerResult(const InputReportReadDataReply& reply)>(
          std::move(read_handler)),
      std::move(ack_handler));
}

template <typename T, typename C>
void Device::WriteData(AddressSpace space, u8 slave, u16 address, T&& data, C&& callback)
{
  OutputReportWriteData write_data = {};
  write_data.space = u8(space);
  write_data.slave_address = slave;
  write_data.address[0] = u8(address >> 8);
  write_data.address[1] = u8(address);

  static constexpr auto MAX_DATA_SIZE = std::size(write_data.data);
  write_data.size = u8(std::min(std::size(data), MAX_DATA_SIZE));

  std::copy_n(std::begin(data), write_data.size, write_data.data);

  // Writes of more than 16 bytes must be split into multiple reports.
  if (std::size(data) > MAX_DATA_SIZE)
  {
    auto next_write = [this, space, slave, address,
                       additional_data =
                           std::vector<u8>(std::begin(data) + MAX_DATA_SIZE, std::end(data)),
                       callback = std::forward<C>(callback)](ErrorCode result) mutable {
      if (result != ErrorCode::Success)
        callback(result);
      else
        WriteData(space, slave, address + MAX_DATA_SIZE, additional_data, std::move(callback));
    };

    QueueReport(write_data, std::move(next_write));
  }
  else
  {
    QueueReport(write_data, std::forward<C>(callback));
  }
}

Device::ReportHandler::ReportHandler(Clock::time_point expired_time) : m_expired_time(expired_time)
{
}

template <typename... T>
void Device::AddReportHandler(T&&... callbacks)
{
  auto& handler = m_report_handlers.emplace_back(Clock::now() + std::chrono::seconds{5});
  (handler.AddHandler(std::forward<T>(callbacks)), ...);
}

template <typename R, typename T>
void Device::ReportHandler::AddHandler(std::function<R(const T&)> handler)
{
  m_callbacks.emplace_back([handler = std::move(handler)](const WiimoteReal::Report& report) {
    if (report[WiimoteReal::REPORT_HID_HEADER_SIZE] != u8(T::REPORT_ID))
      return ReportHandler::HandlerResult::NotHandled;

    T data;

    if (report.size() < sizeof(T) + WiimoteReal::REPORT_HID_HEADER_SIZE + 1)
    {
      // Off-brand "NEW 2in1" Wii Remote likes to shorten read data replies.
      WARN_LOG(WIIMOTE, "WiiRemote: Bad report size (%d) for report 0x%x. Zero-filling.",
               int(report.size()), int(T::REPORT_ID));

      data = {};
      std::memcpy(&data, report.data() + WiimoteReal::REPORT_HID_HEADER_SIZE + 1,
                  report.size() - WiimoteReal::REPORT_HID_HEADER_SIZE + 1);
    }
    else
    {
      data = Common::BitCastPtr<T>(report.data() + WiimoteReal::REPORT_HID_HEADER_SIZE + 1);
    }

    if constexpr (std::is_same_v<decltype(handler(data)), void>)
    {
      handler(data);
      return ReportHandler::HandlerResult::Handled;
    }
    else
    {
      return handler(data);
    }
  });
}

auto Device::ReportHandler::TryToHandleReport(const WiimoteReal::Report& report) -> HandlerResult
{
  for (auto& callback : m_callbacks)
  {
    if (const auto result = callback(report); result != HandlerResult::NotHandled)
      return result;
  }

  return HandlerResult::NotHandled;
}

bool Device::ReportHandler::IsExpired() const
{
  return Clock::now() >= m_expired_time;
}

auto Device::MakeAckHandler(OutputReportID report_id,
                            std::function<void(WiimoteCommon::ErrorCode)> callback)
    -> AckReportHandler
{
  return [report_id, callback = std::move(callback)](const InputReportAck& reply) {
    if (reply.rpt_id != report_id)
      return ReportHandler::HandlerResult::NotHandled;

    callback(reply.error_code);
    return ReportHandler::HandlerResult::Handled;
  };
}

bool Device::IsPerformingTask() const
{
  return !m_report_handlers.empty();
}

void Device::ProcessStatusReport(const InputReportStatus& status)
{
  // Update status periodically to keep battery level value up to date.
  m_status_outdated_time = Clock::now() + std::chrono::seconds(10);

  m_battery = status.GetEstimatedCharge() * BATTERY_INPUT_MAX_VALUE;
  m_leds = status.leds;

  if (!status.ir)
    m_ir_state = {};

  const bool is_ext_connected = status.extension;

  // Handle extension port state change.
  if (is_ext_connected != m_extension_port)
  {
    DEBUG_LOG(WIIMOTE, "WiiRemote: Extension port event: %d.", is_ext_connected);

    m_extension_port = is_ext_connected;

    // Data reporting stops on an extension port event.
    m_reporting_mode = InputReportID::ReportDisabled;

    ProcessExtensionEvent(is_ext_connected);

    // The M+ is now in an unknown state.
    m_mplus_state = {};

    if (is_ext_connected)
    {
      // We can assume the M+ is settled on an attachment event.
      m_mplus_wait_time = Clock::now();
    }
    else
    {
      // "Nunchuk" will be the most used mode and also works with no passthrough extension.
      m_mplus_desired_mode = MotionPlusState::PassthroughMode::Nunchuk;

      // If an extension is not connected the M+ is either disabled or resetting.
      m_mplus_state.current_mode = MotionPlusState::PassthroughMode{};
    }
  }
}

}  // namespace ciface::Wiimote