using System;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>以 m 表示的障碍物净距离保守下界。</summary>
internal sealed class ObstacleDistanceField
{
    private readonly double[] _conservativeDistances;
    private ObstacleDistanceField(double[] conservativeDistances) { _conservativeDistances = conservativeDistances; }
    /// <summary>
    /// 从占据栅格创建距离场。
    ///
    /// 参数：occupied 为行主序占据数组；rows、cols 为其尺寸；resolutionMeters 为格边长，单位 m。
    /// 返回：每个格到最近障碍物的保守净距离下界，单位 m；全空地图中的每项为正无穷。
    /// </summary>
    public static ObstacleDistanceField Create(byte[] occupied, int rows, int cols, double resolutionMeters)
    {
        if (!TryCreate(occupied, rows, cols, resolutionMeters, PlanningOperationBudget.Unlimited(CancellationToken.None),
            out ObstacleDistanceField field, out _))
            throw new InvalidOperationException("Unbounded distance-field creation unexpectedly stopped.");
        return field;
    }

    /// <summary>使用共享预算创建距离场；停止时不返回部分距离数据。</summary>
    internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
        PlanningOperationBudget budget, out ObstacleDistanceField field, out PlanningOperationStopReason stopReason)
    {
        if (occupied == null) throw new ArgumentNullException(nameof(occupied));
        if (budget == null) throw new ArgumentNullException(nameof(budget));
        field = null;
        stopReason = budget.GetStopReason();
        if (stopReason != PlanningOperationStopReason.None) return false;
        bool hasObstacle = false;
        int workItemCount = 0;
        for (int i = 0; i < occupied.Length; i++)
        {
            stopReason = budget.CheckEvery(ref workItemCount);
            if (stopReason != PlanningOperationStopReason.None) return false;
            if (occupied[i] != 0) { hasObstacle = true; break; }
        }
        var distances = new double[occupied.Length];
        if (!hasObstacle)
        {
            for (int i = 0; i < distances.Length; i++)
            {
                stopReason = budget.CheckEvery(ref workItemCount);
                if (stopReason != PlanningOperationStopReason.None) return false;
                distances[i] = double.PositiveInfinity;
            }
            field = new ObstacleDistanceField(distances);
            stopReason = PlanningOperationStopReason.None;
            return true;
        }
        if (!EuclideanDistanceTransform.TryComputeSquaredDistances(occupied, rows, cols, budget, out double[] squared, out stopReason))
            return false;
        double conservativeOffset = Math.Sqrt(2d) * resolutionMeters;
        for (int i = 0; i < distances.Length; i++)
        {
            stopReason = budget.CheckEvery(ref workItemCount);
            if (stopReason != PlanningOperationStopReason.None) return false;
            distances[i] = Math.Max(0d, Math.Sqrt(squared[i]) * resolutionMeters - conservativeOffset);
        }
        field = new ObstacleDistanceField(distances);
        stopReason = PlanningOperationStopReason.None;
        return true;
    }
    internal double[] CopyDistances() { return (double[])_conservativeDistances.Clone(); }
}