using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>
/// 从排序后的纯障碍物快照事务性构建环境占据图。
///
/// 注意：必需来源返回不可用或无效状态时，构建整体失败；可选来源仅记录其状态并继续构建。
/// </summary>
public sealed class EnvironmentMapBuilder
{
    /// <summary>
    /// 投影所有障碍物来源并栅格化为环境地图。
    ///
    /// 参数：request 包含 mm 世界边界、分辨率和来源列表；每个来源 ID 必须唯一且版本非负。
    /// 返回：成功时包含 <see cref="EnvironmentGridMap"/> 和全部来源状态；必需来源失败时返回失败结果而不产生可用地图。
    /// </summary>
    public EnvironmentMapBuildResult Build(MapBuildRequest request)
    {
        return Build(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
    }

    /// <summary>使用共享预算投影来源并栅格化；停止时不发布可写环境地图。</summary>
    internal EnvironmentMapBuildResult Build(MapBuildRequest request, PlanningOperationBudget budget)
    {
        if (budget == null) throw new ArgumentNullException(nameof(budget));
        PlanningOperationStopReason stopReason = budget.GetStopReason();
        if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, null);
        if (request == null || request.Bounds == null) return EnvironmentMapBuildResult.Failure("Map request and bounds are required.", null);
        if (request.ObstacleSources == null) return EnvironmentMapBuildResult.Failure("Obstacle source collection is required.", null);
        var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
        var results = new List<ObstacleProjectionResult>();
        string previousId = null;
        for (int i = 0; i < sources.Length; i++)
        {
            stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
            IMapObstacleSource source = sources[i];
            if (source == null || string.IsNullOrWhiteSpace(source.SourceId) || source.SourceVersion < 0)
                return EnvironmentMapBuildResult.Failure("Each source needs a non-empty id and non-negative version.", results);
            if (string.Equals(previousId, source.SourceId, StringComparison.Ordinal))
                return EnvironmentMapBuildResult.Failure("Obstacle source ids must be unique.", results);
            previousId = source.SourceId;
            ObstacleProjectionResult result;
            try { result = source.ProjectToWorld() ?? ObstacleProjectionResult.Invalid("Source returned no projection result."); }
            catch (Exception exception) { result = ObstacleProjectionResult.Invalid(exception.Message); }
            results.Add(result);
            stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
            if (source.IsRequired && (result.Status == ObstacleSourceStatus.Invalid || result.Status == ObstacleSourceStatus.Unavailable))
                return EnvironmentMapBuildResult.Failure("A required obstacle source failed: " + source.SourceId, results);
        }
        var map = new EnvironmentGridMap(request.Bounds, request.ResolutionMm);
        for (int i = 0; i < results.Count; i++)
            if (results[i].Status == ObstacleSourceStatus.Applied)
                for (int j = 0; j < results[i].Obstacles.Count; j++)
                {
                    if (!MapObstacleRasterizer.TryRasterize(map, results[i].Obstacles[j], budget, out stopReason))
                        return EnvironmentMapBuildResult.Stopped(stopReason, results);
                }
        return EnvironmentMapBuildResult.Success(map, results);
    }
}
