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

using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.SLAM;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.Detection;

public class DetectSession(Guid id, ISLAMService sLAMService) : IDetectSession
{
    private static uint SequenceDetection = 1;
    private readonly List<DetectorInfo> _detectors = [];
    private readonly Lock _lockGoal = new();
    private Thread? _updateThread;
    private bool _isRunning = false;
    private const int UPDATE_FREQUENCY_HZ = 30;
    private const double DETECTION_TIMEOUT_SECONDS = 2.0;

    public Guid SessionId { get; } = id;

    public PoseStamped? Goal
    {
        get
        {
            lock (_lockGoal)
            {
                return field;
            }
        }
        private set
        {
            lock (_lockGoal)
            {
                field = value;
            }
        }
    }

    public void AddShapeReflectiveDetector(string markerId, int priority, Point2D[] markerReferencePoints, RectangleRegion searchRegion, ILidar lidar, Pose lidarPose, double intensityThreshold = 1000)
    {
        // Create detector with configurable intensity threshold
        var detector = new ShapeReflectiveDetector(
            markerReferencePoints,
            searchRegion,
            sLAMService,
            lidar,
            lidarPose,
            intensityThreshold);

        // Add to list with priority
        _detectors.Add(new DetectorInfo(markerId, priority, detector));

        // Sort by priority (higher priority first)
        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
    }

    public void AddQRDetector(string markerId, int priority, string qrCode, ICameraQr camera, Pose cameraPose)
    {
        // Create QR detector
        var detector = new QRDetector(
            qrCode,
            sLAMService,
            camera,
            cameraPose);

        // Add to list with priority
        _detectors.Add(new DetectorInfo(markerId, priority, detector));

        // Sort by priority (higher priority first)
        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
    }

    public void Start()
    {
        if (_isRunning)
            return;

        _isRunning = true;

        // Activate all detectors
        foreach (var detectorInfo in _detectors)
        {
            detectorInfo.Detector.Active();
        }

        // Create and start goal update thread (30Hz)
        _updateThread = new Thread(UpdateGoalLoop)
        {
            Name = "DetectSession_UpdateGoal",
            IsBackground = true
        };
        _updateThread.Start();
    }

    public void Dispose()
    {
        if (!_isRunning)
            return;

        _isRunning = false;

        // Disable all detectors
        foreach (var detectorInfo in _detectors)
        {
            detectorInfo.Detector.Disable();
            detectorInfo.Detector.Dispose();
        }

        // Wait for thread to finish
        _updateThread?.Join();
        _updateThread = null;

        // Clear detectors list
        _detectors.Clear();

        GC.SuppressFinalize(this);
    }

    /// <summary>
    /// Thread loop to update Goal based on detector priorities at 30Hz
    /// Filters detectors by detection time, pose validity, and priority
    /// </summary>
    private void UpdateGoalLoop()
    {
        int delayMs = 1000 / UPDATE_FREQUENCY_HZ; // ~33ms for 30Hz

        while (_isRunning)
        {
            try
            {
                var now = DateTime.UtcNow;
                PoseStamped? bestGoal = null;
                int bestPriority = int.MinValue;
                DateTime bestDetectionTime = DateTime.MinValue;

                // Find best detector based on priority and detection freshness
                foreach (var detectorInfo in _detectors)
                {
                    var markerPose = detectorInfo.Detector.MarkerPose;
                    var detectionTime = detectorInfo.Detector.DetectionTime;

                    // Skip if pose is invalid (default value)
                    if (IsDefaultPose(markerPose))
                        continue;

                    // Skip if detection is too old (timeout)
                    var timeSinceDetection = (now - detectionTime).TotalSeconds;
                    if (timeSinceDetection > DETECTION_TIMEOUT_SECONDS)
                        continue;

                    // Select detector with highest priority
                    // If same priority, prefer more recent detection
                    if (detectorInfo.Priority > bestPriority ||
                        (detectorInfo.Priority == bestPriority && detectionTime > bestDetectionTime))
                    {
                        bestGoal = new PoseStamped()
                        {
                            Header = new RobotNet10.Shared.Header()
                            {
                                FrameId = detectorInfo.MakerId,
                                Stamp = detectionTime,
                                Seq = ++SequenceDetection,
                            },
                            Pose = markerPose,
                        };
                        bestPriority = detectorInfo.Priority;
                        bestDetectionTime = detectionTime;
                    }
                }

                // Update goal if we found a valid one
                if (bestGoal.HasValue)
                {
                    Goal = bestGoal.Value;
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [DetectSession] Updated Goal from detector '{bestGoal.Value.Header.FrameId}' with priority {bestPriority} at pose: [{bestGoal.Value.Pose.Position.X}, {bestGoal.Value.Pose.Position.Y}, {bestGoal.Value.Pose.Orientation.ToYawDegrees()}deg]");
                }
            }
            catch
            {
                // Ignore errors in update loop
            }

            // Sleep to maintain 30Hz update rate
            Thread.Sleep(delayMs);
        }
    }

    /// <summary>
    /// Check if pose is default/invalid
    /// </summary>
    private static bool IsDefaultPose(Pose pose)
    {
        return pose.Position.X == 0 &&
               pose.Position.Y == 0 &&
               pose.Position.Z == 0 &&
               pose.Orientation.X == 0 &&
               pose.Orientation.Y == 0 &&
               pose.Orientation.Z == 0 &&
               pose.Orientation.W == 0;
    }

    /// <summary>
    /// Internal class to store detector with priority
    /// </summary>
    private record DetectorInfo(string MakerId, int Priority, IDetector Detector);
}