BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/WallAlignmentHelper.cs

using Microsoft.Extensions.Logging;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for detecting walls from point cloud data and calculating alignment angles
/// </summary>
public static class WallAlignmentHelper
{
    private const double MIN_WALL_LENGTH = 1.0; // Minimum wall length in meters
    private const double RANSAC_INLIER_THRESHOLD = 0.05; // 5cm tolerance for RANSAC
    private const int RANSAC_ITERATIONS = 100;
    private const int MIN_INLIERS = 20; // Minimum points to consider a valid wall

    #region Internal Types

    /// <summary>
    /// Line representation: ax + by + c = 0 (normalized: a^2 + b^2 = 1)
    /// </summary>
    private struct Line
    {
        public double A { get; set; }
        public double B { get; set; }
        public double C { get; set; }

        /// <summary>
        /// Get angle of line relative to X-axis in radians
        /// Line equation: ax + by + c = 0
        /// Direction vector: (-b, a)
        /// Angle = atan2(a, -b)
        /// </summary>
        public double GetAngle() => Math.Atan2(A, -B);

        /// <summary>
        /// Get perpendicular distance from a point to this line
        /// </summary>
        public double DistanceToPoint(Vector3 point)
        {
            return Math.Abs(A * point.X + B * point.Y + C);
        }
    }

    /// <summary>
    /// Detected wall information
    /// </summary>
    private struct Wall
    {
        public Line Line { get; set; }
        public int InlierCount { get; set; }
        public double Length { get; set; }
        public Vector3 StartPoint { get; set; }
        public Vector3 EndPoint { get; set; }
    }

    #endregion

    #region Public API

    /// <summary>
    /// Detect the longest wall from a collection of 2D points and calculate the minimum
    /// rotation angle needed to align it with either the X or Y axis
    /// </summary>
    /// <param name="points">Point cloud in base_link frame</param>
    /// <param name="logger">Logger for debug information</param>
    /// <returns>
    /// Compensation angle in radians, or null if no valid wall found.
    /// This angle should be applied to robot orientation to make the wall parallel to X or Y axis.
    /// </returns>
    public static double? DetectWallAndCalculateCompensation(
        IReadOnlyList<Vector3> points,
        ILogger logger)
    {
        if (points == null || points.Count < MIN_INLIERS)
        {
            logger.LogWarning("WallAlignment: Insufficient points for wall detection (count: {Count})", points?.Count ?? 0);
            return null;
        }

        logger.LogInformation("WallAlignment: Processing {Count} points for wall detection", points.Count);

        // Detect all walls using RANSAC
        var walls = DetectWallsRANSAC(points, logger);

        if (walls.Count == 0)
        {
            logger.LogWarning("WallAlignment: No walls detected");
            return null;
        }

        // Find the longest wall
        var longestWall = walls.OrderByDescending(w => w.Length).First();

        logger.LogInformation(
            "WallAlignment: Longest wall found - Length: {Length:F2}m, Inliers: {Inliers}, Angle: {Angle:F2}rad ({AngleDeg:F2}°)",
            longestWall.Length,
            longestWall.InlierCount,
            longestWall.Line.GetAngle(),
            longestWall.Line.GetAngle() * 180.0 / Math.PI);

        // Calculate compensation angle
        var wallAngle = longestWall.Line.GetAngle();
        var compensationAngle = CalculateMinimumRotationToAxis(wallAngle);

        logger.LogInformation(
            "WallAlignment: Compensation angle: {Angle:F2}rad ({AngleDeg:F2}°)",
            compensationAngle,
            compensationAngle * 180.0 / Math.PI);

        return compensationAngle;
    }

    #endregion

    #region RANSAC Wall Detection

    /// <summary>
    /// Detect walls using RANSAC line fitting algorithm
    /// </summary>
    private static List<Wall> DetectWallsRANSAC(IReadOnlyList<Vector3> points, ILogger logger)
    {
        var walls = new List<Wall>();
        var unusedPoints = points.ToList();
        var random = new Random(DateTime.Now.Millisecond);

        // Iteratively find walls until not enough points remain
        while (unusedPoints.Count >= MIN_INLIERS)
        {
            Line bestLine = default;
            int bestInlierCount = 0;
            List<Vector3> bestInliers = [];

            // RANSAC iterations
            for (int iter = 0; iter < RANSAC_ITERATIONS; iter++)
            {
                // Randomly select 2 points
                if (unusedPoints.Count < 2) break;

                var idx1 = random.Next(unusedPoints.Count);
                var idx2 = random.Next(unusedPoints.Count);

                if (idx1 == idx2) continue;

                var p1 = unusedPoints[idx1];
                var p2 = unusedPoints[idx2];

                // Skip if points are too close
                var dx = p2.X - p1.X;
                var dy = p2.Y - p1.Y;
                var dist = Math.Sqrt(dx * dx + dy * dy);
                if (dist < 0.1) continue; // Minimum 10cm distance

                // Fit line through these 2 points
                var line = FitLineThroughPoints(p1, p2);

                // Count inliers
                var inliers = new List<Vector3>();
                foreach (var point in unusedPoints)
                {
                    if (line.DistanceToPoint(point) < RANSAC_INLIER_THRESHOLD)
                    {
                        inliers.Add(point);
                    }
                }

                // Update best model
                if (inliers.Count > bestInlierCount)
                {
                    bestInlierCount = inliers.Count;
                    bestInliers = inliers;
                    bestLine = line;
                }
            }

            // Check if we found a valid wall
            if (bestInlierCount < MIN_INLIERS)
            {
                break; // No more walls to find
            }

            // Calculate wall length (distance between furthest inlier points)
            var (startPoint, endPoint, length) = CalculateWallExtent(bestInliers);

            if (length < MIN_WALL_LENGTH)
            {
                // Wall too short, remove inliers and continue
                foreach (var inlier in bestInliers)
                {
                    unusedPoints.Remove(inlier);
                }
                continue;
            }

            // Valid wall found
            walls.Add(new Wall
            {
                Line = bestLine,
                InlierCount = bestInlierCount,
                Length = length,
                StartPoint = startPoint,
                EndPoint = endPoint
            });

            logger.LogDebug(
                "WallAlignment: Wall detected - Length: {Length:F2}m, Inliers: {Inliers}",
                length, bestInlierCount);

            // Remove inliers from unused points
            foreach (var inlier in bestInliers)
            {
                unusedPoints.Remove(inlier);
            }
        }

        return walls;
    }

