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

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>
/// 将障碍物几何保守投影为栅格占据状态的唯一写入入口。
///
/// 注意：调用方不能直接改写 <see cref="EnvironmentGridMap"/>；相交或贴边的栅格均按占据处理。
/// </summary>
public static class MapObstacleRasterizer
{
    /// <summary>
    /// 将一个有效障碍物栅格化到环境地图。
    ///
    /// 参数：map 为待写入的环境栅格；obstacle 为世界 mm 坐标的圆形或轴对齐矩形障碍物。
    /// 返回：无。地图或障碍物为空、障碍物无效、几何类型不受支持时抛出异常。
    /// 注意：该方法只增加占据格，不会清除已有障碍。
    /// </summary>
    public static void Rasterize(EnvironmentGridMap map, IMapObstacle obstacle)
    {
        if (!TryRasterize(map, obstacle, PlanningOperationBudget.Unlimited(CancellationToken.None), out _))
            throw new InvalidOperationException("Unbounded rasterization unexpectedly stopped.");
    }

    /// <summary>使用共享预算将障碍物写入环境栅格；停止时返回 false 且不发布环境地图。</summary>
    internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle, PlanningOperationBudget budget,
        out PlanningOperationStopReason stopReason)
    {
        if (map == null) throw new ArgumentNullException(nameof(map));
        if (obstacle == null || !obstacle.IsValid) throw new ArgumentException("Obstacle must be valid.", nameof(obstacle));
        if (budget == null) throw new ArgumentNullException(nameof(budget));
        stopReason = budget.GetStopReason();
        if (stopReason != PlanningOperationStopReason.None) return false;
        int workItemCount = 0;
        var circle = obstacle as CircleObstacle;
        if (circle != null) return TryRasterizeCircle(map, circle, budget, ref workItemCount, out stopReason);
        var rectangle = obstacle as AxisAlignedRectangleObstacle;
        if (rectangle != null) return TryRasterizeRectangle(map, rectangle, budget, ref workItemCount, out stopReason);
        throw new NotSupportedException("Unsupported map obstacle geometry.");
    }

    private static bool TryRasterizeCircle(EnvironmentGridMap map, CircleObstacle circle, PlanningOperationBudget budget,
        ref int workItemCount, out PlanningOperationStopReason stopReason)
    {
        GetCandidateRange(map, circle.CenterX - circle.RadiusMm, circle.CenterX + circle.RadiusMm,
            circle.CenterY - circle.RadiusMm, circle.CenterY + circle.RadiusMm,
            out int firstRow, out int lastRow, out int firstCol, out int lastCol);
        double radiusSquared = (double)circle.RadiusMm * circle.RadiusMm;
        for (int row = firstRow; row <= lastRow; row++)
        for (int col = firstCol; col <= lastCol; col++)
        {
            stopReason = budget.CheckEvery(ref workItemCount);
            if (stopReason != PlanningOperationStopReason.None) return false;
            map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
            double nearestX = Math.Max(xMin, Math.Min(circle.CenterX, xMax));
            double nearestY = Math.Max(yMin, Math.Min(circle.CenterY, yMax));
            double dx = circle.CenterX - nearestX;
            double dy = circle.CenterY - nearestY;
            if (dx * dx + dy * dy <= radiusSquared) map.MarkOccupied(row, col);
        }
        stopReason = PlanningOperationStopReason.None;
        return true;
    }

    private static bool TryRasterizeRectangle(EnvironmentGridMap map, AxisAlignedRectangleObstacle rectangle, PlanningOperationBudget budget,
        ref int workItemCount, out PlanningOperationStopReason stopReason)
    {
        GetCandidateRange(map, rectangle.XMin, rectangle.XMax, rectangle.YMin, rectangle.YMax,
            out int firstRow, out int lastRow, out int firstCol, out int lastCol);
        for (int row = firstRow; row <= lastRow; row++)
        for (int col = firstCol; col <= lastCol; col++)
        {
            stopReason = budget.CheckEvery(ref workItemCount);
            if (stopReason != PlanningOperationStopReason.None) return false;
            map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
            if (rectangle.XMax >= xMin && rectangle.XMin <= xMax && rectangle.YMax >= yMin && rectangle.YMin <= yMax)
                map.MarkOccupied(row, col);
        }
        stopReason = PlanningOperationStopReason.None;
        return true;
    }

    private static void GetCandidateRange(EnvironmentGridMap map, float xMin, float xMax, float yMin, float yMax,
        out int firstRow, out int lastRow, out int firstCol, out int lastCol)
    {
        if (xMax < map.Bounds.XMin || xMin > map.Bounds.XMax || yMax < map.Bounds.YMin || yMin > map.Bounds.YMax)
        { firstRow = 1; lastRow = 0; firstCol = 1; lastCol = 0; return; }
        // Geometry is closed for conservative rasterisation. Include the cell on
        // the lower side when a boundary lies exactly on a grid line.
        firstCol = Clamp((int)Math.Floor(((double)xMin - map.Bounds.XMin) / map.ResolutionMm) - 1, 0, map.Cols - 1);
        lastCol = Clamp((int)Math.Floor(((double)xMax - map.Bounds.XMin) / map.ResolutionMm), 0, map.Cols - 1);
        firstRow = Clamp((int)Math.Floor(((double)yMin - map.Bounds.YMin) / map.ResolutionMm) - 1, 0, map.Rows - 1);
        lastRow = Clamp((int)Math.Floor(((double)yMax - map.Bounds.YMin) / map.ResolutionMm), 0, map.Rows - 1);
    }
    private static int Clamp(int value, int min, int max) { return value < min ? min : value > max ? max : value; }
}