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

namespace RobotNet10.RobotApp.Detection;

/// <summary>
/// Session for detecting reflective markers based on shape matching with laser scan data
/// Supports 2, 3, or 4 reference points
/// IMPORTANT: Marker origin (0,0) in marker frame MUST be at the centroid of reference points
/// </summary>
/// <remarks>
/// Create a new shape reflective marker detection session
/// </remarks>
/// <param name="markerReferencePoints">Reference points in marker frame (2, 3, or 4 points). Centroid must be at marker origin (0,0).</param>
/// <param name="searchRegion">Search region for marker center (in global frame)</param>
/// <param name="intensityThreshold">Intensity threshold for filtering reflective markers</param>
/// <param name="clusteringEps">OPTICS epsilon parameter for clustering</param>
/// <param name="clusteringMinPts">OPTICS minimum points parameter</param>
/// <param name="clusterThreshold">Reachability threshold for cluster extraction</param>
/// <param name="maxFitError">Maximum allowed fitting error (meters)</param>
/// <exception cref="ArgumentException">Thrown when number of reference points is not 2, 3, or 4</exception>
public class ShapeReflectiveDetector(
    Point2D[] markerReferencePoints,
    RectangleRegion searchRegion,
    ISLAMService sLAMService,
    ILidar lidar,
    Pose lidarPose,
    double intensityThreshold = 2500,
    double clusteringEps = 0.1,
    int clusteringMinPts = 3,
    double clusterThreshold = 0.5,
    double maxFitError = 0.05) : IDetector
{
    // Validate number of reference points (must be 2, 3, or 4)
    private readonly Point2D[] _validatedMarkerPoints = markerReferencePoints.Length is >= 1 and <= 4
        ? markerReferencePoints
        : throw new ArgumentException(
            $"Number of reference points must be between 2 and 4, but got {markerReferencePoints.Length}",
            nameof(markerReferencePoints));

    private readonly OpticsClusteringAlgorithm _optics = new(clusteringEps, clusteringMinPts);

    private readonly Lock _lockPose = new();
    private readonly Lock _lockScan = new();

    private bool _isActive = false;
    private bool _isProcessing = false;
    private Thread? _processingThread;
    private AutoResetEvent? _scanReceivedEvent;
    private LaserScan? _latestScan;
    private DateTime _lastScanTime = DateTime.MinValue;

    /// <summary>
    /// Detected marker pose in global frame
    /// Initialized with search region center, updated when marker is detected
    /// </summary>
    public Pose MarkerPose
    {
        get
        {
            lock (_lockPose)
            {
                return field;
            }
        }
        private set
        {
            lock (_lockPose)
            {
                field = value;
            }
        }
    } = new Pose(new Vector3(searchRegion.Center.X, searchRegion.Center.Y, 0), CreateQuaternionFromYaw(searchRegion.RotationAngle));

    /// <summary>
    /// Timestamp of the last successful marker detection
    /// </summary>
    public DateTime DetectionTime { get; private set; }

    /// <summary>
    /// Activate the marker detection session
    /// Starts listening to laser scan data and processes it in background
    /// </summary>
    public void Active()
    {
        if (_isActive)
            return;

        _isActive = true;
        _scanReceivedEvent = new AutoResetEvent(false);

        // Create and start processing thread
        _processingThread = new Thread(ProcessingThreadLoop)
        {
            Name = "ShapeReflectiveDetection",
            IsBackground = true
        };
        _processingThread.Start();

        // Subscribe to laser scan data
        lidar.ScanDataReceived += OnScanDataReceived;
    }

    /// <summary>
    /// Disable the marker detection session
    /// Stops processing laser scan data and unsubscribes from events
    /// </summary>
    public void Disable()
    {
        if (!_isActive)
            return;

        _isActive = false;

        // Unsubscribe from laser scan data
        lidar.ScanDataReceived -= OnScanDataReceived;

        // Signal the thread to wake up and exit
        _scanReceivedEvent?.Set();

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

        // Dispose wait handle
        _scanReceivedEvent?.Dispose();
        _scanReceivedEvent = null;

        // Clear latest scan
        lock (_lockScan)
        {
            _latestScan = null;
        }
    }

    /// <summary>
    /// Event handler for laser scan data received
    /// Stores the latest scan and signals the processing thread
    /// If already processing, skip this scan to avoid overload
    /// </summary>
    private void OnScanDataReceived(object? _, LidarScanDataEventArgs e)
    {
        // Check if already processing, skip if busy
        lock (_lockScan)
        {
            if (_isProcessing)
                return;

            // Store latest scan data
            _latestScan = e.MeasurementData;
            _lastScanTime = e.Timestamp;
        }

        // Signal the processing thread that new data is available
        _scanReceivedEvent?.Set();
    }

    /// <summary>
    /// Processing thread loop that waits for new scan data and processes it
    /// </summary>
    private void ProcessingThreadLoop()
    {
        while (_isActive)
        {
            // Wait for signal that new scan data is available
            if (_scanReceivedEvent?.WaitOne() == true)
            {
                // Check if still active (might have been disabled)
                if (!_isActive)
                    break;

                // Get the latest scan data
                LaserScan? scanToProcess;
                lock (_lockScan)
                {
                    scanToProcess = _latestScan;
                    _latestScan = null; // Clear after reading

                    // Set processing flag
                    if (scanToProcess != null)
                        _isProcessing = true;
                }

                // Process the scan if available
                if (scanToProcess is LaserScan scan)
                {
                    try
                    {
                        var currentPose = sLAMService.CurrentPose;
                        
                        var markerReferenceSearchRegionsInGlobal = CalculateMarkerReferenceSearchRegionsInGlobal(searchRegion);
                        
                        var markerReferenceSearchRegionsInRobot = TransformRegionsFromGlobalToRobot(markerReferenceSearchRegionsInGlobal, currentPose);
                        
                        var markerReferenceSearchRegions = TransformRegionsFromRobotToLidar(markerReferenceSearchRegionsInRobot, lidarPose);
                        
                        var (angleStart, angleEnd) = CalculateAngleRangeFromRegions(markerReferenceSearchRegions);

                        var points = ConvertLaserScanToPoints(scan, intensityThreshold, angleStart, angleEnd);

                        _optics.ClearPoints();
                        _optics.AddPoints(points);
                        _optics.Run();

                        // Extract cluster indices for this region
                        var clusters = _optics.GetClusters(clusterThreshold);

                        // Extract centroids from clusters in each region
                        var centroids = clusters.Select(cluster => CalculateCentroid([.. cluster])).ToList();

                        // Match centroids to their corresponding search regions
                        var regionCentroids = new List<List<Point2D>>();
                        for (int i = 0; i < markerReferenceSearchRegions.Count; i++)
                        {
                            var region = markerReferenceSearchRegions[i];
                            var matchingCentroids = new List<Point2D>();

                            foreach (var centroid in centroids)
                            {
                                if (region.ContainsPoint(centroid.X, centroid.Y))
                                {
                                    matchingCentroids.Add(centroid);
                                }
                            }

                            regionCentroids.Add(matchingCentroids);
                        }

                        // Find best matching pose in lidar frame
                        var poseInLidar = FindMatchingPoses(regionCentroids);

                        if (poseInLidar.HasValue)
                        {
                            // Transform pose from lidar frame -> robot frame -> global frame
                            var poseInRobot = TransformPose(poseInLidar.Value, lidarPose);
                            var poseInGlobal = TransformPose(poseInRobot, currentPose);

                            // Update marker pose and detection time
                            MarkerPose = poseInGlobal;
                            DetectionTime = _lastScanTime;
                        }
                    }
                    finally
                    {
                        // Clear processing flag
                        lock (_lockScan)
                        {
                            _isProcessing = false;
                        }
                    }
                }
            }
        }
    }

    /// <summary>
    /// Transform search region from global frame to robot frame (inverse transform)
    /// </summary>
    /// <param name="searchRegion">Search region in global frame</param>
    /// <param name="robotPose">Robot pose in global frame</param>
    /// <returns>Transformed search region in robot frame</returns>
    private static RectangleRegion TransformSearchRegionFromGlobalToRobot(RectangleRegion searchRegion, Pose robotPose)
    {
        // Get yaw angle from robot pose
        double robotYaw = robotPose.Orientation.ToYawRadian();

        // Inverse transform: global frame → robot frame
        // Translate center from global to robot origin (inverse)
        double dx = searchRegion.Center.X - robotPose.Position.X;
        double dy = searchRegion.Center.Y - robotPose.Position.Y;

        // Rotate by negative robot yaw (inverse rotation)
        double cosInv = Math.Cos(-robotYaw);
        double sinInv = Math.Sin(-robotYaw);

        double newCenterX = dx * cosInv - dy * sinInv;
        double newCenterY = dx * sinInv + dy * cosInv;

        // Transform rotation angle (subtract robot yaw)
        double newRotation = searchRegion.RotationAngle - robotYaw;

        // Normalize angle to [-π, π]
        newRotation = NormalizeAngle(newRotation);

        return new RectangleRegion(
            new Point2D(newCenterX, newCenterY),
            searchRegion.Width,
            searchRegion.Height,
            newRotation);
    }

    /// <summary>
    /// Transform search region from robot frame to lidar frame (inverse transform)
    /// </summary>
    /// <param name="searchRegion">Search region in robot frame</param>
    /// <param name="lidarPose">Lidar pose relative to robot base</param>
    /// <returns>Transformed search region in lidar frame</returns>
    private static RectangleRegion TransformSearchRegionInverse(RectangleRegion searchRegion, Pose lidarPose)
    {
        // Get yaw angle from lidar pose
        double lidarYaw = lidarPose.Orientation.ToYawRadian();

        // Inverse transform: robot frame → lidar frame
        // Translate center from robot origin to lidar origin (inverse)
        double dx = searchRegion.Center.X - lidarPose.Position.X;
        double dy = searchRegion.Center.Y - lidarPose.Position.Y;

        // Rotate by negative lidar yaw (inverse rotation)
        double cosInv = Math.Cos(-lidarYaw);
        double sinInv = Math.Sin(-lidarYaw);

        double newCenterX = dx * cosInv - dy * sinInv;
        double newCenterY = dx * sinInv + dy * cosInv;

        // Transform rotation angle (subtract lidar yaw)
        double newRotation = searchRegion.RotationAngle - lidarYaw;

        // Normalize angle to [-π, π]
        newRotation = NormalizeAngle(newRotation);

        return new RectangleRegion(
            new Point2D(newCenterX, newCenterY),
            searchRegion.Width,
            searchRegion.Height,
            newRotation);
    }

    /// <summary>
    /// Calculate search regions for each marker reference point in global frame
    /// Transforms each reference point from marker frame to global frame
    /// based on predicted marker pose, then creates search region around it
    /// Each search region inherits size from the main search region to account for pose uncertainty
    /// </summary>
    /// <param name="searchRegion">Predicted marker pose (center + rotation) in global frame</param>
    /// <returns>List of search regions for each reference point in global frame</returns>
    private List<RectangleRegion> CalculateMarkerReferenceSearchRegionsInGlobal(RectangleRegion searchRegion)
    {
        var regions = new List<RectangleRegion>();

        // Use search region center and rotation as predicted marker pose in global frame
        double markerX = searchRegion.Center.X;
        double markerY = searchRegion.Center.Y;
        double markerRotation = searchRegion.RotationAngle;

        double cosTheta = Math.Cos(markerRotation);
        double sinTheta = Math.Sin(markerRotation);

        // Transform each reference point from marker frame to global frame
        for (int i = 0; i < _validatedMarkerPoints.Length; i++)
        {
            var refPoint = _validatedMarkerPoints[i];
            
            // Apply rotation and translation to transform from marker frame to global frame
            double pointX = markerX + refPoint.X * cosTheta - refPoint.Y * sinTheta;
            double pointY = markerY + refPoint.X * sinTheta + refPoint.Y * cosTheta;

            // Create search region centered at this predicted point
            // Use same size as main search region to account for pose uncertainty
            regions.Add(new RectangleRegion(
                new Point2D(pointX, pointY),
                searchRegion.Width,
                searchRegion.Height,
                0));  // Axis-aligned for simplicity
        }

        return regions;
    }

    /// <summary>
    /// Transform a list of rectangle regions from global frame to robot frame
    /// </summary>
    /// <param name="regionsInGlobal">List of regions in global frame</param>
    /// <param name="robotPose">Robot pose in global frame</param>
    /// <returns>List of regions in robot frame</returns>
    private static List<RectangleRegion> TransformRegionsFromGlobalToRobot(List<RectangleRegion> regionsInGlobal, Pose robotPose)
    {
        var regionsInRobot = new List<RectangleRegion>();

        foreach (var region in regionsInGlobal)
        {
            var transformedRegion = TransformSearchRegionFromGlobalToRobot(region, robotPose);
            regionsInRobot.Add(transformedRegion);
        }

        return regionsInRobot;
    }

    /// <summary>
    /// Transform a list of rectangle regions from robot frame to lidar frame
    /// </summary>
    /// <param name="regionsInRobot">List of regions in robot frame</param>
    /// <param name="lidarPose">Lidar pose relative to robot base</param>
    /// <returns>List of regions in lidar frame</returns>
    private static List<RectangleRegion> TransformRegionsFromRobotToLidar(List<RectangleRegion> regionsInRobot, Pose lidarPose)
    {
        var regionsInLidar = new List<RectangleRegion>();

        foreach (var region in regionsInRobot)
        {
            var transformedRegion = TransformSearchRegionInverse(region, lidarPose);
            regionsInLidar.Add(transformedRegion);
        }

        return regionsInLidar;
    }

    /// <summary>
    /// Calculate the angle range needed to cover all search regions
    /// This optimizes laser scan processing by only considering relevant angles
    /// </summary>
    /// <param name="regions">Search regions to analyze</param>
    /// <returns>Tuple of (angleStart, angleEnd) in radians</returns>
    private static (double angleStart, double angleEnd) CalculateAngleRangeFromRegions(List<RectangleRegion> regions)
    {
        double minAngle = double.MaxValue;
        double maxAngle = double.MinValue;

        foreach (var region in regions)
        {
            // Get the 4 corners of the rectangle
            var corners = GetRectangleCorners(region);

            // Calculate angle to each corner from origin (lidar position at 0,0)
            foreach (var corner in corners)
            {
                double angle = Math.Atan2(corner.Y, corner.X);

                if (angle < minAngle) minAngle = angle;
                if (angle > maxAngle) maxAngle = angle;
            }
        }

        // Add small margin (5 degrees) to ensure we don't miss any points at the boundaries
        const double margin = 5.0 * Math.PI / 180.0; // 5 degrees in radians
        minAngle -= margin;
        maxAngle += margin;

        // Clamp to valid angle range [-π, π]
        minAngle = Math.Max(minAngle, -Math.PI);
        maxAngle = Math.Min(maxAngle, Math.PI);

        return (minAngle, maxAngle);
    }

    /// <summary>
    /// Get the 4 corners of a rectangle region
    /// </summary>
    /// <param name="region">Rectangle region</param>
    /// <returns>List of 4 corner points in the same frame as the input region</returns>
    private static List<Point2D> GetRectangleCorners(RectangleRegion region)
    {
        double halfWidth = region.Width / 2.0;
        double halfHeight = region.Height / 2.0;

        // Define 4 corners in local frame (before rotation)
        var localCorners = new List<(double x, double y)>
        {
            (-halfWidth, -halfHeight),
            (halfWidth, -halfHeight),
            (halfWidth, halfHeight),
            (-halfWidth, halfHeight)
        };

        // Transform to region frame (apply rotation and translation)
        var corners = new List<Point2D>();
        double cosTheta = Math.Cos(region.RotationAngle);
        double sinTheta = Math.Sin(region.RotationAngle);

        foreach (var (x, y) in localCorners)
        {
            double worldX = region.Center.X + x * cosTheta - y * sinTheta;
            double worldY = region.Center.Y + x * sinTheta + y * cosTheta;
            corners.Add(new Point2D(worldX, worldY));
        }

        return corners;
    }

    /// <summary>
    /// Find best matching pose from region centroids
    /// Returns the pose with the lowest fitting error
    /// </summary>
    /// <param name="regionCentroids">Centroids from each search region</param>
    /// <returns>Best detected pose, or null if no valid match found</returns>
    private Pose? FindMatchingPoses(List<List<Point2D>> regionCentroids)
    {
        // Generate all combinations of centroids (one from each region)
        // GenerateCombinations already validates that all regions have clusters
        var combinations = GenerateCombinations(regionCentroids);

        if (combinations.Count == 0)
        {
            return null;
        }

        Pose? bestPose = null;
        double bestError = double.MaxValue;
        int validPoseCount = 0;
        int acceptableErrorCount = 0;

        foreach (var combination in combinations)
        {
            // Try to estimate pose from this combination
            var pose = EstimatePoseFromPoints(_validatedMarkerPoints, combination);

            if (pose.HasValue)
            {
                validPoseCount++;
                // Calculate fitting error
                double error = CalculateFitError(_validatedMarkerPoints, combination, pose.Value);

                // Check if error is acceptable and better than previous best
                if (error <= maxFitError && error < bestError)
                {
                    acceptableErrorCount++;
                    bestPose = pose.Value;
                    bestError = error;
                }
            }
        }

        return bestPose;
    }

    /// <summary>
    /// Generate all valid combinations of centroids from different regions
    /// IMPORTANT: Each combination[i] must come from regionCentroids[i] to maintain correct correspondence
    /// with _validatedMarkerPoints[i]. Skipping regions is NOT allowed.
    /// </summary>
    /// <param name="regionCentroids">Centroids from each region</param>
    /// <returns>List of point combinations</returns>
    private List<List<Point2D>> GenerateCombinations(List<List<Point2D>> regionCentroids)
    {
        var combinations = new List<List<Point2D>>();

        // Ensure we have exactly N regions (one per reference point)
        int numReferencePoints = _validatedMarkerPoints.Length;
        
        if (regionCentroids.Count != numReferencePoints)
        {
            return combinations; // Return empty if mismatch
        }

        // Check if ALL required regions have at least one cluster
        // If ANY region is empty, we cannot form valid combinations
        for (int i = 0; i < numReferencePoints; i++)
        {
            if (regionCentroids[i].Count == 0)
            {
                return combinations; // Return empty - missing required points
            }
        }

        // Generate combinations where combination[i] comes from regionCentroids[i]
        void GenerateRecursive(int regionIndex, List<Point2D> current)
        {
            // Base case: processed all regions
            if (regionIndex == numReferencePoints)
            {
                combinations.Add([.. current]);
                return;
            }

            // Try each centroid from the CURRENT region only (no skipping!)
            foreach (var centroid in regionCentroids[regionIndex])
            {
                current.Add(centroid);
                GenerateRecursive(regionIndex + 1, current);
                current.RemoveAt(current.Count - 1);
            }
        }

        GenerateRecursive(0, []);
        return combinations;
    }

    /// <summary>
    /// Estimate marker pose from reference points and measured points
    /// Uses a simplified point set registration algorithm
    /// </summary>
    /// <param name="referencePoints">Reference points in marker frame</param>
    /// <param name="measuredPoints">Measured points in lidar frame</param>
    /// <returns>Estimated pose in lidar frame, or null if estimation fails</returns>
    private static Pose? EstimatePoseFromPoints(Point2D[] referencePoints, List<Point2D> measuredPoints)
    {
        if (referencePoints.Length != measuredPoints.Count)
        {
            return null;
        }

        // Handle single-point detection
        if (referencePoints.Length == 1)
        {
            // For single point: marker position = measured position - reference point offset
            // Since we don't know rotation, assume rotation = 0
            // Marker position = measured point - reference point (with rotation 0)
            double marker_X = measuredPoints[0].X - referencePoints[0].X;
            double marker_Y = measuredPoints[0].Y - referencePoints[0].Y;

            return new Pose
            {
                Position = new Vector3
                {
                    X = marker_X,
                    Y = marker_Y,
                    Z = 0
                },
                Orientation = CreateQuaternionFromYaw(0) // Cannot determine rotation from single point
            };
        }

        if (referencePoints.Length < 2)
        {
            return null;
        }

        // Calculate centroids
        var refCentroid = CalculateCentroid(referencePoints);
        var measCentroid = CalculateCentroid([.. measuredPoints]);

        // Center the point sets
        var refCentered = referencePoints.Select(p => new Point2D(p.X - refCentroid.X, p.Y - refCentroid.Y)).ToList();
        var measCentered = measuredPoints.Select(p => new Point2D(p.X - measCentroid.X, p.Y - measCentroid.Y)).ToList();

        // Calculate rotation using SVD-like approach (simplified for 2D)
        double theta = EstimateRotation(refCentered, measCentered);

        // Calculate marker origin position in lidar frame
        // Formula: t = M_centroid - R(theta) * R_centroid
        // This accounts for cases where reference centroid is not at marker origin (0,0)
        double cosTheta = Math.Cos(theta);
        double sinTheta = Math.Sin(theta);

        // R(theta) * refCentroid
        double rotatedRefX = refCentroid.X * cosTheta - refCentroid.Y * sinTheta;
        double rotatedRefY = refCentroid.X * sinTheta + refCentroid.Y * cosTheta;

        // t = measCentroid - R(theta) * refCentroid
        double markerX = measCentroid.X - rotatedRefX;
        double markerY = measCentroid.Y - rotatedRefY;

        var pose = new Pose
        {
            Position = new Vector3
            {
                X = markerX,
                Y = markerY,
                Z = 0
            },
            Orientation = CreateQuaternionFromYaw(theta)
        };

        return pose;
    }

    /// <summary>
    /// Estimate rotation angle between two centered point sets
    /// </summary>
    private static double EstimateRotation(List<Point2D> refCentered, List<Point2D> measCentered)
    {
        // Use cross-covariance method
        double sxx = 0, sxy = 0, syx = 0, syy = 0;

        for (int i = 0; i < refCentered.Count; i++)
        {
            sxx += measCentered[i].X * refCentered[i].X;
            sxy += measCentered[i].X * refCentered[i].Y;
            syx += measCentered[i].Y * refCentered[i].X;
            syy += measCentered[i].Y * refCentered[i].Y;
        }

        // Calculate rotation angle using atan2
        double theta = Math.Atan2(syx - sxy, sxx + syy);

        return theta;
    }

    /// <summary>
    /// Calculate fitting error between reference and measured points given a pose
    /// Error represents the average Euclidean distance between:
    /// - Predicted positions: where reference points SHOULD be (based on estimated pose)
    /// - Measured positions: where reflective markers were ACTUALLY detected by lidar
    ///
    /// Lower error = better match between model and reality
    /// </summary>
    /// <param name="referencePoints">Reference points in marker frame (model)</param>
    /// <param name="measuredPoints">Measured points in lidar frame (reality)</param>
    /// <param name="pose">Estimated marker pose to validate</param>
    /// <returns>Average distance error in meters</returns>
    private static double CalculateFitError(Point2D[] referencePoints, List<Point2D> measuredPoints, Pose pose)
    {
        if (referencePoints.Length != measuredPoints.Count)
            return double.MaxValue;

        double totalError = 0;
        double theta = pose.Orientation.ToYawRadian();

        // Pre-calculate cos and sin (optimization - computed once instead of per iteration)
        double cosTheta = Math.Cos(theta);
        double sinTheta = Math.Sin(theta);

        for (int i = 0; i < referencePoints.Length; i++)
        {
            // Transform reference point from marker frame to lidar frame using the estimated pose
            // Formula: P_predicted = t + R(theta) * P_reference
            double transformedX = pose.Position.X + referencePoints[i].X * cosTheta - referencePoints[i].Y * sinTheta;
            double transformedY = pose.Position.Y + referencePoints[i].X * sinTheta + referencePoints[i].Y * cosTheta;

            // Calculate Euclidean distance between predicted and measured positions
            double dx = transformedX - measuredPoints[i].X;
            double dy = transformedY - measuredPoints[i].Y;
            double distance = Math.Sqrt(dx * dx + dy * dy);

            totalError += distance;
        }

        double averageError = totalError / referencePoints.Length;
        
        return averageError;
    }

    /// <summary>
    /// Create a quaternion from yaw angle (rotation around Z axis)
    /// </summary>
    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>
    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    /// <summary>
    /// Transform a pose by another pose (pose composition)
    /// result = parentPose * childPose
    /// </summary>
    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)
        };
    }

    private static List<Point> ConvertLaserScanToPoints(LaserScan scan, double intensityThreshold, double angleStart, double angleEnd)
    {
        var points = new List<Point>();

        // Skip invalid scans
        if (scan.Ranges.Length == 0)
        {
            return points;
        }

        int invalidRanges = 0;
        int filteredByAngle = 0;
        int filteredByIntensity = 0;

        double normalizedStart = NormalizeAngle(angleStart);
        double normalizedEnd = NormalizeAngle(angleEnd);

        for (int i = 0; i < scan.Ranges.Length; i++)
        {
            double range = scan.Ranges[i];

            // Skip invalid ranges (out of bounds or NaN/Infinity)
            if (double.IsNaN(range) || double.IsInfinity(range) ||
                range < scan.RangeMin || range > scan.RangeMax)
            {
                invalidRanges++;
                continue;
            }

            // Calculate angle for this measurement
            double alpha = scan.AngleMin + (i * scan.AngleIncrement);

            // Normalize alpha to [-π, π] for proper comparison
            double normalizedAlpha = NormalizeAngle(alpha);

            // Handle angle wrapping around (e.g., from -π to π)
            if (normalizedStart <= normalizedEnd)
            {
                // Normal case: angleStart < angleEnd
                if (normalizedAlpha < normalizedStart || normalizedAlpha > normalizedEnd)
                {
                    filteredByAngle++;
                    continue;
                }
            }
            else
            {
                // Wrapped case: angleEnd < angleStart (e.g., 3π/4 to -3π/4)
                if (normalizedAlpha < normalizedStart && normalizedAlpha > normalizedEnd)
                {
                    filteredByAngle++;
                    continue;
                }
            }

            // Filter by intensity if threshold is set and intensities are available
            if (scan.Intensities.Length > i)
            {
                if (scan.Intensities[i] < intensityThreshold)
                {
                    filteredByIntensity++;
                    continue;
                }
            }

            points.Add(new Point(range * Math.Cos(alpha), range * Math.Sin(alpha), range, alpha));
        }

        return points;
    }

    public static Point2D CalculateCentroid(Point2D[] cluster)
    {
        if (cluster.Length == 0)
            return new Point2D(0, 0);

        double sumX = 0;
        double sumY = 0;

        foreach (var point in cluster)
        {
            sumX += point.X;
            sumY += point.Y;
        }

        return new Point2D(sumX / cluster.Length, sumY / cluster.Length);
    }

    public void Dispose()
    {
        // Disable detector (stops thread, unsubscribes events, disposes resources)
        Disable();

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

public readonly struct RectangleRegion(Point2D center, double width, double height, double rotationAngle)
{
    /// <summary>
    /// Center point of the rectangle
    /// </summary>
    public Point2D Center { get; init; } = center;

    /// <summary>
    /// Width of the rectangle (meters)
    /// </summary>
    public double Width { get; init; } = width;

    /// <summary>
    /// Height of the rectangle (meters)
    /// </summary>
    public double Height { get; init; } = height;

    /// <summary>
    /// Rotation angle in radians (counterclockwise from positive X-axis)
    /// </summary>
    public double RotationAngle { get; init; } = rotationAngle;

    public RectangleRegion(double centerX, double centerY, double width, double height, double rotationAngle)
        : this(new Point2D(centerX, centerY), width, height, rotationAngle)
    {
    }

    /// <summary>
    /// Check if a point is contained within this rectangle
    /// Uses coordinate transformation to handle rotation efficiently
    /// </summary>
    /// <param name="point">Point to check</param>
    /// <returns>True if point is inside the rectangle</returns>
    public bool ContainsPoint(double x, double y)
    {
        // Translate point to rectangle's local coordinate system (center at origin)
        double dx = x - Center.X;
        double dy = y - Center.Y;

        // Rotate point by negative rotation angle to align with rectangle axes
        double cosTheta = Math.Cos(-RotationAngle);
        double sinTheta = Math.Sin(-RotationAngle);

        double localX = dx * cosTheta - dy * sinTheta;
        double localY = dx * sinTheta + dy * cosTheta;

        // Check if point is within rectangle bounds
        double halfWidth = Width / 2.0;
        double halfHeight = Height / 2.0;

        return Math.Abs(localX) <= halfWidth && Math.Abs(localY) <= halfHeight;
    }
}