    /// <summary>
    /// Fit a line through two points using line equation: ax + by + c = 0
    /// where a^2 + b^2 = 1 (normalized)
    /// </summary>
    private static Line FitLineThroughPoints(Vector3 p1, Vector3 p2)
    {
        var dx = p2.X - p1.X;
        var dy = p2.Y - p1.Y;
        var length = Math.Sqrt(dx * dx + dy * dy);

        if (length < 1e-6)
        {
            // Points are identical, return arbitrary line
            return new Line { A = 1, B = 0, C = -p1.X };
        }

        // Normal to line: (dy, -dx) / length (perpendicular to direction vector)
        var a = dy / length;
        var b = -dx / length;
        var c = -(a * p1.X + b * p1.Y);

        return new Line { A = a, B = b, C = c };
    }

    /// <summary>
    /// Calculate wall extent (start point, end point, and length)
    /// </summary>
    private static (Vector3 StartPoint, Vector3 EndPoint, double Length) CalculateWallExtent(
        List<Vector3> inliers)
    {
        if (inliers.Count < 2)
        {
            return (Vector3.Zero, Vector3.Zero, 0);
        }

        // Find two points that are furthest apart
        var maxDist = 0.0;
        var startIdx = 0;
        var endIdx = 0;

        for (int i = 0; i < inliers.Count; i++)
        {
            for (int j = i + 1; j < inliers.Count; j++)
            {
                var dx = inliers[j].X - inliers[i].X;
                var dy = inliers[j].Y - inliers[i].Y;
                var dist = Math.Sqrt(dx * dx + dy * dy);

                if (dist > maxDist)
                {
                    maxDist = dist;
                    startIdx = i;
                    endIdx = j;
                }
            }
        }

        return (inliers[startIdx], inliers[endIdx], maxDist);
    }

    #endregion

    #region Angle Compensation

    /// <summary>
    /// Calculate minimum rotation angle to align the wall with X or Y axis
    /// </summary>
    /// <param name="wallAngle">Wall angle in radians (relative to X-axis)</param>
    /// <returns>Compensation angle in radians</returns>
    private static double CalculateMinimumRotationToAxis(double wallAngle)
    {
        // Normalize angle to [-pi, pi]
        while (wallAngle > Math.PI) wallAngle -= 2 * Math.PI;
        while (wallAngle < -Math.PI) wallAngle += 2 * Math.PI;

        // Calculate rotation needed for each axis
        // For X-axis: wall should be at 0° or ±180°
        // For Y-axis: wall should be at ±90°

        var rotations = new[]
        {
            -wallAngle,                    // Align with X-axis (0°)
            Math.PI - wallAngle,           // Align with X-axis (180°)
            -Math.PI - wallAngle,          // Align with X-axis (-180°)
            Math.PI / 2 - wallAngle,       // Align with Y-axis (90°)
            -Math.PI / 2 - wallAngle       // Align with Y-axis (-90°)
        };

        // Find the smallest absolute rotation
        var minRotation = rotations.OrderBy(Math.Abs).First();

        // Normalize result to [-pi, pi]
        while (minRotation > Math.PI) minRotation -= 2 * Math.PI;
        while (minRotation < -Math.PI) minRotation += 2 * Math.PI;

        return minRotation;
    }

    #endregion

    #region Quaternion Helpers

    /// <summary>
    /// Create a quaternion from a yaw angle (rotation around Z-axis)
    /// </summary>
    public static Quaternion CreateQuaternionFromYaw(double yawRadians)
    {
        // Quaternion for rotation around Z-axis:
        // q = [0, 0, sin(yaw/2), cos(yaw/2)]
        var halfYaw = yawRadians / 2.0;
        return new Quaternion(
            x: 0,
            y: 0,
            z: Math.Sin(halfYaw),
            w: Math.Cos(halfYaw)
        );
    }

    /// <summary>
    /// Extract yaw angle from a quaternion
    /// </summary>
    public static double GetYawFromQuaternion(Quaternion q)
    {
        // Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
        return Math.Atan2(
            2.0 * (q.W * q.Z + q.X * q.Y),
            1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
        );
    }

    /// <summary>
    /// Combine current robot yaw with compensation angle
    /// </summary>
    public static Quaternion ApplyCompensation(Quaternion currentOrientation, double compensationAngle)
    {
        var currentYaw = GetYawFromQuaternion(currentOrientation);
        var newYaw = currentYaw + compensationAngle;
        return CreateQuaternionFromYaw(newYaw);
    }

    #endregion
}