using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.Navisworks.Api;
using NavisworksTransport.Utils.CoordinateSystem;

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 网格地图数据结构
    /// 用于将BIM模型转换为路径规划算法可使用的2D网格表示
    /// 支持动态坐标系（Z-Up 或 Y-Up）
    /// </summary>
    public class GridMap
    {
        /// <summary>
        /// 默认高度层查找容差（米）- 与配置中的安全间隙一致
        /// </summary>
        private const double DEFAULT_LAYER_TOLERANCE_METERS = 0.1;

        /// <summary>
        /// 当前使用的坐标系
        /// </summary>
        private readonly ICoordinateSystem _coordinateSystem;

        /// <summary>
        /// 当前网格采用的宿主坐标系类型。
        /// </summary>
        public CoordinateSystemType CoordinateSystemType => _coordinateSystem.Type;
        /// <summary>
        /// 网格宽度（单元格数量）
        /// </summary>
        public int Width { get; set; }

        /// <summary>
        /// 网格高度（单元格数量）
        /// </summary>
        public int Height { get; set; }

        /// <summary>
        /// 每个网格单元格的实际尺寸（模型单位）
        /// </summary>
        public double CellSize { get; set; }

        /// <summary>
        /// 网格原点（世界坐标系）
        /// </summary>
        public Point3D Origin { get; set; }

        /// <summary>
        /// 网格边界框（世界坐标系）
        /// </summary>
        public BoundingBox3D Bounds { get; set; }

        /// <summary>
        /// 路径规划的起点（用于Z坐标插值）
        /// </summary>
        public Point3D PlanningStartPoint { get; set; }

        /// <summary>
        /// 路径规划的终点（用于Z坐标插值）
        /// </summary>
        public Point3D PlanningEndPoint { get; set; }

        /// <summary>
        /// 是否已设置规划起点和终点
        /// </summary>
        public bool HasPlanningPoints { get; set; }

        /// <summary>
        /// 通道模型项集合（用于高度检测）
        /// 包含所有可通行的建筑结构：楼板、走廊、过道等
        /// 注意：楼板类通道的高度计算应使用顶面（物体行驶面）
        /// </summary>
        public IEnumerable<ModelItem> ChannelItems { get; set; }

        /// <summary>
        /// 通道高度检测器
        /// </summary>
        public ChannelHeightDetector HeightDetector { get; set; }

        /// <summary>
        /// 坡度分析器
        /// </summary>
        public SlopeAnalyzer SlopeAnalyzer { get; set; }

        /// <summary>
        /// 是否启用精确高度计算（网格构建时禁用以提升性能）
        /// </summary>
        public bool EnablePreciseHeightCalculation { get; set; } = false;

        /// <summary>
        /// 网格单元格数组 [x, y]
        /// </summary>
        public GridCell[,] Cells { get; set; }

        /// <summary>
        /// 构造函数（自动使用当前坐标系）
        /// </summary>
        /// <param name="bounds">世界坐标边界框</param>
        /// <param name="cellSize">网格单元格大小</param>
        public GridMap(BoundingBox3D bounds, double cellSize) 
            : this(bounds, cellSize, CoordinateSystemManager.Instance.Current)
        {
        }

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="bounds">世界坐标边界框</param>
        /// <param name="cellSize">网格单元格大小</param>
        /// <param name="coordinateSystem">坐标系</param>
        public GridMap(BoundingBox3D bounds, double cellSize, ICoordinateSystem coordinateSystem)
        {
            if (cellSize <= 0)
                throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize));
            if (coordinateSystem == null)
                throw new ArgumentNullException(nameof(coordinateSystem));

            Bounds = bounds ?? throw new ArgumentNullException(nameof(bounds));
            CellSize = cellSize;
            _coordinateSystem = coordinateSystem;
            
            // 使用坐标系获取水平范围
            var (min1, max1, min2, max2) = _coordinateSystem.GetHorizontalRange(bounds);
            double worldWidth = max1 - min1;
            double worldHeight = max2 - min2;
            
            Width = (int)Math.Ceiling(worldWidth / cellSize);
            Height = (int)Math.Ceiling(worldHeight / cellSize);
            
            // 设置原点为边界框的最小点
            Origin = bounds.Min;
            
            // 初始化网格单元格
            Cells = new GridCell[Width, Height];
            InitializeCells();
            
            // 初始化高度计算组件
            InitializeHeightCalculation();
            
            LogManager.Info($"[网格地图] 创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}");
        }

        /// <summary>
        /// 轻量构造函数，主要用于单元测试与脱离 Navisworks 原生边界句柄的纯坐标语义验证。
        /// </summary>
        public GridMap(Point3D origin, int width, int height, double cellSize, ICoordinateSystem coordinateSystem)
        {
            if (origin == null)
                throw new ArgumentNullException(nameof(origin));
            if (width <= 0)
                throw new ArgumentException("网格宽度必须大于0", nameof(width));
            if (height <= 0)
                throw new ArgumentException("网格高度必须大于0", nameof(height));
            if (cellSize <= 0)
                throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize));
            if (coordinateSystem == null)
                throw new ArgumentNullException(nameof(coordinateSystem));

            Origin = origin;
            Width = width;
            Height = height;
            CellSize = cellSize;
            _coordinateSystem = coordinateSystem;
            Bounds = null;

            Cells = new GridCell[Width, Height];
            InitializeCells();
            InitializeHeightCalculation();

            LogManager.Info($"[网格地图] 轻量创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}");
        }

        /// <summary>
        /// 初始化高度计算组件
        /// </summary>
        private void InitializeHeightCalculation()
        {
            try
            {
                HeightDetector = new ChannelHeightDetector(_coordinateSystem);
                SlopeAnalyzer = new SlopeAnalyzer();
                LogManager.Info("[网格地图] 高度计算组件初始化完成");
            }
            catch (Exception ex)
            {
                LogManager.Warning($"[网格地图] 初始化高度计算组件失败: {ex.Message}");
                EnablePreciseHeightCalculation = false;
            }
        }

        /// <summary>
        /// 初始化所有网格单元格为默认状态
        /// </summary>
        private void InitializeCells()
        {
            for (int x = 0; x < Width; x++)
            {
                for (int y = 0; y < Height; y++)
                {
                    var cell = new GridCell
                    {
                        CellType = CategoryAttributeManager.LogisticsElementType.空洞.ToString(), // 修改：默认为未知/空洞类型
                        IsInChannel = false,
                        HeightLayers = new List<HeightLayer>(),
                        ChannelType = ChannelType.Other
                    };
                    cell.Cost = cell.GetCost(); // 使用GetCost方法计算成本
                    Cells[x, y] = cell;
                }
            }
        }

        /// <summary>
        /// 世界坐标转换为网格坐标
        /// </summary>
        /// <param name="worldPosition">世界坐标点</param>
        /// <returns>网格坐标（可能超出边界）</returns>
        public GridPoint2D WorldToGrid(Point3D worldPosition)
        {
            // 使用坐标系获取水平坐标
            var (h1, h2) = _coordinateSystem.GetHorizontalCoords(worldPosition);
            var (originH1, originH2) = _coordinateSystem.GetHorizontalCoords(Origin);
            
            double gridX = (h1 - originH1) / CellSize;
            double gridY = (h2 - originH2) / CellSize;

            return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY));
        }

        /// <summary>
        /// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及高度计算）。
        /// 当前网格索引语义是采样节点：WorldToGrid 使用 Round 将点归属到最近节点。
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>世界2D坐标点（网格采样节点），高度设为0</returns>
        public Point3D GridToWorld2D(GridPoint2D gridPosition)
        {
            var (originH1, originH2) = _coordinateSystem.GetHorizontalCoords(Origin);
            double worldH1 = originH1 + gridPosition.X * CellSize;
            double worldH2 = originH2 + gridPosition.Y * CellSize;
            
            // 使用坐标系创建点，高度设为0
            return _coordinateSystem.CreatePoint(worldH1, worldH2, 0);
        }

        /// <summary>
        /// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的高度）
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>世界3D坐标点</returns>
        public Point3D GridToWorld3D(GridPoint2D gridPosition)
        {
            var world2D = GridToWorld2D(gridPosition);
            var cell = Cells[gridPosition.X, gridPosition.Y];

            // 获取高度（HeightLayer.Z 存储的是高度值，与坐标系无关）
            double elevation = 0;
            if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
            {
                elevation = cell.HeightLayers[0].Z;
            }

            // 使用坐标系设置高度
            return _coordinateSystem.SetElevation(world2D, elevation);
        }

        /// <summary>
        /// 获取宿主坐标点在当前网格坐标语义下的高程值。
        /// 注意：
        /// - 对 ZUp 文档，高程是 point.Z
        /// - 对 YUp 文档，高程是 point.Y
        /// </summary>
        public double GetWorldElevation(Point3D worldPoint)
        {
            if (worldPoint == null)
            {
                throw new ArgumentNullException(nameof(worldPoint));
            }

            return _coordinateSystem.GetElevation(worldPoint);
        }

        /// <summary>
        /// 在当前网格坐标语义下，为一个宿主点设置高程。
        /// </summary>
        public Point3D SetWorldElevation(Point3D worldPoint, double elevation)
        {
            if (worldPoint == null)
            {
                throw new ArgumentNullException(nameof(worldPoint));
            }

            return _coordinateSystem.SetElevation(worldPoint, elevation);
        }

        /// <summary>
        /// 根据网格坐标和指定高程创建宿主坐标点。
        /// </summary>
        public Point3D CreateWorldPoint(GridPoint2D gridPosition, double elevation)
        {
            Point3D world2D = GridToWorld2D(gridPosition);
            return _coordinateSystem.SetElevation(world2D, elevation);
        }

        /// <summary>
        /// 获取网格索引在宿主水平平面上的最近节点归属边界。
        /// 该边界必须与 WorldToGrid 的 Round 语义一致：索引点前后各半个 CellSize。
        /// </summary>
        public (double minH1, double maxH1, double minH2, double maxH2) GetGridCellPlanarBounds(GridPoint2D gridPosition)
        {
            Point3D centerPoint = GridToWorld2D(gridPosition);
            var (centerH1, centerH2) = _coordinateSystem.GetHorizontalCoords(centerPoint);
            return CalculateNearestNodePlanarBounds(centerH1, centerH2, CellSize);
        }

        /// <summary>
        /// 检查宿主空间线段是否穿过指定网格归属区域的内部。
        /// 只检查宿主水平投影；返回相交线段的参数范围，便于调用方按高度层再判定可通行性。
        /// </summary>
        public bool TryGetSegmentGridCellInteriorIntersection(
            Point3D start,
            Point3D end,
            GridPoint2D gridPosition,
            out double enterT,
            out double exitT)
        {
            if (start == null)
                throw new ArgumentNullException(nameof(start));
            if (end == null)
                throw new ArgumentNullException(nameof(end));

            enterT = 0.0;
            exitT = 1.0;

            var (startH1, startH2) = _coordinateSystem.GetHorizontalCoords(start);
            var (endH1, endH2) = _coordinateSystem.GetHorizontalCoords(end);
            var (minH1, maxH1, minH2, maxH2) = GetGridCellPlanarBounds(gridPosition);

            return TryGetSegmentRectangleInteriorIntersection(
                startH1,
                startH2,
                endH1,
                endH2,
                minH1,
                maxH1,
                minH2,
                maxH2,
                CellSize,
                out enterT,
                out exitT);
        }

        public static (double minH1, double maxH1, double minH2, double maxH2) CalculateNearestNodePlanarBounds(
            double centerH1,
            double centerH2,
            double cellSize)
        {
            double halfCell = cellSize * 0.5;

            return (
                centerH1 - halfCell,
                centerH1 + halfCell,
                centerH2 - halfCell,
                centerH2 + halfCell);
        }

        public static bool TryGetSegmentRectangleInteriorIntersection(
            double startH1,
            double startH2,
            double endH1,
            double endH2,
            double minH1,
            double maxH1,
            double minH2,
            double maxH2,
            double cellSize,
            out double enterT,
            out double exitT)
        {
            enterT = 0.0;
            exitT = 1.0;

            double epsilon = Math.Max(cellSize * 1e-9, 1e-9);
            minH1 += epsilon;
            maxH1 -= epsilon;
            minH2 += epsilon;
            maxH2 -= epsilon;

            if (minH1 > maxH1 || minH2 > maxH2)
            {
                return false;
            }

            double directionH1 = endH1 - startH1;
            double directionH2 = endH2 - startH2;

            return ClipSegmentToAxis(startH1, directionH1, minH1, maxH1, ref enterT, ref exitT) &&
                   ClipSegmentToAxis(startH2, directionH2, minH2, maxH2, ref enterT, ref exitT) &&
                   enterT <= exitT;
        }

        private static bool ClipSegmentToAxis(
            double start,
            double direction,
            double min,
            double max,
            ref double enterT,
            ref double exitT)
        {
            const double parallelTolerance = 1e-12;

            if (Math.Abs(direction) < parallelTolerance)
            {
                return start >= min && start <= max;
            }

            double t1 = (min - start) / direction;
            double t2 = (max - start) / direction;

            if (t1 > t2)
            {
                double temp = t1;
                t1 = t2;
                t2 = temp;
            }

            if (t1 > enterT)
                enterT = t1;
            if (t2 < exitT)
                exitT = t2;

            return enterT <= exitT;
        }



        /// <summary>
        /// 检查网格坐标是否在有效范围内
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>是否有效</returns>
        public bool IsValidGridPosition(GridPoint2D gridPosition)
        {
            return gridPosition.X >= 0 && gridPosition.X < Width &&
                   gridPosition.Y >= 0 && gridPosition.Y < Height;
        }

        /// <summary>
        /// 检查指定网格位置是否可通行
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>是否可通行</returns>
        public bool IsWalkable(GridPoint2D gridPosition)
        {
            if (!IsValidGridPosition(gridPosition))
                return false;

            return Cells[gridPosition.X, gridPosition.Y].HasAnyWalkableLayer();
        }

        /// <summary>
        /// 设置网格单元格的属性
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <param name="isWalkable">是否可通行</param>
        /// <param name="cost">遍历成本</param>
        /// <param name="speedLimit">限速</param>
        /// <param name="cellType">单元格类型</param>
        public void SetCell(GridPoint2D gridPosition, bool isWalkable, double cost, double speedLimit, string cellType)
        {
            if (!IsValidGridPosition(gridPosition))
                return;

            var worldPos = GridToWorld3D(gridPosition);
            var cell = new GridCell
            {
                CellType = cellType,
                IsInChannel = cellType == "通道",
                HeightLayers = new List<HeightLayer>(),
                ChannelType = cellType == "通道" ? ChannelType.Corridor : ChannelType.Other,
                SpeedLimit = speedLimit
            };

            // 应用验证规则并设置成本
            ApplyCellValidationRules(ref cell, cost);

            Cells[gridPosition.X, gridPosition.Y] = cell;
        }

        /// <summary>
        /// 将完整配置的GridCell放置到指定位置
        /// 这是推荐的GridCell设置方式，避免了分步设置的问题
        /// </summary>
        /// <param name="gridPosition">网格位置</param>
        /// <param name="cell">完整配置的GridCell</param>
        public void PlaceCell(GridPoint2D gridPosition, GridCell cell)
        {
            if (!IsValidGridPosition(gridPosition))
                return;

            // 应用验证规则（确保Unknown和障碍物类型的一致性）
            ApplyCellValidationRules(ref cell, cell.Cost);

            Cells[gridPosition.X, gridPosition.Y] = cell;
        }

        /// <summary>
        /// 为指定网格坐标添加高度层（追加模式，不覆盖）
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <param name="layer">要添加的高度层</param>
        public void AddHeightLayer(GridPoint2D gridPosition, HeightLayer layer)
        {
            if (!IsValidGridPosition(gridPosition))
                return;

            var cell = Cells[gridPosition.X, gridPosition.Y];
            
            // 确保HeightLayers列表已初始化
            if (cell.HeightLayers == null)
            {
                cell.HeightLayers = new List<HeightLayer>();
            }

            // 添加新的高度层
            cell.HeightLayers.Add(layer);

            // 更新单元格的基本属性（使用第一个层的属性）
            if (cell.HeightLayers.Count == 1)
            {
                // IsWalkable由层决定，不再在网格级别设置
                cell.CellType = layer.Type;
                cell.IsInChannel = true;
                cell.RelatedModelItem = layer.SourceItem;
                cell.SpeedLimit = layer.SpeedLimit;
                cell.Cost = cell.GetCost();
            }

            Cells[gridPosition.X, gridPosition.Y] = cell;
        }

        /// <summary>
        /// 查找包含指定Z坐标的高度层
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <param name="z">Z坐标（米）</param>
        /// <param name="tolerance">容差（米），默认0.1m</param>
        /// <returns>符合条件的高度层，如果没有则返回null</returns>
        public HeightLayer? FindLayerContainingElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            if (!IsValidGridPosition(gridPosition))
                return null;

            var cell = Cells[gridPosition.X, gridPosition.Y];
            if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
                return null;

            // 查找Z坐标最接近的层
            HeightLayer? bestLayer = null;
            double minDistance = double.MaxValue;

            foreach (var layer in cell.HeightLayers)
            {
                double distance = Math.Abs(layer.Z - elevation);
                if (distance < minDistance && distance <= tolerance)
                {
                    minDistance = distance;
                    bestLayer = layer;
                }
            }

            return bestLayer;
        }

        public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            return FindLayerContainingElevation(gridPosition, z, tolerance);
        }

        /// <summary>
        /// 获取指定网格位置的所有高度层
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>高度层列表，如果没有则返回空列表</returns>
        public List<HeightLayer> GetHeightLayers(GridPoint2D gridPosition)
        {
            if (!IsValidGridPosition(gridPosition))
                return new List<HeightLayer>();

            var cell = Cells[gridPosition.X, gridPosition.Y];
            return cell.HeightLayers ?? new List<HeightLayer>();
        }

        /// <summary>
        /// 应用GridCell验证规则
        /// 确保Unknown和障碍物类型永远不可通行，并设置合适的成本
        /// </summary>
        /// <param name="cell">要验证的GridCell（通过引用传递以便修改）</param>
        /// <param name="originalCost">原始成本值</param>
        private void ApplyCellValidationRules(ref GridCell cell, double originalCost)
        {
            // 🔥 关键验证：确保Unknown和障碍物类型永远不可通行
            if (cell.CellType == "空洞" ||
                cell.CellType == "障碍物")
            {
                // Unknown和障碍物没有可通行层，HasAnyWalkableLayer()自动返回false
                cell.Cost = double.MaxValue;
            }
            else
            {
                // 优先使用传入的cost参数，如果为默认值则用GetCost
                cell.Cost = (originalCost == double.MaxValue || originalCost == 0) ? cell.GetCost() : originalCost;
            }
        }

        /// <summary>
        /// 检查指定位置在指定高度是否可通行
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <param name="height">检查的高度</param>
        /// <returns>是否可通行</returns>
        public bool IsPassableAtHeight(GridPoint2D gridPosition, double height)
        {
            if (!IsValidGridPosition(gridPosition))
                return false;

            var cell = Cells[gridPosition.X, gridPosition.Y];
            if (!cell.HasAnyWalkableLayer())
                return false;

            // 检查是否有任何高度层满足高度要求
            if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
            {
                foreach (var layer in cell.HeightLayers)
                {
                    if (layer.PassableHeight.GetSpan() >= height)
                        return true;
                }
            }
            return false;
        }

        /// <summary>
        /// 检查指定3D点（包含Z坐标）是否在可通行的高度层范围内
        /// 用于路径优化中的斜线连接检查，确保路径不穿过障碍物层
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <param name="zCoord">Z坐标（模型单位）</param>
        /// <param name="tolerance">Z坐标容差（模型单位），默认0.1</param>
        /// <returns>是否可通行</returns>
        public bool IsPassableAtElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            if (!IsValidGridPosition(gridPosition))
                return false;

            var cell = Cells[gridPosition.X, gridPosition.Y];
            if (!cell.HasAnyWalkableLayer())
                return false;

            // 检查是否有任何可通行层包含这个Z坐标
            if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
            {
                foreach (var layer in cell.HeightLayers)
                {
                    // 跳过不可通行的层
                    if (!layer.IsWalkable)
                        continue;

                    // 检查Z坐标是否在这一层的范围内
                    double layerMinZ = layer.Z;
                    double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();

                    if (elevation >= layerMinZ - tolerance && elevation <= layerMaxZ + tolerance)
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS)
        {
            return IsPassableAtElevation(gridPosition, zCoord, tolerance);
        }

        /// <summary>
        /// 获取所有通道类型的网格单元
        /// </summary>
        /// <returns>通道网格单元列表</returns>
        public List<(GridPoint2D Position, GridCell Cell)> GetChannelCells()
        {
            var channelCells = new List<(GridPoint2D Position, GridCell Cell)>();
            
            for (int x = 0; x < Width; x++)
            {
                for (int y = 0; y < Height; y++)
                {
                    var cell = Cells[x, y];
                    if (cell.IsInChannel || cell.CellType == "通道")
                    {
                        channelCells.Add((new GridPoint2D(x, y), cell));
                    }
                }
            }

            return channelCells;
        }

        /// <summary>
        /// 设置通道模型项集合（用于精确高度计算）
        /// </summary>
        /// <param name="channelItems">通道模型项集合</param>
        public void SetChannelItems(IEnumerable<ModelItem> channelItems)
        {
            try
            {
                ChannelItems = channelItems?.ToList() ?? new List<ModelItem>();
                LogManager.Info($"[网格地图] 设置通道模型项: {(ChannelItems?.Count() ?? 0)} 个通道");
                
                // 网格构建阶段不启用精确高度计算，仅存储通道数据供后续使用
                LogManager.Info("[网格地图] 精确高度计算在网格构建时已禁用，将在路径校正时使用");
            }
            catch (Exception ex)
            {
                LogManager.Error($"[网格地图] 设置通道模型项时发生错误: {ex.Message}");
                ChannelItems = new List<ModelItem>();
                EnablePreciseHeightCalculation = false;
            }
        }

        /// <summary>
        /// 切换高度计算模式
        /// </summary>
        /// <param name="enablePrecise">是否启用精确计算</param>
        public void SetHeightCalculationMode(bool enablePrecise)
        {
            EnablePreciseHeightCalculation = enablePrecise && ChannelItems?.Any() == true;
            LogManager.Info($"[网格地图] 高度计算模式: {(EnablePreciseHeightCalculation ? "精确模式" : "传统模式")}");
        }

        /// <summary>
        /// 清除高度计算缓存
        /// </summary>
        public void ClearHeightCache()
        {
            try
            {
                HeightDetector?.ClearCache();
                SlopeAnalyzer?.ClearCache();
                LogManager.Info("[网格地图] 高度计算缓存已清除");
            }
            catch (Exception ex)
            {
                LogManager.Warning($"[网格地图] 清除高度缓存时发生错误: {ex.Message}");
            }
        }

        /// <summary>
        /// 获取高度计算统计信息
        /// </summary>
        /// <returns>统计信息字符串</returns>
        public string GetHeightCalculationStats()
        {
            try
            {
                var heightCacheCount = HeightDetector?.GetCacheCount() ?? 0;
                var slopeCacheCount = SlopeAnalyzer?.GetCacheCount() ?? 0;
                var channelCount = ChannelItems?.Count() ?? 0;
                
                return $"精确计算: {(EnablePreciseHeightCalculation ? "启用" : "禁用")}, " +
                       $"通道数: {channelCount}, " +
                       $"高度缓存: {heightCacheCount}, " +
                       $"坡度缓存: {slopeCacheCount}";
            }
            catch
            {
                return "统计信息获取失败";
            }
        }

        /// <summary>
        /// 获取网格单元格信息
        /// </summary>
        /// <param name="gridPosition">网格坐标</param>
        /// <returns>单元格信息</returns>
        public GridCell? GetCell(GridPoint2D gridPosition)
        {
            if (!IsValidGridPosition(gridPosition))
                return null;
                
            return Cells[gridPosition.X, gridPosition.Y];
        }

        /// <summary>
        /// 计算网格内存使用量（估算）
        /// </summary>
        /// <returns>内存使用量（字节）</returns>
        public long GetMemoryUsage()
        {
            // GridCell结构体大小估算：bool(1) + double(8) + enum(4) ≈ 16字节
            return Width * Height * 16L;
        }

        /// <summary>
        /// 获取网格统计信息
        /// </summary>
        /// <returns>统计信息字符串</returns>
        public string GetStatistics()
        {
            // 总计数
            int totalWalkable = 0;
            int totalNonWalkable = 0;
            
            // 按类型统计 - 可通行的
            int walkable_楼板 = 0;
            int walkable_门 = 0;
            int walkable_通道 = 0;
            int walkable_装卸区 = 0;
            int walkable_停车位 = 0;
            int walkable_楼梯 = 0;
            int walkable_电梯 = 0;
            int walkable_走廊 = 0;
            int walkable_Other = 0;
            
            // 按类型统计 - 不可通行的
            int nonWalkable_空洞 = 0;
            int nonWalkable_障碍物 = 0;
            int nonWalkable_楼板 = 0;
            int nonWalkable_门 = 0;
            int nonWalkable_通道 = 0;
            int nonWalkable_Other = 0;

            for (int x = 0; x < Width; x++)
            {
                for (int y = 0; y < Height; y++)
                {
                    var cell = Cells[x, y];
                    
                    if (cell.HasAnyWalkableLayer())
                    {
                        totalWalkable++;
                        // 使用字符串比较（支持自定义类别）
                        if (cell.CellType == "楼板") walkable_楼板++;
                        else if (cell.CellType == "门") walkable_门++;
                        else if (cell.CellType == "通道") walkable_通道++;
                        else if (cell.CellType == "装卸区") walkable_装卸区++;
                        else if (cell.CellType == "停车位") walkable_停车位++;
                        else if (cell.CellType == "楼梯") walkable_楼梯++;
                        else if (cell.CellType == "电梯") walkable_电梯++;
                        else if (cell.CellType == "走廊") walkable_走廊++;
                        else walkable_Other++;
                    }
                    else
                    {
                        totalNonWalkable++;
                        // 使用字符串比较（支持自定义类别）
                        if (cell.CellType == "空洞") nonWalkable_空洞++;
                        else if (cell.CellType == "障碍物") nonWalkable_障碍物++;
                        else if (cell.CellType == "楼板") nonWalkable_楼板++;
                        else if (cell.CellType == "门") nonWalkable_门++;
                        else if (cell.CellType == "通道") nonWalkable_通道++;
                        else nonWalkable_Other++;
                    }
                }
            }

            var stats = $"=== 网格统计详细信息 ===\n";
            stats += $"网格尺寸: {Width}x{Height} (总计 {Width * Height} 个单元格)\n";
            stats += $"总体分布: 可通行 {totalWalkable} 个, 不可通行 {totalNonWalkable} 个\n";
            
            stats += $"【可通行单元格详细统计】\n";
            stats += $"  楼板: {walkable_楼板}, 通道: {walkable_通道}, 走廊: {walkable_走廊}\n";
            stats += $"  门: {walkable_门}, 装卸区: {walkable_装卸区}, 停车位: {walkable_停车位}\n";
            stats += $"  楼梯: {walkable_楼梯}, 电梯: {walkable_电梯}, 其他: {walkable_Other}\n";
            
            stats += $"【不可通行单元格详细统计】\n";
            stats += $"  空洞: {nonWalkable_空洞}, 障碍物: {nonWalkable_障碍物}\n";
            stats += $"  不可通行楼板: {nonWalkable_楼板}, 不可通行门: {nonWalkable_门}\n";
            stats += $"  不可通行通道: {nonWalkable_通道}, 其他不可通行: {nonWalkable_Other}\n";
            
            stats += $"内存使用: {GetMemoryUsage() / 1024.0:F1} KB";

            return stats;
        }
    }

    /// <summary>
    /// 网格单元格结构体
    /// 存储单个网格单元格的状态信息
    /// </summary>
    public struct GridCell
    {
        /// <summary>
        /// 遍历成本（1.0为标准成本）
        /// </summary>
        public double Cost { get; set; }

        /// <summary>
        /// 单元格类型（类别ID字符串，支持内置和自定义类别）
        /// </summary>
        public string CellType { get; set; }

        /// <summary>
        /// 相关的模型项引用（可选）
        /// </summary>
        public ModelItem RelatedModelItem { get; set; }

        /// <summary>
        /// 是否在通道内 - A*改进方案新增字段
        /// </summary>
        public bool IsInChannel { get; set; }

        /// <summary>
        /// 多高度层列表 - 支持同一网格坐标多个高度层（如楼梯下方通道）
        /// </summary>
        public List<HeightLayer> HeightLayers { get; set; }

        /// <summary>
        /// 通道类型 - 用于区分不同类型的通道
        /// </summary>
        public ChannelType ChannelType { get; set; }

        /// <summary>
        /// 限速（米/秒），0表示未设置限速
        /// </summary>
        public double SpeedLimit { get; set; }

        /// <summary>
        /// 检查是否至少有一个可通行的高度层
        /// </summary>
        /// <returns>如果至少有一层可通行返回true，否则返回false</returns>
        public bool HasAnyWalkableLayer()
        {
            return HeightLayers != null && HeightLayers.Any(layer => layer.IsWalkable);
        }

        /// <summary>
        /// 根据物流类型获取通行成本
        /// </summary>
        /// <returns>通行成本</returns>
        public double GetCost()
        {
            if (!HasAnyWalkableLayer())
                return double.MaxValue;

            // 使用类别定义获取权重（支持内置和自定义类别）
            return CategoryAttributeManager.GetCategoryWeight(CellType);
        }

        /// <summary>
        /// 创建障碍物单元格
        /// </summary>
        /// <returns>障碍物单元格</returns>
        public static GridCell CreateObstacle()
        {
            var cell = new GridCell
            {
                CellType = CategoryAttributeManager.LogisticsElementType.障碍物.ToString(),
                IsInChannel = false,
                HeightLayers = new List<HeightLayer>(), // 空列表，HasAnyWalkableLayer()返回false
                ChannelType = ChannelType.Other,
                SpeedLimit = 0
            };
            cell.Cost = cell.GetCost();
            return cell;
        }

        /// <summary>
        /// 创建门单元格
        /// </summary>
        /// <param name="isOpen">门是否打开</param>
        /// <returns>门单元格</returns>
        public static GridCell CreateDoor(bool isOpen = true)
        {
            var cell = new GridCell
            {
                CellType = CategoryAttributeManager.LogisticsElementType.门.ToString(),
                IsInChannel = isOpen,
                HeightLayers = new List<HeightLayer>(),
                ChannelType = ChannelType.Other,
                SpeedLimit = 0
            };
            cell.Cost = cell.GetCost();
            return cell;
        }

        /// <summary>
        /// 创建通道单元格
        /// </summary>
        /// <param name="channelType">通道类型</param>
        /// <returns>通道单元格</returns>
        public static GridCell CreateChannel(ChannelType channelType = ChannelType.Corridor)
        {
            var cell = new GridCell
            {
                CellType = CategoryAttributeManager.LogisticsElementType.通道.ToString(),
                IsInChannel = true,
                HeightLayers = new List<HeightLayer>(),
                ChannelType = channelType,
                SpeedLimit = 0
            };
            cell.Cost = cell.GetCost();
            return cell;
        }
    }

    /// <summary>
    /// 高度层结构体
    /// 用于支持多高度层的网格单元格
    /// </summary>
    public struct HeightLayer
    {
        /// <summary>
        /// 该层的Z坐标（模型单位）
        /// </summary>
        public double Z { get; set; }

        /// <summary>
        /// 可通行高度范围（用于物体检查）
        /// </summary>
        public HeightInterval PassableHeight { get; set; }

        /// <summary>
        /// 来源模型项
        /// </summary>
        public ModelItem SourceItem { get; set; }

        /// <summary>
        /// 限速（米/秒），0表示未设置限速
        /// </summary>
        public double SpeedLimit { get; set; }

        /// <summary>
        /// 层类型（类别ID字符串，支持自定义类别）
        /// </summary>
        public string Type { get; set; }

        /// <summary>
        /// 是否是边界层（用于膨胀计算）
        /// 边界定义：当前网格层数 > 邻居网格层数，且层间高度差超过阈值
        /// </summary>
        public bool IsBoundary { get; set; }

        /// <summary>
        /// 该层是否可通行（用于多层膨胀）
        /// true: 该层可以通行
        /// false: 该层被膨胀影响，不可通行
        /// </summary>
        public bool IsWalkable { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        public HeightLayer(double z, HeightInterval passableHeight, ModelItem sourceItem, double speedLimit, string type)
        {
            Z = z;
            PassableHeight = passableHeight;
            SourceItem = sourceItem;
            SpeedLimit = speedLimit;
            Type = type;
            IsBoundary = false; // 默认不是边界
            IsWalkable = true;  // 默认可通行，通过膨胀算法修改
        }
    }

    /// <summary>
    /// 高度区间结构体 - 用于2.5D路径规划
    /// 表示某个网格位置的可通行高度范围
    /// </summary>
    public struct HeightInterval
    {
        /// <summary>
        /// 最小高度（米）
        /// </summary>
        public double MinZ { get; set; }

        /// <summary>
        /// 最大高度（米）
        /// </summary>
        public double MaxZ { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="minZ">最小高度</param>
        /// <param name="maxZ">最大高度</param>
        public HeightInterval(double minZ, double maxZ)
        {
            MinZ = minZ;
            MaxZ = maxZ;
        }

        /// <summary>
        /// 检查指定高度是否在此区间内
        /// </summary>
        /// <param name="height">要检查的高度</param>
        /// <returns>是否在区间内</returns>
        public bool Contains(double height)
        {
            return height >= MinZ && height <= MaxZ;
        }

        /// <summary>
        /// 检查指定高度范围是否与此区间有重叠
        /// </summary>
        /// <param name="minHeight">要检查的最小高度</param>
        /// <param name="maxHeight">要检查的最大高度</param>
        /// <returns>是否有重叠</returns>
        public bool Overlaps(double minHeight, double maxHeight)
        {
            return maxHeight >= MinZ && minHeight <= MaxZ;
        }

        /// <summary>
        /// 获取区间的高度跨度
        /// </summary>
        /// <returns>高度跨度</returns>
        public double GetSpan()
        {
            return MaxZ - MinZ;
        }

        /// <summary>
        /// 获取区间的中心高度
        /// </summary>
        /// <returns>中心高度</returns>
        public double GetCenter()
        {
            return (MinZ + MaxZ) / 2.0;
        }

        /// <summary>
        /// 重写ToString方法
        /// </summary>
        /// <returns>字符串表示</returns>
        public override string ToString()
        {
            return $"[{MinZ:F2}m - {MaxZ:F2}m]";
        }
    }

    /// <summary>
    /// 通道覆盖信息
    /// 存储通道构建器生成的网格地图和相关数据
    /// </summary>
    public class ChannelCoverage
    {
        /// <summary>
        /// 网格地图
        /// </summary>
        public GridMap GridMap { get; set; }

        /// <summary>
        /// 通道物品列表
        /// </summary>
        public List<ModelItem> ChannelItems { get; set; } = new List<ModelItem>();

        /// <summary>
        /// 总边界框
        /// </summary>
        public BoundingBox3D TotalBounds { get; set; }

        /// <summary>
        /// 可通行网格的最小Z坐标（基于实际网格位置）
        /// </summary>
        public double WalkableMinZ { get; set; } = double.MaxValue;

        /// <summary>
        /// 可通行网格的最大Z坐标（基于实际网格位置）
        /// </summary>
        public double WalkableMaxZ { get; set; } = double.MinValue;

        /// <summary>
        /// 可通行网格数量
        /// </summary>
        public int WalkableGridCount { get; set; } = 0;

        /// <summary>
        /// 获取统计信息
        /// </summary>
        /// <returns>统计信息字符串</returns>
        public string GetStatistics()
        {
            var channelCellCount = 0;
            if (GridMap?.Cells != null)
            {
                for (int x = 0; x < GridMap.Width; x++)
                {
                    for (int y = 0; y < GridMap.Height; y++)
                    {
                        if (GridMap.Cells[x, y].CellType == "通道")
                        {
                            channelCellCount++;
                        }
                    }
                }
            }

            var zRangeInfo = WalkableGridCount > 0 
                ? $", Z范围: [{WalkableMinZ:F2}, {WalkableMaxZ:F2}], 可通行网格: {WalkableGridCount}个"
                : ", Z范围: 未计算";

            return $"通道覆盖统计: {ChannelItems.Count} 个通道物品, {channelCellCount} 个通道网格单元, " +
                   $"网格大小: {GridMap?.Width}x{GridMap?.Height}{zRangeInfo}";
        }
    }

    /// <summary>
    /// 自动路径规划异常
    /// </summary>
    public class AutoPathPlanningException : Exception
    {
        public bool IsBusinessError { get; }
        
        public AutoPathPlanningException(string message, bool isBusinessError = false) : base(message) 
        { 
            IsBusinessError = isBusinessError;
        }
        
        public AutoPathPlanningException(string message, Exception innerException, bool isBusinessError = false) : base(message, innerException) 
        { 
            IsBusinessError = isBusinessError;
        }
    }
    
    /// <summary>
    /// 路径规划业务异常（用户选择导致的正常失败）
    /// </summary>
    public class PathPlanningBusinessException : AutoPathPlanningException
    {
        public PathPlanningBusinessException(string message) : base(message, true) { }
    }
}
