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

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

namespace RobotNet10.RobotApp.Detection;

/// <summary>
/// Detector for QR code markers using camera
/// Polls QR camera for specific QR code and transforms pose to global frame
/// </summary>
/// <remarks>
/// Create a new QR code detection session
/// </remarks>
/// <param name="qrCode">QR code string to detect</param>
/// <param name="slamService">SLAM service for current robot pose</param>
/// <param name="camera">QR camera device</param>
/// <param name="cameraPose">Camera pose relative to robot base</param>
public class QRDetector(
    string qrCode,
    ISLAMService slamService,
    ICameraQr camera,
    Pose cameraPose) : IDetector
{
    private readonly Lock _lockPose = new();
    private bool _isActive = false;
    private Thread? _pollingThread;
    private const int POLLING_FREQUENCY_HZ = 30; // Poll at 30Hz

    /// <summary>
    /// Detected marker pose in global frame
    /// Initialized with default pose, updated when QR code is detected
    /// </summary>
    public Pose MarkerPose
    {
        get
        {
            lock (_lockPose)
            {
                return field;
            }
        }
        private set
        {
            lock (_lockPose)
            {
                field = value;
            }
        }
    }

    /// <summary>
    /// Timestamp of the last successful QR detection
    /// </summary>
    public DateTime DetectionTime { get; private set; } = DateTime.MinValue;

    /// <summary>
    /// Activate the QR detection
    /// Starts polling QR camera in background thread
    /// </summary>
    public void Active()
    {
        if (_isActive)
            return;

        _isActive = true;

        // Create and start polling thread
        _pollingThread = new Thread(PollingThreadLoop)
        {
            Name = $"QRDetection_{qrCode}",
            IsBackground = true
        };
        _pollingThread.Start();
    }

    /// <summary>
    /// Disable the QR detection
    /// Stops polling thread
    /// </summary>
    public void Disable()
    {
        if (!_isActive)
            return;

        _isActive = false;

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

    /// <summary>
    /// Polling thread loop that checks QR camera periodically
    /// Runs at 30Hz to check for QR code detection
    /// </summary>
    private void PollingThreadLoop()
    {
        int delayMs = 1000 / POLLING_FREQUENCY_HZ; // ~33ms for 30Hz

        while (_isActive)
        {
            try
            {
                // Check if camera is connected
                if (!camera.IsConnected)
                {
                    Thread.Sleep(delayMs);
                    continue;
                }

                // Get QR code pose from camera (in camera frame)
                var poseStampedInCamera = camera[qrCode];

                if (poseStampedInCamera.HasValue)
                {
                    // Get current robot pose in global frame
                    var currentRobotPose = slamService.CurrentPose;

                    // Transform: camera frame -> robot frame -> global frame
                    var poseInRobot = TransformPose(poseStampedInCamera.Value.Pose, cameraPose);
                    var poseInGlobal = TransformPose(poseInRobot, currentRobotPose);

                    // Update marker pose and detection time
                    MarkerPose = poseInGlobal;
                    DetectionTime = poseStampedInCamera.Value.Header.Stamp;

                    // Console.WriteLine($"{DetectionTime:HH:mm:ss.ffffff} [QRDetector] Detected found QR code '{qrCode}' at global pose: [{MarkerPose.Position.X}, {MarkerPose.Position.Y}, {MarkerPose.Orientation.ToYawDegrees()}deg]");
                }
            }
            catch
            {
                // Ignore errors in polling loop
            }

            // Sleep to maintain polling rate
            Thread.Sleep(delayMs);
        }
    }

    /// <summary>
    /// Transform a pose by another pose (pose composition)
    /// result = parentPose * childPose
    /// </summary>
    /// <param name="childPose">Pose in child frame</param>
    /// <param name="parentPose">Parent frame pose</param>
    /// <returns>Transformed pose in parent's parent frame</returns>
    private static Pose TransformPose(Pose childPose, Pose parentPose)
    {
        // Get yaw angles
        double parentYaw = parentPose.Orientation.ToYawRadian();
        double childYaw = childPose.Orientation.ToYawRadian();

        // Rotate child position by parent orientation
        double cosParent = Math.Cos(parentYaw);
        double sinParent = Math.Sin(parentYaw);

        double globalX = parentPose.Position.X + childPose.Position.X * cosParent - childPose.Position.Y * sinParent;
        double globalY = parentPose.Position.Y + childPose.Position.X * sinParent + childPose.Position.Y * cosParent;

        // Combine orientations
        double globalYaw = NormalizeAngle(parentYaw + childYaw);

        return new Pose
        {
            Position = new Vector3
            {
                X = globalX,
                Y = globalY,
                Z = parentPose.Position.Z + childPose.Position.Z
            },
            Orientation = CreateQuaternionFromYaw(globalYaw)
        };
    }

    /// <summary>
    /// Create a quaternion from yaw angle (rotation around Z axis)
    /// </summary>
    /// <param name="yaw">Yaw angle in radians</param>
    /// <returns>Quaternion representing rotation around Z axis</returns>
    private static RobotNet10.Shared.Geometry.Quaternion CreateQuaternionFromYaw(double yaw)
    {
        double halfYaw = yaw / 2.0;
        return new RobotNet10.Shared.Geometry.Quaternion
        {
            X = 0,
            Y = 0,
            Z = Math.Sin(halfYaw),
            W = Math.Cos(halfYaw)
        };
    }

    /// <summary>
    /// Normalize angle to [-π, π]
    /// </summary>
    /// <param name="angle">Angle in radians</param>
    /// <returns>Normalized angle in [-π, π]</returns>
    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    public void Dispose()
    {
        // Disable detector (stops thread)
        Disable();

        // Suppress finalization since we've cleaned up
        GC.SuppressFinalize(this);
    }
}