service: irs: Implement moment image processor

6 files changed, 169 insertions, 17 deletions

diff --git a/src/core/hle/service/hid/irs.cpp b/src/core/hle/service/hid/irs.cpp
index 221c33b86..d383a266d 100644
--- a/src/core/hle/service/hid/irs.cpp
+++ b/src/core/hle/service/hid/irs.cpp
@@ -138,7 +138,7 @@ void IRS::RunMomentProcessor(HLERequestContext& ctx) {
 
     if (result.IsSuccess()) {
         auto& device = GetIrCameraSharedMemoryDeviceEntry(parameters.camera_handle);
-        MakeProcessor<MomentProcessor>(parameters.camera_handle, device);
+        MakeProcessorWithCoreContext<MomentProcessor>(parameters.camera_handle, device);
         auto& image_transfer_processor = GetProcessor<MomentProcessor>(parameters.camera_handle);
         image_transfer_processor.SetConfig(parameters.processor_config);
         npad_device->SetPollingMode(Core::HID::EmulatedDeviceIndex::RightIndex,
diff --git a/src/core/hle/service/hid/irsensor/clustering_processor.cpp b/src/core/hle/service/hid/irsensor/clustering_processor.cpp
index e2f4ae876..c559eb0d5 100644
--- a/src/core/hle/service/hid/irsensor/clustering_processor.cpp
+++ b/src/core/hle/service/hid/irsensor/clustering_processor.cpp
@@ -3,16 +3,18 @@
 
 #include <queue>
 
+#include "core/core.h"
+#include "core/core_timing.h"
 #include "core/hid/emulated_controller.h"
 #include "core/hid/hid_core.h"
 #include "core/hle/service/hid/irsensor/clustering_processor.h"
 
 namespace Service::IRS {
-ClusteringProcessor::ClusteringProcessor(Core::HID::HIDCore& hid_core_,
+ClusteringProcessor::ClusteringProcessor(Core::System& system_,
                                          Core::IrSensor::DeviceFormat& device_format,
                                          std::size_t npad_index)
-    : device{device_format} {
-    npad_device = hid_core_.GetEmulatedControllerByIndex(npad_index);
+    : device{device_format}, system{system_} {
+    npad_device = system.HIDCore().GetEmulatedControllerByIndex(npad_index);
 
     device.mode = Core::IrSensor::IrSensorMode::ClusteringProcessor;
     device.camera_status = Core::IrSensor::IrCameraStatus::Unconnected;
@@ -48,7 +50,7 @@ void ClusteringProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType ty
     }
 
     next_state = {};
-    const auto camera_data = npad_device->GetCamera();
+    const auto& camera_data = npad_device->GetCamera();
     auto filtered_image = camera_data.data;
 
     RemoveLowIntensityData(filtered_image);
@@ -83,7 +85,7 @@ void ClusteringProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType ty
     }
 
     next_state.sampling_number = camera_data.sample;
-    next_state.timestamp = next_state.timestamp + 131;
+    next_state.timestamp = system.CoreTiming().GetGlobalTimeNs().count();
     next_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low;
     shared_memory->clustering_lifo.WriteNextEntry(next_state);
 
@@ -202,14 +204,14 @@ ClusteringProcessor::ClusteringData ClusteringProcessor::MergeCluster(
 }
 
 u8 ClusteringProcessor::GetPixel(const std::vector<u8>& data, std::size_t x, std::size_t y) const {
-    if ((y * width) + x > data.size()) {
+    if ((y * width) + x >= data.size()) {
         return 0;
     }
     return data[(y * width) + x];
 }
 
 void ClusteringProcessor::SetPixel(std::vector<u8>& data, std::size_t x, std::size_t y, u8 value) {
-    if ((y * width) + x > data.size()) {
+    if ((y * width) + x >= data.size()) {
         return;
     }
     data[(y * width) + x] = value;
diff --git a/src/core/hle/service/hid/irsensor/clustering_processor.h b/src/core/hle/service/hid/irsensor/clustering_processor.h
index dc01a8ea7..83f34734a 100644
--- a/src/core/hle/service/hid/irsensor/clustering_processor.h
+++ b/src/core/hle/service/hid/irsensor/clustering_processor.h
@@ -8,6 +8,10 @@
 #include "core/hle/service/hid/irs_ring_lifo.h"
 #include "core/hle/service/hid/irsensor/processor_base.h"
 
+namespace Core {
+class System;
+}
+
 namespace Core::HID {
 class EmulatedController;
 } // namespace Core::HID
@@ -15,8 +19,7 @@ class EmulatedController;
 namespace Service::IRS {
 class ClusteringProcessor final : public ProcessorBase {
 public:
-    explicit ClusteringProcessor(Core::HID::HIDCore& hid_core_,
-                                 Core::IrSensor::DeviceFormat& device_format,
+    explicit ClusteringProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format,
                                  std::size_t npad_index);
     ~ClusteringProcessor() override;
 
@@ -106,5 +109,7 @@ private:
     Core::IrSensor::DeviceFormat& device;
     Core::HID::EmulatedController* npad_device;
     int callback_key{};
+
+    Core::System& system;
 };
 } // namespace Service::IRS
diff --git a/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp b/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp
index 803a6277c..22067a591 100644
--- a/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp
+++ b/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp
@@ -49,7 +49,7 @@ void ImageTransferProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType
         return;
     }
 
-    const auto camera_data = npad_device->GetCamera();
+    const auto& camera_data = npad_device->GetCamera();
 
     // This indicates how much ambient light is present
     processor_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low;
diff --git a/src/core/hle/service/hid/irsensor/moment_processor.cpp b/src/core/hle/service/hid/irsensor/moment_processor.cpp
index dbaca420a..cf045bda7 100644
--- a/src/core/hle/service/hid/irsensor/moment_processor.cpp
+++ b/src/core/hle/service/hid/irsensor/moment_processor.cpp
@@ -1,24 +1,137 @@
 // SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project
 // SPDX-License-Identifier: GPL-3.0-or-later
 
+#include "core/core.h"
+#include "core/core_timing.h"
+#include "core/hid/emulated_controller.h"
+#include "core/hid/hid_core.h"
 #include "core/hle/service/hid/irsensor/moment_processor.h"
 
 namespace Service::IRS {
-MomentProcessor::MomentProcessor(Core::IrSensor::DeviceFormat& device_format)
-    : device(device_format) {
+static constexpr auto format = Core::IrSensor::ImageTransferProcessorFormat::Size40x30;
+static constexpr std::size_t ImageWidth = 40;
+static constexpr std::size_t ImageHeight = 30;
+
+MomentProcessor::MomentProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format,
+                                 std::size_t npad_index)
+    : device(device_format), system{system_} {
+    npad_device = system.HIDCore().GetEmulatedControllerByIndex(npad_index);
+
     device.mode = Core::IrSensor::IrSensorMode::MomentProcessor;
     device.camera_status = Core::IrSensor::IrCameraStatus::Unconnected;
     device.camera_internal_status = Core::IrSensor::IrCameraInternalStatus::Stopped;
+
+    shared_memory = std::construct_at(
+        reinterpret_cast<MomentSharedMemory*>(&device_format.state.processor_raw_data));
+
+    Core::HID::ControllerUpdateCallback engine_callback{
+        .on_change = [this](Core::HID::ControllerTriggerType type) { OnControllerUpdate(type); },
+        .is_npad_service = true,
+    };
+    callback_key = npad_device->SetCallback(engine_callback);
 }
 
-MomentProcessor::~MomentProcessor() = default;
+MomentProcessor::~MomentProcessor() {
+    npad_device->DeleteCallback(callback_key);
+};
 
-void MomentProcessor::StartProcessor() {}
+void MomentProcessor::StartProcessor() {
+    device.camera_status = Core::IrSensor::IrCameraStatus::Available;
+    device.camera_internal_status = Core::IrSensor::IrCameraInternalStatus::Ready;
+}
 
 void MomentProcessor::SuspendProcessor() {}
 
 void MomentProcessor::StopProcessor() {}
 
+void MomentProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType type) {
+    if (type != Core::HID::ControllerTriggerType::IrSensor) {
+        return;
+    }
+
+    next_state = {};
+    const auto& camera_data = npad_device->GetCamera();
+
+    const auto window_width = static_cast<std::size_t>(current_config.window_of_interest.width);
+    const auto window_height = static_cast<std::size_t>(current_config.window_of_interest.height);
+    const auto window_start_x = static_cast<std::size_t>(current_config.window_of_interest.x);
+    const auto window_start_y = static_cast<std::size_t>(current_config.window_of_interest.y);
+
+    const std::size_t block_width = window_width / Columns;
+    const std::size_t block_height = window_height / Rows;
+
+    for (std::size_t row = 0; row < Rows; row++) {
+        for (std::size_t column = 0; column < Columns; column++) {
+            const size_t x_pos = (column * block_width) + window_start_x;
+            const size_t y_pos = (row * block_height) + window_start_y;
+            auto& statistic = next_state.statistic[column + (row * Columns)];
+            statistic = GetStatistic(camera_data.data, x_pos, y_pos, block_width, block_height);
+        }
+    }
+
+    next_state.sampling_number = camera_data.sample;
+    next_state.timestamp = system.CoreTiming().GetGlobalTimeNs().count();
+    next_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low;
+    shared_memory->moment_lifo.WriteNextEntry(next_state);
+
+    if (!IsProcessorActive()) {
+        StartProcessor();
+    }
+}
+
+u8 MomentProcessor::GetPixel(const std::vector<u8>& data, std::size_t x, std::size_t y) const {
+    if ((y * ImageWidth) + x >= data.size()) {
+        return 0;
+    }
+    return data[(y * ImageWidth) + x];
+}
+
+MomentProcessor::MomentStatistic MomentProcessor::GetStatistic(const std::vector<u8>& data,
+                                                               std::size_t start_x,
+                                                               std::size_t start_y,
+                                                               std::size_t width,
+                                                               std::size_t height) const {
+    // The actual implementation is always 320x240
+    static constexpr std::size_t RealWidth = 320;
+    static constexpr std::size_t RealHeight = 240;
+    static constexpr std::size_t Threshold = 30;
+    MomentStatistic statistic{};
+    std::size_t active_points{};
+
+    // Sum all data points on the block that meet with the threshold
+    for (std::size_t y = 0; y < width; y++) {
+        for (std::size_t x = 0; x < height; x++) {
+            const size_t x_pos = x + start_x;
+            const size_t y_pos = y + start_y;
+            const auto pixel =
+                GetPixel(data, x_pos * ImageWidth / RealWidth, y_pos * ImageHeight / RealHeight);
+
+            if (pixel < Threshold) {
+                continue;
+            }
+
+            statistic.average_intensity += pixel;
+
+            statistic.centroid.x += static_cast<float>(x_pos);
+            statistic.centroid.y += static_cast<float>(y_pos);
+
+            active_points++;
+        }
+    }
+
+    // Return an empty field if no points were available
+    if (active_points == 0) {
+        return {};
+    }
+
+    // Finally calculate the actual centroid and average intensity
+    statistic.centroid.x /= static_cast<float>(active_points);
+    statistic.centroid.y /= static_cast<float>(active_points);
+    statistic.average_intensity /= static_cast<f32>(width * height);
+
+    return statistic;
+}
+
 void MomentProcessor::SetConfig(Core::IrSensor::PackedMomentProcessorConfig config) {
     current_config.camera_config.exposure_time = config.camera_config.exposure_time;
     current_config.camera_config.gain = config.camera_config.gain;
@@ -29,6 +142,8 @@ void MomentProcessor::SetConfig(Core::IrSensor::PackedMomentProcessorConfig conf
     current_config.preprocess =
         static_cast<Core::IrSensor::MomentProcessorPreprocess>(config.preprocess);
     current_config.preprocess_intensity_threshold = config.preprocess_intensity_threshold;
+
+    npad_device->SetCameraFormat(format);
 }
 
 } // namespace Service::IRS
diff --git a/src/core/hle/service/hid/irsensor/moment_processor.h b/src/core/hle/service/hid/irsensor/moment_processor.h
index d4bd22e0f..398cfbdc1 100644
--- a/src/core/hle/service/hid/irsensor/moment_processor.h
+++ b/src/core/hle/service/hid/irsensor/moment_processor.h
@@ -6,12 +6,22 @@
 #include "common/bit_field.h"
 #include "common/common_types.h"
 #include "core/hid/irs_types.h"
+#include "core/hle/service/hid/irs_ring_lifo.h"
 #include "core/hle/service/hid/irsensor/processor_base.h"
 
+namespace Core {
+class System;
+}
+
+namespace Core::HID {
+class EmulatedController;
+} // namespace Core::HID
+
 namespace Service::IRS {
 class MomentProcessor final : public ProcessorBase {
 public:
-    explicit MomentProcessor(Core::IrSensor::DeviceFormat& device_format);
+    explicit MomentProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format,
+                             std::size_t npad_index);
     ~MomentProcessor() override;
 
     // Called when the processor is initialized
@@ -27,6 +37,9 @@ public:
     void SetConfig(Core::IrSensor::PackedMomentProcessorConfig config);
 
 private:
+    static constexpr std::size_t Columns = 8;
+    static constexpr std::size_t Rows = 6;
+
     // This is nn::irsensor::MomentProcessorConfig
     struct MomentProcessorConfig {
         Core::IrSensor::CameraConfig camera_config;
@@ -50,12 +63,29 @@ private:
         u64 timestamp;
         Core::IrSensor::CameraAmbientNoiseLevel ambient_noise_level;
         INSERT_PADDING_BYTES(4);
-        std::array<MomentStatistic, 0x30> stadistic;
+        std::array<MomentStatistic, Columns * Rows> statistic;
     };
     static_assert(sizeof(MomentProcessorState) == 0x258, "MomentProcessorState is an invalid size");
 
+    struct MomentSharedMemory {
+        Service::IRS::Lifo<MomentProcessorState, 6> moment_lifo;
+    };
+    static_assert(sizeof(MomentSharedMemory) == 0xE20, "MomentSharedMemory is an invalid size");
+
+    void OnControllerUpdate(Core::HID::ControllerTriggerType type);
+    u8 GetPixel(const std::vector<u8>& data, std::size_t x, std::size_t y) const;
+    MomentStatistic GetStatistic(const std::vector<u8>& data, std::size_t start_x,
+                                 std::size_t start_y, std::size_t width, std::size_t height) const;
+
+    MomentSharedMemory* shared_memory = nullptr;
+    MomentProcessorState next_state{};
+
     MomentProcessorConfig current_config{};
     Core::IrSensor::DeviceFormat& device;
+    Core::HID::EmulatedController* npad_device;
+    int callback_key{};
+
+    Core::System& system;
 };
 
 } // namespace Service::IRS