BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/MarkerDetector.cs

using Microsoft.Extensions.Configuration;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.SLAM;
using RobotNet10.Shared.Detection;
using RobotNet10.Shared.Enum;
using RobotNet10.Shared.Geometry;
using SkiaSharp;

namespace RobotNet10.RobotApp.Detection;

public class MarkerDetector(
    IConfiguration configuration,
    IDeviceProvider deviceProvider,
    ISLAMService slamService) : IMarkerDetector
{
    private readonly MarkerDetectorConfiguration _config = BindConfig(configuration);
    private readonly List<IDetectSession> _sessions = [];
    private readonly Lock _lock = new();

    private static MarkerDetectorConfiguration BindConfig(IConfiguration configuration)
    {
        var section = configuration.GetSection("Detection:MarkerDetector");
        if (!section.Exists())
            throw new InvalidOperationException("Configuration section 'Detection:MarkerDetector' not found.");

        var config = new MarkerDetectorConfiguration();
        section.Bind(config);
        return config;
    }

    public Task<IDetectSession> CreateSessionAsync(MarkersSearchRequest request)
    {
        var session = new DetectSession(Guid.NewGuid(), slamService);

        // Create search region from request in global frame
        // (X, Y as center, Yaw as rotation - all in global/world coordinates)
        var searchRegion = new RectangleRegion(
            request.X,
            request.Y,
            request.Width,
            request.Length,
            request.Yaw);

        foreach (var entry in request.MarkerSearchRequests)
        {
            switch (entry.Type)
            {
                case MarkerType.ShapeReflective:
                    AddShapeReflectiveDetector(session, entry, searchRegion);
                    break;

                case MarkerType.QRCode:
                    AddQRDetector(session, entry);
                    break;

                case MarkerType.ArUco:
                    // TODO: Implement ArUco detector
                    break;

                default:
                    // TODO: Handle other marker types
                    break;
            }
        }

        session.Start();

        lock (_lock)
        {
            _sessions.Add(session);
        }

        return Task.FromResult<IDetectSession>(session);
    }

    public IReadOnlyList<IDetectSession> GetActiveSessions()
    {
        lock (_lock)
        {
            return [.. _sessions];
        }
    }

    public IDetectSession? GetSession(Guid sessionId)
    {
        lock (_lock)
        {
            return _sessions.FirstOrDefault(s => s.SessionId == sessionId);
        }
    }

    public void Dispose()
    {
        lock (_lock)
        {
            foreach (var session in _sessions)
            {
                session.Dispose();
            }
            _sessions.Clear();
        }
        GC.SuppressFinalize(this);
    }

    private void AddShapeReflectiveDetector(DetectSession session, MarkerEntry entry, RectangleRegion searchRegion)
    {
        // Get device from provider and cast to ILidar
        var device = deviceProvider.GetDevice(entry.DeviceId)
            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");

        if (device is not ILidar lidar)
            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ILidar.");

        // Get lidar transform from configuration (includes pose and intensity threshold)
        var lidarTransform = GetDeviceTransform(_config.LidarDevices, entry.DeviceId);
        var lidarPose = new Pose(
            lidarTransform.Position,
            new RobotNet10.Shared.Geometry.Quaternion(
                lidarTransform.Orientation.X,
                lidarTransform.Orientation.Y,
                lidarTransform.Orientation.Z,
                lidarTransform.Orientation.W));

        // Parse reference points from Parameters [x1, y1, x2, y2, ...]
        var referencePoints = entry.ReferencePoints.Select(p => new Point2D(p.X, p.Y)).ToArray();

        session.AddShapeReflectiveDetector(entry.MarkerId,
            entry.Priority,
            referencePoints,
            searchRegion,
            lidar,
            lidarPose,
            lidarTransform.IntensityThreshold);
    }

    private void AddQRDetector(DetectSession session, MarkerEntry entry)
    {
        // Get device from provider and cast to ICameraQr
        var device = deviceProvider.GetDevice(entry.DeviceId)
            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");

        if (device is not ICameraQr camera)
            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ICameraQr.");

        // Get camera pose from configuration
        var cameraPose = GetDevicePose(_config.QrDevices, entry.DeviceId);

        // Use MarkerId as QR code string to detect
        var qrCode = entry.Code;

        session.AddQRDetector(entry.MarkerId,
            entry.Priority,
            qrCode,
            camera,
            cameraPose);
    }

    private static Pose GetDevicePose(DeviceTransform[] devices, string deviceId)
    {
        var transform = GetDeviceTransform(devices, deviceId);
        return new Pose(
            transform.Position,
            new RobotNet10.Shared.Geometry.Quaternion(
                transform.Orientation.X,
                transform.Orientation.Y,
                transform.Orientation.Z,
                transform.Orientation.W));
    }

    private static DeviceTransform GetDeviceTransform(DeviceTransform[] devices, string deviceId)
    {
        var transform = Array.Find(devices, d => d.DeviceId == deviceId);
        if (string.IsNullOrEmpty(transform.DeviceId))
            throw new ArgumentException($"Device transform for '{deviceId}' not found in configuration.");

        return transform;
    }
}