using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;

/// <summary>
/// Converts point cloud (x,y) to MCL scan format (angle_min, angle_max, angle_increment, range_min, range_max, ranges[]).
/// Used when feeding TimedPointCloudData to MCL (bin by angle, take min range per bin).
/// </summary>
public static class MclScanHelper
{
    /// <summary>
    /// Transforms points from base_link frame to sensor (lidar) frame.
    /// baseFromLidar: p_base = R(yaw)*p_lidar + (tx, ty). So p_lidar = R(-yaw)*(p_base - (tx, ty)).
    /// </summary>
    /// <param name="pointsBase">Points (x, y) in base_link frame</param>
    /// <param name="tx">X of lidar origin in base_link (baseFromLidar translation)</param>
    /// <param name="ty">Y of lidar origin in base_link</param>
    /// <param name="yawRad">Yaw of lidar frame in base_link (radians)</param>
    /// <returns>Points (x, y) in sensor/lidar frame</returns>
    public static IEnumerable<(double x, double y)> BaseToSensorFrame(
        IEnumerable<(double x, double y)> pointsBase,
        double tx, double ty, double yawRad)
    {
        double c = Math.Cos(-yawRad);
        double s = Math.Sin(-yawRad);
        foreach (var (xb, yb) in pointsBase)
        {
            double dx = xb - tx;
            double dy = yb - ty;
            yield return (c * dx - s * dy, s * dx + c * dy);
        }
    }

    /// <summary>
    /// Bins points by angle and returns ranges (min range per bin). Angles in radians; ranges in meters.
    /// Points should be in sensor (lidar) frame so that angles are relative to sensor (required by MCL likelihood).
    /// </summary>
    public static (double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, double[] ranges) ConvertToScan(
        IEnumerable<Vector2> points,
        double angleMinDeg,
        double angleMaxDeg,
        int numBins,
        double rangeMin,
        double rangeMax)
    {
        double angleMin = angleMinDeg * Math.PI / 180.0;
        double angleMax = angleMaxDeg * Math.PI / 180.0;
        double angleIncrement = (angleMax - angleMin) / Math.Max(1, numBins);
        var bins = new List<double>[numBins];
        for (int i = 0; i < numBins; i++)
            bins[i] = [];

        foreach (var point in points)
        {
            var range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
            if (range < 1e-6) continue;
            var angle = Math.Atan2(point.Y, point.X);
            int bin = (int)Math.Floor((angle - angleMin) / angleIncrement);
            if (bin < 0) bin = 0;
            if (bin >= numBins) bin = numBins - 1;
            bins[bin].Add(range);
        }

        var ranges = new double[numBins];
        for (int i = 0; i < numBins; i++)
        {
            if (bins[i].Count == 0)
                ranges[i] = rangeMax;
            else
                ranges[i] = bins[i].Min();
        }
        return (angleMin, angleMax, angleIncrement, rangeMin, rangeMax, ranges);
    }
}