namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Euclidean Distance Transform using Felzenszwalb-Huttenlocher algorithm O(n).
/// Shared implementation used by both MclService and ScanMatchingQualityEvaluator.
/// Reference: "Distance Transforms of Sampled Functions", Felzenszwalb & Huttenlocher, 2012.
/// </summary>
public static class DistanceTransformHelper
{
    #region Public API

    /// <summary>
    /// Compute Euclidean distance (in meters) from each cell to the nearest occupied cell.
    /// binaryMap[v,u] == 0 → occupied, != 0 → free.
    /// </summary>
    public static double[,] ComputeEuclidean(byte[,] binaryMap, int width, int height, double resolution)
    {
        // Step 1: Initialize squared distances (0 for occupied, inf for free)
        const int inf = int.MaxValue / 2;
        var distSq = new int[height, width];
        for (int v = 0; v < height; v++)
            for (int u = 0; u < width; u++)
                distSq[v, u] = binaryMap[v, u] == 0 ? 0 : inf;

        // Step 2: 1D distance transform along rows (horizontal pass)
        var tempDist = new int[Math.Max(width, height)];
        for (int y = 0; y < height; y++)
        {
            for (int x = 0; x < width; x++)
                tempDist[x] = distSq[y, x];

            DistanceTransform1D(tempDist, width);

            for (int x = 0; x < width; x++)
                distSq[y, x] = tempDist[x];
        }

        // Step 3: 1D distance transform along columns (vertical pass)
        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
                tempDist[y] = distSq[y, x];

            DistanceTransform1D(tempDist, height);

            for (int y = 0; y < height; y++)
                distSq[y, x] = tempDist[y];
        }

        // Step 4: Convert squared distance (in pixels) to Euclidean distance (in meters)
        var result = new double[height, width];
        for (int v = 0; v < height; v++)
            for (int u = 0; u < width; u++)
                result[v, u] = Math.Sqrt(distSq[v, u]) * resolution;

        return result;
    }

    #endregion

    #region 1D Transform

    /// <summary>
    /// 1D squared Euclidean distance transform using parabola lower envelope algorithm.
    /// Operates in-place on the input array.
    /// </summary>
    private static void DistanceTransform1D(int[] f, int n)
    {
        if (n == 0) return;

        // v stores parabola indices, z stores intersection points
        var v = new int[n];
        var z = new double[n + 1];
        int k = 0; // index of rightmost parabola
        v[0] = 0;
        z[0] = double.NegativeInfinity;
        z[1] = double.PositiveInfinity;

        // Build lower envelope of parabolas
        for (int q = 1; q < n; q++)
        {
            double s;
            while (true)
            {
                int vk = v[k];
                double fq = f[q];
                double fvk = f[vk];
                s = ((fq + q * q) - (fvk + vk * vk)) / (2.0 * (q - vk));

                if (s > z[k])
                    break;

                k--;
                if (k < 0)
                {
                    k = 0;
                    break;
                }
            }

            k++;
            v[k] = q;
            z[k] = s;
            z[k + 1] = double.PositiveInfinity;
        }

        // Fill in values of distance transform
        k = 0;
        var result = new int[n];
        for (int q = 0; q < n; q++)
        {
            while (z[k + 1] < q)
                k++;
            int vk = v[k];
            int dx = q - vk;
            result[q] = dx * dx + f[vk];
        }

        // Copy result back
        Array.Copy(result, f, n);
    }

    #endregion
}