using System;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>
/// 建图阶段使用的可变环境占据栅格。
///
/// 单位：边界、世界查询和栅格边长均为 mm。
/// 注意：只有 <see cref="MapObstacleRasterizer"/> 可以写入占据状态；规划阶段应改用不可变的 <see cref="PlanningGridMap"/>。
/// </summary>
public sealed class EnvironmentGridMap
{
    private readonly byte[] _cells;
    private int _occupiedCount;

    /// <summary>
    /// 创建空的环境占据栅格。
    ///
    /// 参数：bounds 为左闭右开的世界边界，单位 mm；resolutionMm 为格边长，单位 mm。
    /// 返回：无；边界为空或分辨率不合法时抛出异常。
    /// </summary>
    public EnvironmentGridMap(MapBoundsMm bounds, float resolutionMm)
    {
        if (bounds == null) throw new ArgumentNullException(nameof(bounds));
        bounds.GetDimensions(resolutionMm, out int rows, out int cols);
        Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
        _cells = new byte[checked(rows * cols)];
    }

    /// <summary>地图世界边界，单位 mm，采用左闭右开规则。</summary>
    public MapBoundsMm Bounds { get; }
    /// <summary>单个栅格边长，单位 mm。</summary>
    public float ResolutionMm { get; }
    /// <summary>栅格行数，Y 方向从下限向上递增。</summary>
    public int Rows { get; }
    /// <summary>栅格列数，X 方向从下限向右递增。</summary>
    public int Cols { get; }
    /// <summary>当前已被标记为障碍的格数。</summary>
    public int OccupiedCount { get { return _occupiedCount; } }

    /// <summary>判断行列索引是否有效。参数 row、col 分别为从零开始的行和列；有效时返回 true。</summary>
    public bool IsInBounds(int row, int col) { return row >= 0 && row < Rows && col >= 0 && col < Cols; }
    /// <summary>判断世界坐标是否位于地图内。参数 xMm、yMm 单位为 mm；上边界与右边界返回 false。</summary>
    public bool IsWorldInBounds(float xMm, float yMm) { return Bounds.Contains(xMm, yMm); }

    /// <summary>
    /// 将世界坐标转换为栅格索引。
    ///
    /// 参数：xMm、yMm 为世界坐标，单位 mm；row、col 为输出索引。
    /// 返回：坐标在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
    /// </summary>
    public bool TryWorldToGrid(float xMm, float yMm, out int row, out int col)
    {
        row = -1; col = -1;
        if (!IsWorldInBounds(xMm, yMm)) return false;
        col = (int)Math.Floor(((double)xMm - Bounds.XMin) / ResolutionMm);
        row = (int)Math.Floor(((double)yMm - Bounds.YMin) / ResolutionMm);
        return IsInBounds(row, col);
    }

    /// <summary>查询栅格是否占据。越界索引按障碍处理，返回 true。</summary>
    public bool IsOccupied(int row, int col) { return !IsInBounds(row, col) || _cells[row * Cols + col] != 0; }
    /// <summary>按世界坐标查询占据状态。参数 xMm、yMm 单位为 mm；坐标越界时保守地返回 true。</summary>
    public bool IsOccupiedWorld(float xMm, float yMm)
    {
        return !TryWorldToGrid(xMm, yMm, out int row, out int col) || IsOccupied(row, col);
    }

    /// <summary>
    /// 获取一个栅格的世界坐标范围。
    ///
    /// 参数：row、col 为有效索引；xMin、xMax、yMin、yMax 为输出边界，单位 mm。
    /// 返回：无；索引越界时抛出 <see cref="ArgumentOutOfRangeException"/>。
    /// </summary>
    public void GetCellBounds(int row, int col, out float xMin, out float xMax, out float yMin, out float yMax)
    {
        if (!IsInBounds(row, col)) throw new ArgumentOutOfRangeException();
        xMin = Bounds.XMin + col * ResolutionMm;
        yMin = Bounds.YMin + row * ResolutionMm;
        xMax = Math.Min(Bounds.XMax, xMin + ResolutionMm);
        yMax = Math.Min(Bounds.YMax, yMin + ResolutionMm);
    }

    internal void MarkOccupied(int row, int col)
    {
        if (!IsInBounds(row, col)) return;
        int index = row * Cols + col;
        if (_cells[index] == 0) { _cells[index] = 1; _occupiedCount++; }
    }
    internal byte[] CopyCells() { return (byte[])_cells.Clone(); }
}
