using System;

namespace MultiWheelC.TrajectoryPlanning.Mapping;

/// <summary>
/// 供粗路径规划使用的不可变地图快照。
///
/// 单位：世界查询方法使用 m；<see cref="Bounds"/> 和 <see cref="ResolutionMm"/> 保留原始 mm 数据。
/// 注意：世界坐标越界一律按占据处理，净距离为零。
/// </summary>
public sealed class PlanningGridMap
{
    private readonly byte[] _occupied;
    private readonly double[] _conservativeDistances;

    internal PlanningGridMap(MapBoundsMm bounds, float resolutionMm, int rows, int cols, byte[] occupied, double[] conservativeDistances,
        long snapshotId, bool planningReady, string planningBlockReason, string inputFingerprint, string occupancyHash)
    {
        Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
        _occupied = occupied; _conservativeDistances = conservativeDistances;
        SnapshotId = snapshotId; PlanningReady = planningReady; PlanningBlockReason = planningBlockReason ?? string.Empty;
        InputFingerprint = inputFingerprint ?? string.Empty; OccupancyHash = occupancyHash ?? string.Empty;
    }
    /// <summary>源环境图的世界边界，单位 mm，采用左闭右开规则。</summary>
    public MapBoundsMm Bounds { get; }
    /// <summary>源环境图的栅格边长，单位 mm。</summary>
    public float ResolutionMm { get; }
    /// <summary>规划世界查询对应的栅格边长，单位 m。</summary>
    public double ResolutionMeters { get { return ResolutionMm / 1000d; } }
    /// <summary>栅格行数。</summary>
    public int Rows { get; }
    /// <summary>栅格列数。</summary>
    public int Cols { get; }
    /// <summary>工厂为本次返回快照分配的单调编号，用于区分不同构建结果。</summary>
    public long SnapshotId { get; }
    /// <summary>地图是否允许进入粗路径规划。true 时可直接查询；false 时应先处理 <see cref="PlanningBlockReason"/>。</summary>
    public bool PlanningReady { get; }
    /// <summary>禁止规划的原因。<see cref="PlanningReady"/> 为 true 时为空字符串。</summary>
    public string PlanningBlockReason { get; }
    /// <summary>完整建图输入的稳定指纹，用于识别精确输入缓存命中。</summary>
    public string InputFingerprint { get; }
    /// <summary>占据栅格内容哈希，用于识别可复用的占据与距离数组。</summary>
    public string OccupancyHash { get; }

    /// <summary>查询世界位置是否占据。参数 xMeters、yMeters 单位为 m；位置越界时保守地返回 true。</summary>
    public bool IsOccupiedWorld(double xMeters, double yMeters)
    {
        return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) || _occupied[row * Cols + col] != 0;
    }
    /// <summary>
    /// 查询到最近障碍物的保守净距离下界。
    ///
    /// 参数：xMeters、yMeters 为世界坐标，单位 m。
    /// 返回：单位 m 的非负距离下界；地图内无障碍物时为正无穷，越界时为零。
    /// </summary>
    public double GetConservativeObstacleDistanceMeters(double xMeters, double yMeters)
    {
        return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) ? 0d : _conservativeDistances[row * Cols + col];
    }
    /// <summary>
    /// 将规划世界坐标转换为栅格索引。
    ///
    /// 参数：xMeters、yMeters 为世界坐标，单位 m；row、col 为输出索引。
    /// 返回：位置在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
    /// </summary>
    public bool TryWorldToGrid(double xMeters, double yMeters, out int row, out int col)
    {
        row = -1; col = -1;
        double xMm = xMeters * 1000d, yMm = yMeters * 1000d;
        if (xMm < Bounds.XMin || xMm >= Bounds.XMax || yMm < Bounds.YMin || yMm >= Bounds.YMax) return false;
        col = (int)Math.Floor((xMm - Bounds.XMin) / ResolutionMm);
        row = (int)Math.Floor((yMm - Bounds.YMin) / ResolutionMm);
        return row >= 0 && row < Rows && col >= 0 && col < Cols;
    }
    /// <summary>按行列索引查询占据状态。参数从零开始；任一索引越界时返回 true。</summary>
    public bool IsOccupied(int row, int col) { return row < 0 || row >= Rows || col < 0 || col >= Cols || _occupied[row * Cols + col] != 0; }
    internal byte[] CopyOccupied() { return (byte[])_occupied.Clone(); }
    internal bool OccupancyEquals(PlanningGridMap other)
    {
        if (other == null || Rows != other.Rows || Cols != other.Cols || ResolutionMm != other.ResolutionMm || !Bounds.Equals(other.Bounds) || _occupied.Length != other._occupied.Length) return false;
        for (int i = 0; i < _occupied.Length; i++) if (_occupied[i] != other._occupied[i]) return false;
        return true;
    }
    internal PlanningGridMap WithMetadata(long snapshotId, bool planningReady, string blockReason, string inputFingerprint, string occupancyHash)
    {
        return new PlanningGridMap(Bounds, ResolutionMm, Rows, Cols, _occupied, _conservativeDistances, snapshotId, planningReady, blockReason, inputFingerprint, occupancyHash);
    }
}