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

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>将建图阶段的 mm 环境栅格适配为规划阶段的不可变 m 查询快照。</summary>
public static class PlanningMapAdapter
{
    /// <summary>
    /// 创建规划地图快照及其保守距离场。
    ///
    /// 参数：environmentMap 为已经完成障碍物栅格化的环境图，坐标与分辨率单位均为 mm。
    /// 返回：不可变的 <see cref="PlanningGridMap"/>；其世界查询使用 m，初始元数据由工厂随后分配。
    /// </summary>
    public static PlanningGridMap Create(EnvironmentGridMap environmentMap)
    {
        if (!TryCreate(environmentMap, PlanningOperationBudget.Unlimited(CancellationToken.None), out PlanningGridMap map, out _))
            throw new InvalidOperationException("Unbounded planning-map adaptation unexpectedly stopped.");
        return map;
    }

    /// <summary>使用共享预算创建规划快照；停止时不返回部分地图。</summary>
    internal static bool TryCreate(EnvironmentGridMap environmentMap, PlanningOperationBudget budget,
        out PlanningGridMap map, out PlanningOperationStopReason stopReason)
    {
        if (environmentMap == null) throw new ArgumentNullException(nameof(environmentMap));
        if (budget == null) throw new ArgumentNullException(nameof(budget));
        map = null;
        stopReason = budget.GetStopReason();
        if (stopReason != PlanningOperationStopReason.None) return false;
        byte[] occupied = environmentMap.CopyCells();
        stopReason = budget.GetStopReason();
        if (stopReason != PlanningOperationStopReason.None) return false;
        if (!ObstacleDistanceField.TryCreate(occupied, environmentMap.Rows, environmentMap.Cols,
            environmentMap.ResolutionMm / 1000d, budget, out ObstacleDistanceField field, out stopReason))
            return false;
        map = new PlanningGridMap(environmentMap.Bounds, environmentMap.ResolutionMm, environmentMap.Rows, environmentMap.Cols,
            occupied, field.CopyDistances(), 0, false, "Map metadata has not been assigned.", string.Empty, string.Empty);
        stopReason = budget.GetStopReason();
        if (stopReason == PlanningOperationStopReason.None) return true;
        map = null;
        return false;
    }
}