using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.Navisworks.Api;
using Roy_T.AStar.Grids;
using Roy_T.AStar.Primitives;
using Roy_T.AStar.Paths;
using NavisworksTransport.Utils;
using NavisworksTransport.Core.Config;
using NavisworksTransport.Utils.CoordinateSystem;

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 路径查找结果
    /// </summary>
    public class PathFindingResult
    {
        /// <summary>
        /// 路径点列表
        /// </summary>
        public List<Point3D> PathPoints { get; set; } = new List<Point3D>();
        
        /// <summary>
        /// 路径是否完整
        /// </summary>
        public bool IsComplete { get; set; } = true;
        
        /// <summary>
        /// 原始终点（用户指定的）
        /// </summary>
        public Point3D OriginalEndPoint { get; set; }
        
        /// <summary>
        /// 实际到达点
        /// </summary>
        public Point3D ActualEndPoint { get; set; }
        
        /// <summary>
        /// 完成百分比
        /// </summary>
        public double CompletionPercentage { get; set; } = 100.0;
        
        /// <summary>
        /// 路径查找是否成功
        /// </summary>
        public bool IsSuccess => PathPoints.Count >= 2;
        
        /// <summary>
        /// 失败消息
        /// </summary>
        public string ErrorMessage { get; set; }
        
        /// <summary>
        /// 创建失败结果
        /// </summary>
        public static PathFindingResult Failure(string errorMessage)
        {
            return new PathFindingResult
            {
                PathPoints = new List<Point3D>(),
                IsComplete = false,
                CompletionPercentage = 0.0,
                ErrorMessage = errorMessage
            };
        }
    }

    /// <summary>
    /// 自动路径查找器
    /// 基于A*算法的路径规划核心类
    /// 支持传统模式和真3D图路径规划
    /// </summary>
    public class AutoPathFinder
    {
        /// <summary>
        /// 缓坡速度系数 - 当坡度在 3cm 到 25cm 之间时使用
        /// </summary>
        private const float SLOPE_SPEED_FACTOR_GENTLE = 0.8f;

        /// <summary>
        /// 楼梯速度系数 - 当坡度在 25cm 到 50cm 之间时使用
        /// </summary>
        private const float SLOPE_SPEED_FACTOR_STAIR = 0.5f;

        /// <summary>
        /// 陡坡速度系数 - 当坡度超过 50cm 时使用
        /// </summary>
        private const float SLOPE_SPEED_FACTOR_STEEP = 0.1f;

        /// <summary>
        /// A*高度惩罚因子 - 用于计算距离目标高度的速度扣减
        /// </summary>
        private const double HEIGHT_PENALTY_FACTOR = 0.1;

        /// <summary>
        /// 转弯角度阈值（弧度）- 用于检测路径转弯
        /// </summary>
        private const double TURN_ANGLE_THRESHOLD_RADIANS = 0.1;

        /// <summary>
        /// 获取水平边连接允许的最大高度差（米）
        /// 从配置文件读取，足以支持台阶连接（约0.15米）和地面到楼梯底部的连接（约0.3米）
        /// 使用时需转换为模型单位
        /// </summary>
        private double GetMaxHeightDiffMeters()
        {
            return ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters;
        }

        /// <summary>
        /// 3D节点信息（存储每个A*节点对应的3D坐标和类型）
        /// </summary>
        private class Node3DInfo
        {
            public int X { get; set; }
            public int Y { get; set; }
            public int LayerIndex { get; set; }
            public double Z { get; set; }
            public string CellType { get; set; }
        }

        /// <summary>
        /// 3D图构建结果
        /// </summary>
        private class Graph3DResult
        {
            /// <summary>
            /// 节点字典：(x, y, layerIndex) -> A*节点
            /// </summary>
            public Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node> Nodes { get; set; }

            /// <summary>
            /// 节点信息字典：A*节点 -> 3D信息
            /// </summary>
            public Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo> NodeInfo { get; set; }

            /// <summary>
            /// 所有节点列表（用于查找最近节点）
            /// </summary>
            public List<Roy_T.AStar.Graphs.Node> AllNodes { get; set; }

            /// <summary>
            /// 最大速度
            /// </summary>
            public Velocity MaxVelocity { get; set; }
        }

        /// <summary>
        /// 真3D路径规划（使用通道覆盖数据、高度约束和路径策略）
        /// </summary>
        public PathFindingResult FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, PathStrategy strategy = PathStrategy.Shortest)
        {
            try
            {
                LogManager.Info("[3D路径规划] 开始执行");

                // 构建3D图
                var graph3D = BuildGraphWithStrategy(gridMap, channelCoverage, objectHeight, start, end, strategy);

                // 执行A*搜索
                var pathResult = ExecuteAStarOnGraph(start, end, gridMap, graph3D);

                // 优化路径
                pathResult = OptimizePath(pathResult, gridMap);

                return pathResult;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[3D路径规划] 发生异常: {ex.Message}\n{ex.StackTrace}");
                throw; // 直接抛出异常，不返回默认路径
            }
        }

        /// <summary>
        /// 速度计算委托：根据当前节点、邻居节点和其他上下文信息计算边的速度
        /// </summary>
        private delegate float SpeedCalculator(
            int currentX, int currentY, int currentLayerIndex, double currentZ,
            int neighborX, int neighborY, int neighborLayerIndex, double neighborZ,
            double heightDiff, double baseSpeed, double maxHeightDiff,
            Point3D startPos, Point3D endPos);

        /// <summary>
        /// 构建3D图（每个(x,y,layerIndex)作为独立节点）
        /// </summary>
        private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, SpeedCalculator speedCalculator = null)
        {
            LogManager.Info("[3D图构建] 开始构建真3D图");

            var nodes = new Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node>();
            var nodeInfo = new Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo>();
            var allNodes = new List<Roy_T.AStar.Graphs.Node>();

            double baseSpeed = 5.0; // km/h
            float maxSpeed = (float)baseSpeed;

            // 使用配置的模型单位值
            double maxHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff;
            LogManager.Info($"[3D图构建] 高度差阈值: {ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters}米 = {maxHeightDiff:F2}模型单位");

            // === 阶段1：创建所有3D节点 ===
            int totalNodesCreated = 0;
            for (int x = 0; x < gridMap.Width; x++)
            {
                for (int y = 0; y < gridMap.Height; y++)
                {
                    var cell = gridMap.Cells[x, y];
                    if (!cell.HasAnyWalkableLayer())
                        continue;

                    // 为每个高度层创建独立节点
                    for (int li = 0; li < cell.HeightLayers.Count; li++)
                    {
                        var layer = cell.HeightLayers[li];

                        // 检查该层是否可通行（膨胀影响）
                        if (!layer.IsWalkable)
                            continue;

                        // 楼梯/电梯区域跳过层0（下方楼板，不是可行走表面）
                        if (li == 0 && (layer.Type == "楼梯" ||
                                         layer.Type == "电梯"))
                        {
                            continue;
                        }

                        if (layer.PassableHeight.GetSpan() < objectHeight)
                            continue;

                        // 使用2D位置（用于A*启发式）
                        var position = new Position(x, y);
                        var node = new Roy_T.AStar.Graphs.Node(position);

                        nodes[(x, y, li)] = node;
                        nodeInfo[node] = new Node3DInfo
                        {
                            X = x,
                            Y = y,
                            LayerIndex = li,
                            Z = layer.Z,
                            CellType = layer.Type
                        };
                        allNodes.Add(node);
                        totalNodesCreated++;
                    }
                }
            }

            LogManager.Info($"[3D图构建] 阶段1完成：创建了 {totalNodesCreated} 个3D节点");

            // === 阶段2：创建水平边（同层的相邻节点，高度差在阈值内）===
            var directions = new[] { (1, 0), (-1, 0), (0, 1), (0, -1) };
            int horizontalEdges = 0;

            for (int x = 0; x < gridMap.Width; x++)
            {
                for (int y = 0; y < gridMap.Height; y++)
                {
                    var cell = gridMap.Cells[x, y];
                    if (!cell.HasAnyWalkableLayer())
                        continue;

                    for (int li = 0; li < cell.HeightLayers.Count; li++)
                    {
                        var currentLayer = cell.HeightLayers[li];
                        if (currentLayer.PassableHeight.GetSpan() < objectHeight)
                            continue;

                        if (!nodes.TryGetValue((x, y, li), out var currentNode))
                            continue;

                        double currentZ = currentLayer.Z;

                        foreach (var (dx, dy) in directions)
                        {
                            int nx = x + dx;
                            int ny = y + dy;

                            if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
                                continue;

                            var neighborCell = gridMap.Cells[nx, ny];
                            if (!neighborCell.HasAnyWalkableLayer())
                                continue;

                            // 查找邻居中所有满足高度约束的层
                            for (int nli = 0; nli < neighborCell.HeightLayers.Count; nli++)
                            {
                                var neighborLayer = neighborCell.HeightLayers[nli];
                                if (neighborLayer.PassableHeight.GetSpan() < objectHeight)
                                    continue;

                                double heightDiff = Math.Abs(neighborLayer.Z - currentZ);
                                if (heightDiff > maxHeightDiff)
                                    continue;

                                if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
                                    continue;

                                // 使用策略特定的速度计算器，如果未提供则使用默认的高度差计算
                                float speed;
                                if (speedCalculator != null)
                                {
                                    speed = speedCalculator(x, y, li, currentZ, nx, ny, nli, neighborLayer.Z,
                                        heightDiff, baseSpeed, maxHeightDiff, startPos, endPos);
                                }
                                else
                                {
                                    speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
                                }

                                maxSpeed = Math.Max(maxSpeed, speed);
                                var velocity = Velocity.FromKilometersPerHour(speed);

                                currentNode.Connect(neighborNode, velocity);
                                horizontalEdges++;
                            }
                        }
                    }
                }
            }

            LogManager.Info($"[3D图构建] 阶段2完成：创建了 {horizontalEdges} 条水平边");

            // === 阶段3：创建垂直边（仅在楼梯/电梯内部的相邻层）===
            int verticalEdges = 0;

            for (int x2 = 0; x2 < gridMap.Width; x2++)
            {
                for (int y2 = 0; y2 < gridMap.Height; y2++)
                {
                    var cell = gridMap.Cells[x2, y2];

                    // 检查该网格是否有任何楼梯或电梯层（允许垂直移动）
                    bool hasStairsOrElevator = cell.HeightLayers.Any(layer =>
                        layer.Type == "楼梯" ||
                        layer.Type == "电梯");

                    if (!hasStairsOrElevator || cell.HeightLayers.Count < 2)
                        continue;

                    // 连接同一(x,y)位置的不同高度层（相邻层之间）
                    for (int li = 0; li < cell.HeightLayers.Count - 1; li++)
                    {
                        var layer1 = cell.HeightLayers[li];
                        var layer2 = cell.HeightLayers[li + 1];

                        // 垂直边只连接相同类型的层（楼梯到楼梯，电梯到电梯）
                        // 不允许从通道跳到楼梯，必须通过水平边进入楼梯
                        if (layer1.Type != layer2.Type)
                            continue;

                        // 垂直边只在楼梯/电梯类型之间创建
                        if (layer1.Type != "楼梯" &&
                            layer1.Type != "电梯")
                            continue;

                        if (layer1.PassableHeight.GetSpan() < objectHeight ||
                            layer2.PassableHeight.GetSpan() < objectHeight)
                            continue;

                        double heightDiff = Math.Abs(layer2.Z - layer1.Z);
                        if (heightDiff > maxHeightDiff)
                            continue;

                        if (!nodes.TryGetValue((x2, y2, li), out var node1) ||
                            !nodes.TryGetValue((x2, y2, li + 1), out var node2))
                            continue;

                        // 垂直移动速度：电梯较快，楼梯较慢
                        float verticalSpeed = layer1.Type == "电梯"
                            ? (float)(baseSpeed * 0.5)
                            : (float)(baseSpeed * 0.3);
                        maxSpeed = Math.Max(maxSpeed, verticalSpeed);
                        var velocity = Velocity.FromKilometersPerHour(verticalSpeed);

                        // 双向连接
                        node1.Connect(node2, velocity);
                        node2.Connect(node1, velocity);
                        verticalEdges += 2;
                    }
                }
            }

            LogManager.Info($"[3D图构建] 阶段3完成：创建了 {verticalEdges} 条垂直边（双向）");
            LogManager.Info($"[3D图构建] 总结：{totalNodesCreated}个节点，{horizontalEdges + verticalEdges}条边");

            return new Graph3DResult
            {
                Nodes = nodes,
                NodeInfo = nodeInfo,
                AllNodes = allNodes,
                MaxVelocity = Velocity.FromKilometersPerHour(maxSpeed)
            };
        }

        /// <summary>
        /// 根据高度差计算速度（高度差单位：模型单位）
        /// </summary>
        /// <param name="heightDiff">高度差（模型单位）</param>
        /// <param name="baseSpeed">基础速度（km/h）</param>
        /// <param name="maxHeightDiff">最大允许高度差（模型单位，对应0.5米）</param>
        private float CalculateSpeedByHeightDiff(double heightDiff, double baseSpeed, double maxHeightDiff)
        {
            // 计算相对高度差比例
            double heightRatio = heightDiff / maxHeightDiff;

            if (heightRatio < 0.06) return (float)baseSpeed;           // 平地 (<3cm，即<0.06*0.5m)
            if (heightRatio <= 0.5) return (float)(baseSpeed * SLOPE_SPEED_FACTOR_GENTLE);   // 缓坡 (≤0.25m)
            if (heightRatio <= 1.0) return (float)(baseSpeed * SLOPE_SPEED_FACTOR_STAIR);    // 楼梯 (≤0.5m)
            return (float)(baseSpeed * SLOPE_SPEED_FACTOR_STEEP);                            // 过陡 (>0.5m)
        }

        /// <summary>
        /// 根据策略构建图
        /// </summary>
        private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
        {
            SpeedCalculator speedCalc = null;

            switch (strategy)
            {
                case PathStrategy.Shortest:
                    LogManager.Info($"[3D图构建] 使用最短路径策略");
                    // 默认策略，speedCalc = null，使用高度差计算
                    break;

                case PathStrategy.Straightest:
                    LogManager.Info($"[3D图构建] 使用直线优先策略");
                    // 直线优先：计算沿边方向能够直走的距离，距离越长速度越高
                    {
                        speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
                        {
                            // 基础速度（根据高度差）
                            float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);

                            // 计算边的方向
                            var direction = new GridPoint2D(nx - cx, ny - cy);
                            var currentPos = new GridPoint2D(cx, cy);

                            // 计算沿该方向能够直走的距离
                            int straightDistance = CalculateStraightDistance(currentPos, direction, gridMap, objectHeight);

                            // 直走距离越长，速度加成越高（最多+30 km/h）
                            float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
                            float finalSpeed = baseSpeedValue + bonusSpeed;

                            return finalSpeed;
                        };
                    }
                    break;

                case PathStrategy.SafestCenter:
                    LogManager.Info($"[3D图构建] 使用安全优先策略");
                    // 安全优先：计算安全距离图，基于到障碍物的距离调整速度
                    {
                        var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
                        LogManager.Info($"[3D图构建] 安全距离图计算完成");

                        speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
                        {
                            // 基础速度（根据高度差）
                            float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);

                            // 获取两个节点的安全距离，取较大值（更安全的距离）
                            int currentSafety = safetyDistanceMap[cx, cy];
                            int neighborSafety = safetyDistanceMap[nx, ny];
                            int maxSafety = Math.Max(currentSafety, neighborSafety);

                            // 根据安全距离计算速度系数
                            float safetySpeed = CalculateSpeedBySafetyDistance(maxSafety);

                            // 返回基础速度和安全速度的加权组合
                            // 高度差影响基础速度，安全距离影响额外速度
                            return baseSpeedValue * 0.3f + safetySpeed * 0.7f;
                        };
                    }
                    break;

                default:
                    LogManager.Warning($"[3D图构建] 未知策略 {strategy}，使用默认最短路径");
                    break;
            }

            return BuildGraph3D(gridMap, channelCoverage, objectHeight, startPos, endPos, speedCalc);
        }

        /// <summary>
        /// 在3D图上执行A*搜索
        /// </summary>
        private PathFindingResult ExecuteAStarOnGraph(Point3D start, Point3D end, GridMap gridMap, Graph3DResult graph3D)
        {
            var startGridPos = gridMap.WorldToGrid(start);
            var endGridPos = gridMap.WorldToGrid(end);

            // 找到起点和终点最近的3D节点
            double startElevation = gridMap.GetWorldElevation(start);
            double endElevation = gridMap.GetWorldElevation(end);

            var startNode = FindClosest3DNode(graph3D, startGridPos, startElevation);
            var endNode = FindClosest3DNode(graph3D, endGridPos, endElevation);

            // 起点和终点必须检查可达性
            bool startOnObstacle = (startNode == null);
            bool endOnObstacle = (endNode == null);

            // 如果起点在障碍物上，返回失败结果（正常业务流程，不记录错误）
            if (startOnObstacle && endOnObstacle)
            {
                LogManager.Info($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})和终点网格({endGridPos.X},{endGridPos.Y})都在不可通行区域");
                return PathFindingResult.Failure("起点和终点都在不可通行区域，请选择可通行的位置");
            }
            else if (startOnObstacle)
            {
                LogManager.Info($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})在不可通行区域");
                return PathFindingResult.Failure("起点在不可通行区域，请选择可通行的位置");
            }

            // 如果终点不可达，寻找最接近终点的可达节点（类似2.5D的ClosestApproach逻辑）
            if (endOnObstacle)
            {
                LogManager.Warning($"[A*执行-3D] 终点网格({endGridPos.X},{endGridPos.Y})不可达，寻找最接近的可达节点");

                // 从所有节点中找到最接近终点的节点
                double minDistance = double.MaxValue;
                foreach (var node in graph3D.AllNodes)
                {
                    var info = graph3D.NodeInfo[node];
                    var nodeWorldPos = gridMap.CreateWorldPoint(new GridPoint2D(info.X, info.Y), info.Z);

                    double distance = Math.Sqrt(
                        Math.Pow(nodeWorldPos.X - end.X, 2) +
                        Math.Pow(nodeWorldPos.Y - end.Y, 2) +
                        Math.Pow(nodeWorldPos.Z - end.Z, 2)
                    );

                    if (distance < minDistance)
                    {
                        minDistance = distance;
                        endNode = node;
                    }
                }

                if (endNode != null)
                {
                    var closestInfo = graph3D.NodeInfo[endNode];
                    LogManager.Info($"[A*执行-3D] 找到最接近终点的可达节点: ({closestInfo.X},{closestInfo.Y},层{closestInfo.LayerIndex},Z={closestInfo.Z:F2})，距离原终点 {minDistance:F2}模型单位");
                }
                else
                {
                    LogManager.Warning($"[A*执行-3D] 无法找到任何可达节点（图中没有任何节点）");
                    return PathFindingResult.Failure("无法找到任何可达节点，网格地图可能完全被障碍物覆盖");
                }
            }

            var startInfo = graph3D.NodeInfo[startNode];
            var endInfo = graph3D.NodeInfo[endNode];
            LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},高程={startElevation:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},高程={startInfo.Z:F2})");
            LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},高程={endElevation:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},高程={endInfo.Z:F2})");

            // 执行A*
            var pathfinder = new Roy_T.AStar.Paths.PathFinder();
            var astarPath = pathfinder.FindPath(startNode, endNode, graph3D.MaxVelocity);

            if (astarPath.Edges.Count == 0)
            {
                LogManager.Warning($"[A*执行-3D] 未找到路径，起点({startInfo.X},{startInfo.Y})到终点({endInfo.X},{endInfo.Y})之间被障碍物完全阻断");
                return PathFindingResult.Failure("起点和终点之间被障碍物完全阻断，无法找到可行路径");
            }

            // 转换为世界坐标路径
            var path3D = ConvertGraphPathToWorld(astarPath, graph3D, gridMap, start, end);

            // 计算路径完成度，检查是否到达终点
            double completionPercentage = 100.0;
            bool isComplete = true;
            Point3D actualEnd = path3D.Count > 0 ? path3D[path3D.Count - 1] : start;

            // 如果终点在障碍物上，路径肯定不完整
            if (endOnObstacle)
            {
                isComplete = false;
                actualEnd = path3D[path3D.Count - 1];

                // 计算实际距离和总距离
                double totalDistance = Math.Sqrt(
                    Math.Pow(end.X - start.X, 2) +
                    Math.Pow(end.Y - start.Y, 2) +
                    Math.Pow(end.Z - start.Z, 2)
                );
                double actualDistance = Math.Sqrt(
                    Math.Pow(actualEnd.X - start.X, 2) +
                    Math.Pow(actualEnd.Y - start.Y, 2) +
                    Math.Pow(actualEnd.Z - start.Z, 2)
                );

                completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;

                LogManager.Warning($"[3D路径规划] 终点在障碍物上，使用最接近的可达节点，路径不完整：完成度 {completionPercentage:F1}%");
            }
            else
            {
                // 检查A*路径最后一个节点是否到达终点网格
                if (astarPath.Edges.Count > 0)
                {
                    var lastEdge = astarPath.Edges[astarPath.Edges.Count - 1];
                    var lastNode = lastEdge.End as Roy_T.AStar.Graphs.Node;

                    if (lastNode != null && graph3D.NodeInfo.ContainsKey(lastNode))
                    {
                        var lastInfo = graph3D.NodeInfo[lastNode];

                        // 检查最后节点是否到达终点（网格坐标和高度层都要匹配）
                        if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y && lastInfo.LayerIndex == endInfo.LayerIndex)
                        {
                            // 到达终点网格和高度层，路径完整
                            isComplete = true;
                            actualEnd = path3D[path3D.Count - 1];
                            completionPercentage = 100.0;
                            LogManager.Info($"[3D路径规划] 找到完整路径，到达终点网格({endGridPos.X},{endGridPos.Y}) Layer{endInfo.LayerIndex}");
                        }
                        else
                        {
                            // 未到达终点，计算完成度
                            isComplete = false;
                            actualEnd = path3D[path3D.Count - 1];

                            // 计算实际距离和总距离
                            double totalDistance = Math.Sqrt(
                                Math.Pow(end.X - start.X, 2) +
                                Math.Pow(end.Y - start.Y, 2) +
                                Math.Pow(end.Z - start.Z, 2)
                            );
                            double actualDistance = Math.Sqrt(
                                Math.Pow(actualEnd.X - start.X, 2) +
                                Math.Pow(actualEnd.Y - start.Y, 2) +
                                Math.Pow(actualEnd.Z - start.Z, 2)
                            );

                            completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;

                            // 判断是否到达了目标网格但高度层不对
                            if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y)
                            {
                                LogManager.Warning($"[3D路径规划] 部分路径：到达目标网格({lastInfo.X},{lastInfo.Y})但高度层不匹配 - 实际Layer{lastInfo.LayerIndex}, 目标Layer{endInfo.LayerIndex}");
                            }
                            else
                            {
                                LogManager.Warning($"[3D路径规划] 部分路径：到达网格({lastInfo.X},{lastInfo.Y}) Layer{lastInfo.LayerIndex}，距离终点网格({endGridPos.X},{endGridPos.Y}) Layer{endInfo.LayerIndex}还有距离");
                            }
                            LogManager.Info($"[3D路径规划] 完成度: {completionPercentage:F1}%，实际终点: ({actualEnd.X:F2}, {actualEnd.Y:F2}, Z={actualEnd.Z:F2})");
                        }
                    }
                }
            }

            LogManager.Info($"[3D路径规划] 路径生成完成，包含 {path3D.Count} 个路径点，完成度: {completionPercentage:F1}%");

            return new PathFindingResult
            {
                PathPoints = path3D,
                IsComplete = isComplete,
                OriginalEndPoint = end,
                ActualEndPoint = actualEnd,
                CompletionPercentage = completionPercentage
            };
        }

        /// <summary>
        /// 找到最接近指定位置和高度的3D节点
        /// </summary>
        private Roy_T.AStar.Graphs.Node FindClosest3DNode(Graph3DResult graph3D, GridPoint2D gridPos, double targetZ)
        {
            Roy_T.AStar.Graphs.Node closestNode = null;
            double minDist = double.MaxValue;

            foreach (var node in graph3D.AllNodes)
            {
                var info = graph3D.NodeInfo[node];
                if (info.X == gridPos.X && info.Y == gridPos.Y)
                {
                    double heightDiff = Math.Abs(info.Z - targetZ);
                    if (heightDiff < minDist)
                    {
                        minDist = heightDiff;
                        closestNode = node;
                    }
                }
            }

            return closestNode;
        }

        /// <summary>
        /// 将图路径转换为世界坐标路径
        /// </summary>
        private List<Point3D> ConvertGraphPathToWorld(Roy_T.AStar.Paths.Path astarPath, Graph3DResult graph3D, GridMap gridMap, Point3D originalStart, Point3D originalEnd)
        {
            var path3D = new List<Point3D>();
            path3D.Add(originalStart);

            int pointIndex = 0;
            LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, {originalStart.Z:F2}), 高程={gridMap.GetWorldElevation(originalStart):F2}");
            pointIndex++;

            // 添加第一条边的起点（如果与originalStart不同）
            if (astarPath.Edges.Count > 0)
            {
                var firstEdge = astarPath.Edges[0];
                var firstNode = firstEdge.Start as Roy_T.AStar.Graphs.Node;
                if (firstNode != null && graph3D.NodeInfo.ContainsKey(firstNode))
                {
                    var firstInfo = graph3D.NodeInfo[firstNode];
                    var firstPoint = gridMap.CreateWorldPoint(new GridPoint2D(firstInfo.X, firstInfo.Y), firstInfo.Z);

                    // 如果与起点高度不同，添加这个节点（避免直接跳跃）
                    if (Math.Abs(gridMap.GetWorldElevation(firstPoint) - gridMap.GetWorldElevation(originalStart)) > 0.01)
                    {
                        path3D.Add(firstPoint);
                        LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, 高程={firstInfo.Z:F2}, 类型={firstInfo.CellType}");
                        pointIndex++;
                    }
                }
            }

            // 添加所有A*路径节点
            foreach (var edge in astarPath.Edges)
            {
                var endNode = edge.End as Roy_T.AStar.Graphs.Node;
                if (endNode == null || !graph3D.NodeInfo.ContainsKey(endNode))
                    continue;

                var nodeInfo = graph3D.NodeInfo[endNode];
                path3D.Add(gridMap.CreateWorldPoint(new GridPoint2D(nodeInfo.X, nodeInfo.Y), nodeInfo.Z));

                LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, 高程={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
                pointIndex++;
            }

            // 🔥 修复：不再强制添加原始终点，由ExecuteAStarOnGraph根据完成度决定
            LogManager.Info($"[路径转换完成] 总共 {path3D.Count} 个路径点");

            return path3D;
        }


        /// <summary>
        /// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）[已废弃，保留作为备份]
        /// </summary>
        private PathFindingResult FindPath_Legacy_2_5D(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, PathStrategy strategy = PathStrategy.Shortest)
        {
            try
            {
                LogManager.Info("[2.5D路径规划] 开始执行");

                // 保存原始起点和终点，用于最终路径修正
                var originalStart = start;
                var originalEnd = end;

                // 1. 检查起点和终点的高度约束
                var startGridPos = gridMap.WorldToGrid(start);
                if (!IsPointPassableAtHeight(startGridPos, start, gridMap, objectHeight))
                {
                    LogManager.Warning("[2.5D路径规划] 起点位置不满足高度约束");
                }

                var endGridPos = gridMap.WorldToGrid(end);
                if (!IsPointPassableAtHeight(endGridPos, end, gridMap, objectHeight))
                {
                    LogManager.Warning("[2.5D路径规划] 终点位置不满足高度约束");
                }

                // 2. 根据策略选择合适的网格转换方法（返回A*网格和激活层字典）
                var (astarGrid, activeLayerZ) = ConvertToAStarGridWithStrategy(gridMap, channelCoverage, objectHeight, start, end, strategy);

                // 3. 执行A*算法
                var pathResult = ExecuteAStarAlgorithm(start, end, gridMap, astarGrid);

                if (pathResult == null || !pathResult.PathPoints.Any())
                {
                    LogManager.Warning("[2.5D路径规划] 未找到有效路径");
                    return PathFindingResult.Failure("未找到有效路径，起点和终点之间可能被障碍物阻断");
                }

                LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");

                // 4. 提取网格路径
                var gridPath = pathResult.PathPoints
                    .Select(point => gridMap.WorldToGrid(point))
                    .ToList();

                // 5. 使用激活层字典还原3D路径
                var enhancedPath = ApplyActiveLayers(gridPath, gridMap, activeLayerZ, originalStart, originalEnd);

                // 更新PathFindingResult
                pathResult.PathPoints = enhancedPath;
                pathResult.OriginalEndPoint = originalEnd;

                LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {enhancedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");

                // 路径优化 - 去除共线点
                pathResult = OptimizePath(pathResult, gridMap);

                // 记录缓存统计信息
                LogCacheStatistics();

                return pathResult;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[2.5D路径规划] 路径查找失败: {ex.Message}");
                throw; // 直接抛出异常，不返回默认路径
            }
        }

        /// <summary>
        /// 将网格地图转换为A*算法使用的格式（2.5D模式）
        /// </summary>
        /// <param name="gridMap">网格地图</param>
        /// <param name="channelCoverage">通道覆盖数据</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>A*算法网格</returns>
        private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridWith2_5D(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
        {
            try
            {
                LogManager.Info($"[A*转换-2.5D] 开始转换网格格式: {gridMap.Width}x{gridMap.Height}，物体高度: {objectHeight}m");

                // 初始化激活层字典
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
                double targetZ = gridMap.GetWorldElevation(endPos);  // 使用宿主坐标语义下的终点高程作为目标高度
                double baseSpeed = 5.0; // km/h

                LogManager.Info($"[A*转换-2.5D] 目标高程: {targetZ:F2}m，基础速度: {baseSpeed}km/h");

                // 输出详细的网格统计信息
                LogManager.Info($"[A*转换-2.5D] 输入网格统计信息:\n{gridMap.GetStatistics()}");

                // 单位转换
                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
                LogManager.Info($"[A*转换-2.5D] 单位转换: {gridMap.CellSize:F2}模型单位 -> {cellSizeInMeters:F2}米");

                // 创建A*网格
                var gridSize = new GridSize(gridMap.Width, gridMap.Height);
                var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
                var traversalVelocity = Velocity.FromKilometersPerHour((float)baseSpeed);

                var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, traversalVelocity);
                LogManager.Info($"[A*转换-2.5D] A*网格创建完成");

                // 先断开所有节点连接
                LogManager.Info($"[2.5D断开诊断] 开始断开所有节点连接，网格尺寸: {gridMap.Width}x{gridMap.Height}");
                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var pos = new GridPosition(x, y);
                        var nodeBefore = grid.GetNode(pos);
                        var outgoingCountBefore = nodeBefore.Outgoing.Count();

                        grid.DisconnectNode(pos);

                        // 🔥 诊断记录：断开后验证
                        var nodeAfter = grid.GetNode(pos);
                        var outgoingCountAfter = nodeAfter.Outgoing.Count();

                        // 只记录关键坐标的详细信息
                        if ((x >= 49 && x <= 62 && y == 25) || (x == 71 && y == 23))
                        {
                            LogManager.Info($"[2.5D断开诊断] 网格({x},{y}): 断开前边数={outgoingCountBefore} -> 断开后边数={outgoingCountAfter}");
                        }
                    }
                }
                LogManager.Info($"[2.5D断开诊断] 所有节点连接断开完成");

                // 只连接可通行的节点
                int connectedCells = 0;
                int totalWalkableCells = 0;
                int totalNonWalkableCells = 0;
                int heightConstrainedCells = 0; // 本来可通行但被高度约束排除的

                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var cell = gridMap.Cells[x, y];

                        // 统计基本可通行性
                        if (cell.HasAnyWalkableLayer())
                        {
                            totalWalkableCells++;
                        }
                        else
                        {
                            totalNonWalkableCells++;
                        }

                        // 检查是否有满足物体高度的层
                        if (!HasCompatibleLayer(cell, objectHeight))
                        {
                            if (cell.HasAnyWalkableLayer())
                            {
                                heightConstrainedCells++;
                                if (heightConstrainedCells <= 10)
                                {
                                    var layerInfo = cell.HeightLayers != null && cell.HeightLayers.Count > 0
                                        ? $"有{cell.HeightLayers.Count}个高度层"
                                        : "无高度层";
                                    LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：物体高度{objectHeight:F2}模型单位，{layerInfo}");
                                }
                                else if (heightConstrainedCells == 11)
                                {
                                    LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除，不再详细记录...");
                                }
                            }
                            continue;
                        }

                        // 为当前单元格选择最优高度层
                        var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
                        if (!currentZ.HasValue)
                            continue;

                        var pos = new GridPosition(x, y);
                        var gridPos = new GridPoint2D(x, y);

                        // 记录激活层
                        activeLayerZ[gridPos] = currentZ.Value;
                        connectedCells++;

                        // 连接右侧邻居
                        if (x + 1 < gridMap.Width)
                        {
                            var rightCell = gridMap.Cells[x + 1, y];
                            var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
                            if (rightZ.HasValue)
                            {
                                var rightPos = new GridPosition(x + 1, y);
                                var rightGridPos = new GridPoint2D(x + 1, y);

                                // 计算高度差和速度系数
                                double heightDiff = Math.Abs(rightZ.Value - currentZ.Value);
                                double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
                                float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));

                                grid.AddEdge(pos, rightPos, adjustedVelocity);
                                activeLayerZ[rightGridPos] = rightZ.Value;

                                if ((x >= 49 && x <= 62 && y == 25) || (x == 71 && y == 23))
                                {
                                    LogManager.Debug($"[A*构建] 边 ({x},{y})Z={currentZ.Value:F2} → ({x+1},{y})Z={rightZ.Value:F2}, " +
                                                   $"高度差={heightDiff:F2}m, 速度系数={speedMultiplier:F2}");
                                }
                            }
                        }

                        // 连接下方邻居
                        if (y + 1 < gridMap.Height)
                        {
                            var bottomCell = gridMap.Cells[x, y + 1];
                            var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
                            if (bottomZ.HasValue)
                            {
                                var bottomPos = new GridPosition(x, y + 1);
                                var bottomGridPos = new GridPoint2D(x, y + 1);

                                double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
                                float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));

                                grid.AddEdge(pos, bottomPos, adjustedVelocity);
                                activeLayerZ[bottomGridPos] = bottomZ.Value;
                            }
                        }

                        // 连接左侧邻居
                        if (x - 1 >= 0)
                        {
                            var leftCell = gridMap.Cells[x - 1, y];
                            var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
                            if (leftZ.HasValue)
                            {
                                var leftPos = new GridPosition(x - 1, y);
                                var leftGridPos = new GridPoint2D(x - 1, y);

                                double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
                                float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));

                                grid.AddEdge(pos, leftPos, adjustedVelocity);
                                activeLayerZ[leftGridPos] = leftZ.Value;
                            }
                        }

                        // 连接上方邻居
                        if (y - 1 >= 0)
                        {
                            var topCell = gridMap.Cells[x, y - 1];
                            var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
                            if (topZ.HasValue)
                            {
                                var topPos = new GridPosition(x, y - 1);
                                var topGridPos = new GridPoint2D(x, y - 1);

                                double distanceToTarget = Math.Abs(topZ.Value - targetZ);
                                float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));

                                grid.AddEdge(pos, topPos, adjustedVelocity);
                                activeLayerZ[topGridPos] = topZ.Value;
                            }
                        }
                    }
                }

                LogManager.Info($"[A*转换-2.5D] 网格转换完成，A*节点连接统计:");
                LogManager.Info($"[A*转换-2.5D]   - 总网格单元格: {gridMap.Width * gridMap.Height}");
                LogManager.Info($"[A*转换-2.5D]   - 基本可通行单元格: {totalWalkableCells}");
                LogManager.Info($"[A*转换-2.5D]   - 基本不可通行单元格: {totalNonWalkableCells}");
                LogManager.Info($"[A*转换-2.5D]   - 被高度约束排除: {heightConstrainedCells}");
                LogManager.Info($"[A*转换-2.5D]   - 最终连接到A*网格的节点: {connectedCells}");
                LogManager.Info($"[A*转换-2.5D]   - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
                LogManager.Info($"[A*转换-2.5D] 记录了 {activeLayerZ.Count} 个网格的激活层");

                return (grid, activeLayerZ);
            }
            catch (Exception ex)
            {
                LogManager.Error($"[A*转换-2.5D] 转换网格格式时发生错误: {ex.Message}");
                throw new AutoPathPlanningException($"2.5D网格转换失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 根据路径策略选择合适的A*网格转换方法
        /// </summary>
        /// <param name="gridMap">网格地图</param>
        /// <param name="channelCoverage">通道覆盖数据</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <param name="startPos">起点坐标</param>
        /// <param name="endPos">终点坐标</param>
        /// <param name="strategy">路径规划策略</param>
        /// <returns>A*网格和激活层字典的元组</returns>
        private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
        {
            switch (strategy)
            {
                case PathStrategy.Shortest:
                    LogManager.Info($"[策略路由] 使用最短路径策略");
                    return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, objectHeight, startPos, endPos);

                case PathStrategy.Straightest:
                    LogManager.Info($"[策略路由] 使用直线优先策略");
                    return ConvertToAStarGridStraightest(gridMap, channelCoverage, objectHeight, startPos, endPos);

                case PathStrategy.SafestCenter:
                    LogManager.Info($"[策略路由] 使用安全优先策略");
                    return ConvertToAStarGridSafestCenter(gridMap, channelCoverage, objectHeight, startPos, endPos);

                default:
                    LogManager.Warning($"[策略路由] 未知策略 {strategy}，使用默认最短路径");
                    return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, objectHeight, startPos, endPos);
            }
        }

        /// <summary>
        /// 直线优先的A*网格转换方法
        /// 通过调整不同方向边的速度来引导算法优先选择主方向路径
        /// </summary>
        /// <param name="gridMap">网格地图</param>
        /// <param name="channelCoverage">通道覆盖数据</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <param name="startPos">起点坐标</param>
        /// <param name="endPos">终点坐标</param>
        /// <returns>A*网格</returns>
        private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
        {
            try
            {
                LogManager.Info($"[局部直线优先] 开始实施局部直线优先算法");

                // 初始化激活高度层字典和目标高度
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
                double targetZ = gridMap.GetWorldElevation(endPos);

                // 1. 创建网格基础结构
                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
                var gridSize = new GridSize(gridMap.Width, gridMap.Height);
                var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));

                // 使用基础速度创建网格，后续会动态调整
                var baseVelocity = Velocity.FromKilometersPerHour(5);  // 基础速度
                var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, baseVelocity);

                LogManager.Info($"[局部直线优先] A*网格创建完成，尺寸: {gridMap.Width}x{gridMap.Height}");

                // 2. 断开所有连接，准备重新按局部直线距离连接
                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var pos = new GridPosition(x, y);
                        grid.DisconnectNode(pos);
                    }
                }
                LogManager.Info($"[直线优先断开诊断] 所有节点连接断开完成");

                // 3. 对角线固定速度（较低优先级）
                var diagonalVelocity = Velocity.FromKilometersPerHour(6);

                // 4. 智能连接：基于局部直线距离设置速度
                int connectedCells = 0;
                int heightConstrainedCells = 0;
                int dynamicConnections = 0;

                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var cell = gridMap.Cells[x, y];

                        // 检查是否有兼容的高度层
                        if (!HasCompatibleLayer(cell, objectHeight))
                        {
                            if (cell.HasAnyWalkableLayer())
                            {
                                heightConstrainedCells++;
                            }
                            continue;
                        }

                        // 选择当前网格的最佳高度层
                        var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
                        if (!currentZ.HasValue)
                        {
                            continue;
                        }

                        var pos = new GridPosition(x, y);
                        var gridPos = new GridPoint2D(x, y);

                        // 记录当前网格的激活高度层
                        activeLayerZ[gridPos] = currentZ.Value;
                        connectedCells++;

                        // 🔥 核心改动：右方向连接（基于局部直线距离）
                        if (x + 1 < gridMap.Width)
                        {
                            var rightCell = gridMap.Cells[x + 1, y];
                            
                            // 检查右侧网格是否有兼容的高度层
                            if (HasCompatibleLayer(rightCell, objectHeight))
                            {
                                // 选择右侧网格的最佳高度层
                                var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
                                if (rightZ.HasValue)
                                {
                                    var rightPos = new GridPosition(x + 1, y);
                                    var rightGridPos = new GridPoint2D(x + 1, y);

                                    // 记录右侧网格的激活高度层
                                    activeLayerZ[rightGridPos] = rightZ.Value;

                                    // 计算沿右方向的直线距离
                                    var rightDistance = CalculateStraightDistance(gridPos, new GridPoint2D(1, 0), gridMap, objectHeight);

                                    // 计算基础速度（基于直线距离）
                                    float baseSpeed = 5.0f;
                                    float bonusSpeed = Math.Min(rightDistance * 0.5f, 30.0f);
                                    float straightSpeed = baseSpeed + bonusSpeed;

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = straightSpeed * heightMultiplier;
                                    var rightVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 添加单向边
                                    grid.AddEdge(pos, rightPos, rightVelocity);
                                    dynamicConnections++;
                                }
                            }
                        }

                        // 🔥 核心改动：下方向连接（基于局部直线距离）
                        if (y + 1 < gridMap.Height)
                        {
                            var bottomCell = gridMap.Cells[x, y + 1];
                            
                            // 检查下方网格是否有兼容的高度层
                            if (HasCompatibleLayer(bottomCell, objectHeight))
                            {
                                // 选择下方网格的最佳高度层
                                var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
                                if (bottomZ.HasValue)
                                {
                                    var bottomPos = new GridPosition(x, y + 1);
                                    var bottomGridPos = new GridPoint2D(x, y + 1);

                                    // 记录下方网格的激活高度层
                                    activeLayerZ[bottomGridPos] = bottomZ.Value;

                                    // 计算沿下方向的直线距离
                                    var downDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, 1), gridMap, objectHeight);

                                    // 计算基础速度（基于直线距离）
                                    float baseSpeed = 5.0f;
                                    float bonusSpeed = Math.Min(downDistance * 0.5f, 30.0f);
                                    float straightSpeed = baseSpeed + bonusSpeed;

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = straightSpeed * heightMultiplier;
                                    var downVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 添加单向边
                                    grid.AddEdge(pos, bottomPos, downVelocity);
                                    dynamicConnections++;
                                }
                            }
                        }

                        // 🔥 核心改动：左方向连接（基于局部直线距离）
                        if (x - 1 >= 0)
                        {
                            var leftCell = gridMap.Cells[x - 1, y];
                            
                            // 检查左侧网格是否有兼容的高度层
                            if (HasCompatibleLayer(leftCell, objectHeight))
                            {
                                // 选择左侧网格的最佳高度层
                                var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
                                if (leftZ.HasValue)
                                {
                                    var leftPos = new GridPosition(x - 1, y);
                                    var leftGridPos = new GridPoint2D(x - 1, y);

                                    // 记录左侧网格的激活高度层
                                    activeLayerZ[leftGridPos] = leftZ.Value;

                                    // 计算沿左方向的直线距离
                                    var leftDistance = CalculateStraightDistance(gridPos, new GridPoint2D(-1, 0), gridMap, objectHeight);

                                    // 计算基础速度（基于直线距离）
                                    float baseSpeed = 5.0f;
                                    float bonusSpeed = Math.Min(leftDistance * 0.5f, 30.0f);
                                    float straightSpeed = baseSpeed + bonusSpeed;

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = straightSpeed * heightMultiplier;
                                    var leftVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 添加单向边
                                    grid.AddEdge(pos, leftPos, leftVelocity);
                                    dynamicConnections++;
                                }
                            }
                        }

                        // 🔥 核心改动：上方向连接（基于局部直线距离）
                        if (y - 1 >= 0)
                        {
                            var topCell = gridMap.Cells[x, y - 1];
                            
                            // 检查上方网格是否有兼容的高度层
                            if (HasCompatibleLayer(topCell, objectHeight))
                            {
                                // 选择上方网格的最佳高度层
                                var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
                                if (topZ.HasValue)
                                {
                                    var topPos = new GridPosition(x, y - 1);
                                    var topGridPos = new GridPoint2D(x, y - 1);

                                    // 记录上方网格的激活高度层
                                    activeLayerZ[topGridPos] = topZ.Value;

                                    // 计算沿上方向的直线距离
                                    var upDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, -1), gridMap, objectHeight);

                                    // 计算基础速度（基于直线距离）
                                    float baseSpeed = 5.0f;
                                    float bonusSpeed = Math.Min(upDistance * 0.5f, 30.0f);
                                    float straightSpeed = baseSpeed + bonusSpeed;

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(topZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = straightSpeed * heightMultiplier;
                                    var upVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 添加单向边
                                    grid.AddEdge(pos, topPos, upVelocity);
                                    dynamicConnections++;
                                }
                            }
                        }

                        // 对角线连接（使用固定的较低速度）
                        // 右下对角线
                        if (x + 1 < gridMap.Width && y + 1 < gridMap.Height)
                        {
                            var diagonalCell = gridMap.Cells[x + 1, y + 1];
                            if (HasCompatibleLayer(diagonalCell, objectHeight))
                            {
                                var diagonalZ = SelectBestLayerForTarget(diagonalCell.HeightLayers, targetZ, objectHeight);
                                if (diagonalZ.HasValue)
                                {
                                    var diagonalPos = new GridPosition(x + 1, y + 1);
                                    var diagonalGridPos = new GridPoint2D(x + 1, y + 1);
                                    activeLayerZ[diagonalGridPos] = diagonalZ.Value;
                                    grid.AddEdge(pos, diagonalPos, diagonalVelocity);
                                }
                            }
                        }
                    }
                }

                LogManager.Info($"[局部直线优先] 网格转换完成，A*节点连接统计:");
                LogManager.Info($"[局部直线优先]   - 总网格单元格: {gridMap.Width * gridMap.Height}");
                LogManager.Info($"[局部直线优先]   - 连接的节点: {connectedCells}");
                LogManager.Info($"[局部直线优先]   - 被高度约束排除: {heightConstrainedCells}");
                LogManager.Info($"[局部直线优先]   - 动态连接数: {dynamicConnections}");
                LogManager.Info($"[局部直线优先]   - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
                LogManager.Info($"[局部直线优先]   - 激活层记录数: {activeLayerZ.Count}");

                return (grid, activeLayerZ);
            }
            catch (Exception ex)
            {
                LogManager.Error($"[局部直线优先] 网格转换失败: {ex.Message}");
                throw new AutoPathPlanningException($"局部直线优先网格转换失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 检查单元格是否满足高度约束
        /// </summary>
        /// <param name="cell">网格单元格</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>是否可通行</returns>
        private bool IsPassableWithHeight(GridCell cell, double objectHeight)
        {
            if (!cell.HasAnyWalkableLayer())
                return false;

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

            return false;
        }

        /// <summary>
        /// 检查单元格是否有满足物体高度的高度层
        /// </summary>
        /// <param name="cell">网格单元格</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>是否有兼容的高度层</returns>
        private bool HasCompatibleLayer(GridCell cell, double objectHeight)
        {
            if (!cell.HasAnyWalkableLayer())
                return false;

            if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
                return false;

            foreach (var layer in cell.HeightLayers)
            {
                if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight)
                    return true;
            }

            return false;
        }

        /// <summary>
        /// 从高度层列表中选择最接近目标高度且满足物体高度的层
        /// </summary>
        /// <param name="layers">高度层列表</param>
        /// <param name="targetZ">目标高度（米）</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>选中的高度层Z坐标，如果没有合适的返回null</returns>
        private double? SelectBestLayerForTarget(List<HeightLayer> layers, double targetZ, double objectHeight)
        {
            if (layers == null || layers.Count == 0)
                return null;

            HeightLayer? bestLayer = null;
            double minDistance = double.MaxValue;

            foreach (var layer in layers)
            {
                if (!layer.IsWalkable)
                    continue;

                // 必须满足物体高度
                if (layer.PassableHeight.GetSpan() < objectHeight)
                    continue;

                // 选择最接近目标高度的层
                double distance = Math.Abs(layer.Z - targetZ);
                if (distance < minDistance)
                {
                    minDistance = distance;
                    bestLayer = layer;
                }
            }

            return bestLayer?.Z;
        }

        /// <summary>
        /// 根据高度层与目标高度的距离计算速度系数
        /// </summary>
        /// <param name="distanceToTarget">与目标高度的距离（米）</param>
        /// <returns>速度系数（0-1之间）</returns>
        private float CalculateSpeedMultiplier(double distanceToTarget)
        {
            // 距离目标高度越近，速度越快（A*成本越低）
            // 公式：速度 = 1 / (1 + 距离 * 惩罚系数)
            // 例如：距离0m → 速度1.0x, 距离3m → 速度0.77x, 距离6m → 速度0.625x
            return (float)(1.0 / (1.0 + distanceToTarget * HEIGHT_PENALTY_FACTOR));
        }

        /// <summary>
        /// 计算从指定位置沿指定方向能走的最大直线距离
        /// </summary>
        /// <param name="startPos">起始网格位置</param>
        /// <param name="direction">方向 (1,0)=右, (0,1)=下, (-1,0)=左, (0,-1)=上</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>直线距离（网格单位）</returns>
        private int CalculateStraightDistance(GridPoint2D startPos, GridPoint2D direction,
            GridMap gridMap, double objectHeight)
        {
            int distance = 0;
            var currentPos = new GridPoint2D(startPos.X + direction.X, startPos.Y + direction.Y);

            // 沿方向前进，直到遇到障碍
            while (IsValidAndPassable(currentPos, gridMap, objectHeight))
            {
                distance++;
                currentPos = new GridPoint2D(currentPos.X + direction.X, currentPos.Y + direction.Y);

                // 防止无限循环，设置最大搜索距离
                if (distance > 50) break;  // 最多看前50格
            }

            return distance;
        }

        /// <summary>
        /// 检查网格位置是否有效且可通行
        /// </summary>
        /// <param name="pos">网格位置</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>是否有效且可通行</returns>
        private bool IsValidAndPassable(GridPoint2D pos, GridMap gridMap, double objectHeight)
        {
            // 检查边界
            if (pos.X < 0 || pos.X >= gridMap.Width || pos.Y < 0 || pos.Y >= gridMap.Height)
                return false;

            // 检查可通行性
            var cell = gridMap.Cells[pos.X, pos.Y];
            return IsPassableWithHeight(cell, objectHeight);
        }

        /// <summary>
        /// 根据直线距离计算速度权重
        /// </summary>
        /// <param name="straightDistance">直线距离</param>
        /// <returns>对应的速度</returns>
        private Velocity CalculateVelocityByDistance(int straightDistance)
        {
            // 基础速度：5km/h
            float baseSpeed = 5.0f;

            // 奖励机制：每能走1格直线，速度提升0.5km/h，最大提升到15km/h
            float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);

            return Velocity.FromKilometersPerHour(baseSpeed + bonusSpeed);
        }

        /// <summary>
        /// 将A*路径的米坐标转换为网格坐标
        /// </summary>
        /// <param name="astarPath">A*路径</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>网格坐标路径</returns>
        public List<GridPoint2D> ConvertPathToGridCoordinates(Path astarPath, GridMap gridMap)
        {
            var gridPath = new List<GridPoint2D>();
            double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);

            // 只添加起始节点一次，然后只添加每条边的终点
            if (astarPath.Edges.Count > 0)
            {
                // 添加第一条边的起点
                var startNode = astarPath.Edges[0].Start;
                var startGridPos = ConvertAStarPositionToGrid(startNode.Position, gridMap);
                gridPath.Add(startGridPos);

                // 添加每条边的终点（避免重复）
                foreach (var edge in astarPath.Edges)
                {
                    var gridPoint = ConvertAStarPositionToGrid(edge.End.Position, gridMap);

                    // 检查是否与上一个点重复（理论上A*不应该产生重复，但保险起见）
                    if (gridPath.Count == 0 || !gridPath[gridPath.Count - 1].Equals(gridPoint))
                    {
                        gridPath.Add(gridPoint);
                    }
                }
            }

            return gridPath;
        }

        /// <summary>
        /// 将A*路径转换为世界坐标
        /// </summary>
        /// <param name="astarPath">A*路径</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>世界坐标路径</returns>
        private List<Point3D> ConvertPathToWorldCoordinates(Path astarPath, GridMap gridMap)
        {
            var worldPath = new List<Point3D>();

            LogManager.Info($"[路径转换] 开始转换A*路径，共{astarPath.Edges.Count}条边");

            // A*返回的是网格左下角坐标，使用统一的转换方法
            if (astarPath.Edges.Count > 0)
            {
                // 🔥 修复：使用统一方法转换A*米坐标为网格坐标，然后转为世界坐标
                var startNode = astarPath.Edges[0].Start;
                var startGridPos = ConvertAStarPositionToGrid(startNode.Position, gridMap);
                var startWorldPos = gridMap.GridToWorld3D(startGridPos);

                LogManager.Info($"[路径转换] 起始节点: A*米坐标({startNode.Position.X:F2}, {startNode.Position.Y:F2}) -> 世界坐标({startWorldPos.X:F2}, {startWorldPos.Y:F2}, {startWorldPos.Z:F2})");
                worldPath.Add(startWorldPos);

                // 添加每条边的终点（避免重复）
                foreach (var edge in astarPath.Edges)
                {
                    // 使用统一方法转换A*米坐标为网格坐标
                    var endGridPos = ConvertAStarPositionToGrid(edge.End.Position, gridMap);

                    // 将网格坐标转换为世界坐标以获取Z坐标
                    var endWorldPos = gridMap.GridToWorld3D(endGridPos);
                    var endCell = gridMap.GetCell(endGridPos);

                    if (endCell.HasValue)
                    {
                        // 调试：检查门网格的Z坐标
                        double endElevation = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
                            ? endCell.Value.HeightLayers[0].Z
                            : 0;

                        if (endCell.Value.CellType == "门")
                        {
                            LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].高程={endElevation:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
                        }
                        endWorldPos = gridMap.SetWorldElevation(endWorldPos, endElevation);
                    }
                    else
                    {
                        LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，高程保持为默认值");
                    }

                    // 检查是否与上一个点重复（A*可能在转弯点有重复）
                    bool isDuplicate = worldPath.Count > 0 &&
                                     Math.Abs(worldPath[worldPath.Count - 1].X - endWorldPos.X) < 0.001 &&
                                     Math.Abs(worldPath[worldPath.Count - 1].Y - endWorldPos.Y) < 0.001;

                    if (!isDuplicate)
                    {
                        worldPath.Add(endWorldPos);
                    }
                }
            }

            LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（网格左下角）");
            return worldPath;
        }

        /// <summary>
        /// 专门的斜线路径优化算法
        /// 在现有优化基础上，通过更激进的斜线连接进一步减少路径点数
        /// </summary>
        /// <param name="optimizedPath">已经过初步优化的路径</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>经过斜线优化后的路径</returns>
        private List<Point3D> ApplyDiagonalOptimization(List<Point3D> optimizedPath, GridMap gridMap)
        {
            if (optimizedPath.Count <= 2)
                return optimizedPath;

            try
            {
                LogManager.Info($"[斜线优化] 开始斜线路径优化，输入路径包含 {optimizedPath.Count} 个点");

                var diagonalOptimizedPath = new List<Point3D> { optimizedPath[0] }; // 保留起点

                int currentIndex = 0;
                while (currentIndex < optimizedPath.Count - 1)
                {
                    int farthestIndex = currentIndex + 1; // 默认下一个点

                    // 🔥 更激进的斜线检测：从P+2开始尝试，直到找到最远可连接点
                    for (int testIndex = currentIndex + 2; testIndex < optimizedPath.Count; testIndex++)
                    {
                        var startPoint = optimizedPath[currentIndex];
                        var endPoint = optimizedPath[testIndex];
                        var distance = CalculateHostHorizontalDistance(startPoint, endPoint, gridMap);

                        // 🔥 修复：检查高度变化，有高度变化就不能优化掉
                        // 1. 起终点高度差不能超过阈值（原有逻辑：防止跨越大高度差，例如楼梯）
                        // 2. 中间点高度必须与起点和终点一致（新增逻辑：任何高度变化都不能优化掉）
                        bool canSkip = false;
                        double maxAllowedHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff; // 从配置文件读取高度差阈值（模型单位）

                        double startZ = gridMap.GetWorldElevation(startPoint);
                        double endZ = gridMap.GetWorldElevation(endPoint);
                        double totalHeightDiff = Math.Abs(endZ - startZ);

                        // 检查1：起终点高度差必须在阈值内
                        if (totalHeightDiff <= maxAllowedHeightDiff)
                        {
                            // 检查2：所有中间点的高度必须与起点和终点一致（不能有高度变化）
                            bool hasMiddleHeightChange = false;
                            const double heightTolerance = 1e-6; // 浮点精度容差

                            for (int midIndex = currentIndex + 1; midIndex < testIndex; midIndex++)
                            {
                                double midZ = gridMap.GetWorldElevation(optimizedPath[midIndex]);
                                // 如果中间点高度与起点或终点不同，说明有高度变化，不能优化掉
                                if (Math.Abs(midZ - startZ) > heightTolerance ||
                                    Math.Abs(midZ - endZ) > heightTolerance)
                                {
                                    hasMiddleHeightChange = true;
                                    break;
                                }
                            }

                            canSkip = !hasMiddleHeightChange;
                        }

                        if (canSkip && IsDirectPathClear(startPoint, endPoint, gridMap))
                        {
                            // 可以直线连接，更新最远索引
                            farthestIndex = testIndex;
                            //LogManager.Debug($"[斜线优化] ✓ 成功连接：点{currentIndex}→点{testIndex}，距离={distance:F2}m");
                        }
                        else
                        {
                            // 新增：记录失败详情
                            var startGrid = gridMap.WorldToGrid(startPoint);
                            var endGrid = gridMap.WorldToGrid(endPoint);
                            // LogManager.Debug($"[斜线优化] ✗ 连接失败：" +
                            //                 $"点{currentIndex}[网格({startGrid.X},{startGrid.Y})]→点{testIndex}[网格({endGrid.X},{endGrid.Y})]，" +
                            //                 $"世界坐标：({startPoint.X:F2},{startPoint.Y:F2})→({endPoint.X:F2},{endPoint.Y:F2})，" +
                            //                 $"距离={distance:F2}m，跨越{Math.Abs(endGrid.X-startGrid.X)+Math.Abs(endGrid.Y-startGrid.Y)}个网格");
                        }
                        // 🔧 关键改进：不像现有算法那样遇到失败就停止，而是继续尝试更远的点
                        // 这样可以发现更多斜线连接机会
                    }

                    // 如果找到了比相邻点更远的连接，记录优化效果
                    if (farthestIndex > currentIndex + 1)
                    {
                        int skippedPoints = farthestIndex - currentIndex - 1;
                        //LogManager.Info($"[斜线优化] 从点{currentIndex}直连到点{farthestIndex}，跳过{skippedPoints}个中间点");
                    }

                    currentIndex = farthestIndex;
                    if (currentIndex < optimizedPath.Count)
                    {
                        diagonalOptimizedPath.Add(optimizedPath[currentIndex]);
                    }
                }

                int reducedPoints = optimizedPath.Count - diagonalOptimizedPath.Count;
                LogManager.Info($"[斜线优化] 斜线优化完成，点数变化: {optimizedPath.Count} -> {diagonalOptimizedPath.Count} (减少{reducedPoints}个点)");

                // 打印斜线优化后的路径点详情
                LogManager.Info($"[斜线优化后路径详情] 共 {diagonalOptimizedPath.Count} 个点：");
                for (int i = 0; i < diagonalOptimizedPath.Count; i++)
                {
                    var pt = diagonalOptimizedPath[i];
                    LogManager.Info($"  [斜线优化后点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
                }

                return diagonalOptimizedPath;
            }
            catch (Exception ex)
            {
                LogManager.Warning($"[斜线优化] 斜线优化失败: {ex.Message}，使用输入路径");
                return optimizedPath;
            }
        }

        /// <summary>
        /// 检查两点间的直线路径是否畅通
        /// 使用密集采样确保整条线段都在可通行区域内，避免穿越障碍网格边缘
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>是否畅通</returns>
        private bool IsDirectPathClear(Point3D start, Point3D end, GridMap gridMap)
        {
            try
            {
                // 计算线段长度
                var distance = CalculateHostHorizontalDistance(start, end, gridMap);

                // 如果距离太小，直接检查起点和终点
                if (distance < gridMap.CellSize * 0.1)
                {
                    var startGrid = gridMap.WorldToGrid(start);
                    var endGrid = gridMap.WorldToGrid(end);
                    bool result = gridMap.IsValidGridPosition(startGrid) && gridMap.IsWalkable(startGrid) &&
                           gridMap.IsValidGridPosition(endGrid) && gridMap.IsWalkable(endGrid);
                    return result;
                }

                // 密集采样：每半个网格单位采样一次，确保不会跳过任何障碍网格
                int samples = (int)Math.Ceiling(distance / (gridMap.CellSize * 0.5));
                // 最少采样5个点，最多10000个点（提高性能限制）
                samples = Math.Max(5, Math.Min(samples, 10000));

                for (int i = 0; i <= samples; i++)
                {
                    // 线性插值计算采样点
                    double t = samples > 0 ? (double)i / samples : 0;
                    double interpolatedElevation =
                        gridMap.GetWorldElevation(start) +
                        t * (gridMap.GetWorldElevation(end) - gridMap.GetWorldElevation(start));

                    var samplePoint = gridMap.SetWorldElevation(
                        new Point3D(
                            start.X + t * (end.X - start.X),
                            start.Y + t * (end.Y - start.Y),
                            start.Z + t * (end.Z - start.Z)),
                        interpolatedElevation);

                    // 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内
                    // 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层
                    var gridPos = gridMap.WorldToGrid(samplePoint);
                    double tolerance = 0.1; // Z坐标容差（模型单位）

                    if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAtElevation(gridPos, gridMap.GetWorldElevation(samplePoint), tolerance))
                    {
                        // 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
                        var (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
                        var gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
                        var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
                        var gridCenterZ = gridMap.GetWorldElevation(samplePoint);

                        // 计算偏差
                        var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);
                        var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
                        var deltaH2 = sampleHorizontal.h2 - gridCenterH2;

                        // LogManager.Debug($"[斜线检查] 采样点{i}/{samples}失败：" +
                        //                 $"采样点({samplePoint.X:F3},{samplePoint.Y:F3},{samplePoint.Z:F3})，" +
                        //                 $"网格({gridPos.X},{gridPos.Y})左下角({gridMinX:F3},{gridMinY:F3})，" +
                        //                 $"网格中心({gridCenterX:F3},{gridCenterY:F3},{gridCenterZ:F3})，" +
                        //                 $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
                        //                 $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "Z坐标不在可通行层范围")}");
                        return false;
                    }

                    // 🔥 新增：邻居障碍位置检查
                    if (!IsSamplePointSafeFromNeighborObstacles(samplePoint, gridPos, gridMap))
                    {
                        // 🔥 修复：使用Origin计算网格左下角
                        var (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
                        var gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
                        var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
                        var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);
                        var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
                        var deltaH2 = sampleHorizontal.h2 - gridCenterH2;
                        
                        // LogManager.Debug($"[斜线检查] 采样点{i}/{samples}邻居障碍检查失败：" +
                        //                 $"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
                        //                 $"网格左下角({gridMinX:F3},{gridMinY:F3})，" +
                        //                 $"网格中心({gridCenterX:F3},{gridCenterY:F3})，" +
                        //                 $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})");
                        return false;
                    }
                }

                if (SegmentIntersectsBlockedGridCellInterior(start, end, gridMap))
                {
                    return false;
                }

                return true;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[斜线检查] 检查过程异常: {ex.Message}");
                return false; // 发生错误时保守返回false
            }
        }

        /// <summary>
        /// 检查采样点是否在网格内远离相邻障碍物的安全位置
        /// </summary>
        /// <param name="samplePoint">采样点世界坐标</param>
        /// <param name="gridPos">采样点所在网格坐标</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>是否安全</returns>
        private bool IsSamplePointSafeFromNeighborObstacles(Point3D samplePoint, GridPoint2D gridPos, GridMap gridMap)
        {
            try
            {
                // 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
                // 计算采样点在所在网格内的相对位置 (0.0 到 1.0)
                var (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
                var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);

                var relativeX = (sampleHorizontal.h1 - gridMinH1) / gridMap.CellSize;
                var relativeY = (sampleHorizontal.h2 - gridMinH2) / gridMap.CellSize;

                // 确保相对位置在有效范围内
                relativeX = Math.Max(0.0, Math.Min(1.0, relativeX));
                relativeY = Math.Max(0.0, Math.Min(1.0, relativeY));

                // 定义8个邻居方向的偏移 (左上、上、右上、左、右、左下、下、右下)
                int[] dx = { -1, 0, 1, -1, 1, -1, 0, 1 };
                int[] dy = { -1, -1, -1, 0, 0, 1, 1, 1 };
                string[] directionNames = { "左上", "上", "右上", "左", "右", "左下", "下", "右下" };

                var obstacleNeighbors = new List<string>();

                // 检查每个邻居网格
                for (int i = 0; i < 8; i++)
                {
                    var neighborX = gridPos.X + dx[i];
                    var neighborY = gridPos.Y + dy[i];
                    var neighborPos = new GridPoint2D(neighborX, neighborY);

                    bool isValidNeighbor = gridMap.IsValidGridPosition(neighborPos);
                    bool isWalkableNeighbor = isValidNeighbor && gridMap.IsWalkable(neighborPos);

                    // 如果邻居网格无效或是障碍物，应用位置约束
                    if (!isValidNeighbor || !isWalkableNeighbor)
                    {
                        obstacleNeighbors.Add(directionNames[i]);

                        // 根据邻居方向应用对应的位置约束
                        bool constraintSatisfied = CheckPositionConstraintForObstacleNeighbor(i, relativeX, relativeY);

                        if (!constraintSatisfied)
                        {
                            // 🔥 修复：基于正确的网格左下角计算网格中心点
                            var gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
                            var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
                            var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
                            var deltaH2 = sampleHorizontal.h2 - gridCenterH2;
                            
                            LogManager.Debug($"[邻居障碍] 位置约束失败：" +
                                            $"{directionNames[i]}方向有障碍，" +
                                            $"网格({gridPos.X},{gridPos.Y})，" +
                                            $"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
                                            $"网格左下角H=({gridMinH1:F3},{gridMinH2:F3})，" +
                                            $"网格中心H=({gridCenterH1:F3},{gridCenterH2:F3})，" +
                                            $"偏差(ΔH1={deltaH1:F3}, ΔH2={deltaH2:F3})，" +
                                            $"相对位置({relativeX:F3},{relativeY:F3})");
                            return false;
                        }
                    }
                }

                return true; // 所有约束都满足，位置安全
            }
            catch (Exception ex)
            {
                LogManager.Error($"[邻居障碍检查] 检查过程异常: {ex.Message}");
                return false; // 发生错误时保守返回false
            }
        }

        /// <summary>
        /// 根据障碍邻居的方向检查位置约束
        /// </summary>
        /// <param name="neighborDirection">邻居方向索引 (0-7)</param>
        /// <param name="relativeX">采样点在网格内的相对X位置 (0.0-1.0)</param>
        /// <param name="relativeY">采样点在网格内的相对Y位置 (0.0-1.0)</param>
        /// <returns>是否满足位置约束</returns>
        private bool CheckPositionConstraintForObstacleNeighbor(int neighborDirection, double relativeX, double relativeY)
        {
            // 容差值仅用于处理浮点数精度问题
            const double tolerance = 1e-3;
            bool result;

            switch (neighborDirection)
            {
                case 0: // 左上邻居是障碍
                    // 采样点不能同时X<0且Y>1
                    result = (relativeX >= -tolerance) || (relativeY <= 1.0 + tolerance);
                    break;

                case 1: // 上邻居是障碍
                    // 采样点不能Y>1（允许Y=1）
                    result = relativeY <= 1.0 + tolerance;
                    break;

                case 2: // 右上邻居是障碍
                    // 采样点不能同时X>1且Y>1
                    result = (relativeX <= 1.0 + tolerance) || (relativeY <= 1.0 + tolerance);
                    break;

                case 3: // 左邻居是障碍
                    // 采样点不能X<0（允许X=0）
                    result = relativeX >= -tolerance;
                    break;

                case 4: // 右邻居是障碍
                    // 采样点不能X>1（允许X=1）
                    result = relativeX <= 1.0 + tolerance;
                    break;

                case 5: // 左下邻居是障碍
                    // 采样点不能同时X<0且Y<0
                    result = (relativeX >= -tolerance) || (relativeY >= -tolerance);
                    break;

                case 6: // 下邻居是障碍
                    // 采样点不能Y<0（允许Y=0）
                    result = relativeY >= -tolerance;
                    break;

                case 7: // 右下邻居是障碍
                    // 采样点不能同时X>1且Y<0
                    result = (relativeX <= 1.0 + tolerance) || (relativeY >= -tolerance);
                    break;

                default:
                    result = true; // 未知方向，默认通过
                    break;
            }

            return result;
        }

        private static double CalculateHostHorizontalDistance(Point3D start, Point3D end, GridMap gridMap)
        {
            var startHorizontal = GetHostHorizontalCoords(start, gridMap);
            var endHorizontal = GetHostHorizontalCoords(end, gridMap);
            double dh1 = endHorizontal.h1 - startHorizontal.h1;
            double dh2 = endHorizontal.h2 - startHorizontal.h2;
            return Math.Sqrt(dh1 * dh1 + dh2 * dh2);
        }

        private static (double h1, double h2) GetGridCellMinHorizontalCoords(GridMap gridMap, GridPoint2D gridPosition)
        {
            var (minH1, _, minH2, _) = gridMap.GetGridCellPlanarBounds(gridPosition);
            return (minH1, minH2);
        }

        private static (double h1, double h2) GetHostHorizontalCoords(Point3D point, GridMap gridMap)
        {
            var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType);
            return HostPlanarGridHelper.GetHorizontalCoords3(
                new System.Numerics.Vector3((float)point.X, (float)point.Y, (float)point.Z),
                adapter);
        }

        private bool SegmentIntersectsBlockedGridCellInterior(Point3D start, Point3D end, GridMap gridMap)
        {
            var startHorizontal = GetHostHorizontalCoords(start, gridMap);
            var endHorizontal = GetHostHorizontalCoords(end, gridMap);
            var originHorizontal = GetHostHorizontalCoords(gridMap.Origin, gridMap);

            int minGridX = (int)Math.Round((Math.Min(startHorizontal.h1, endHorizontal.h1) - originHorizontal.h1) / gridMap.CellSize) - 1;
            int maxGridX = (int)Math.Round((Math.Max(startHorizontal.h1, endHorizontal.h1) - originHorizontal.h1) / gridMap.CellSize) + 1;
            int minGridY = (int)Math.Round((Math.Min(startHorizontal.h2, endHorizontal.h2) - originHorizontal.h2) / gridMap.CellSize) - 1;
            int maxGridY = (int)Math.Round((Math.Max(startHorizontal.h2, endHorizontal.h2) - originHorizontal.h2) / gridMap.CellSize) + 1;

            minGridX = Math.Max(0, minGridX);
            minGridY = Math.Max(0, minGridY);
            maxGridX = Math.Min(gridMap.Width - 1, maxGridX);
            maxGridY = Math.Min(gridMap.Height - 1, maxGridY);

            for (int x = minGridX; x <= maxGridX; x++)
            {
                for (int y = minGridY; y <= maxGridY; y++)
                {
                    var gridPosition = new GridPoint2D(x, y);

                    if (!gridMap.TryGetSegmentGridCellInteriorIntersection(start, end, gridPosition, out double enterT, out double exitT))
                    {
                        continue;
                    }

                    double sampleT = Math.Max(0.0, Math.Min(1.0, (enterT + exitT) * 0.5));
                    Point3D samplePoint = new Point3D(
                        start.X + sampleT * (end.X - start.X),
                        start.Y + sampleT * (end.Y - start.Y),
                        start.Z + sampleT * (end.Z - start.Z));
                    double sampleElevation = gridMap.GetWorldElevation(samplePoint);

                    if (!gridMap.IsPassableAtElevation(gridPosition, sampleElevation, 0.1))
                    {
                        LogManager.Debug(
                            $"[斜线检查] 线段穿过不可通行网格内部: 网格({x},{y}), " +
                            $"t=[{enterT:F3},{exitT:F3}], sampleT={sampleT:F3}");
                        return true;
                    }
                }
            }

            return false;
        }


        /// <summary>
        /// 计算路径总长度
        /// </summary>
        /// <param name="path">路径</param>
        /// <returns>总长度</returns>
        public double CalculatePathLength(List<Point3D> path)
        {
            if (path == null || path.Count < 2)
                return 0;

            double totalLength = 0;
            for (int i = 1; i < path.Count; i++)
            {
                double dx = path[i].X - path[i - 1].X;
                double dy = path[i].Y - path[i - 1].Y;
                totalLength += Math.Sqrt(dx * dx + dy * dy);
            }

            return totalLength;
        }

        /// <summary>
        /// 计算路径复杂度（转弯次数）
        /// </summary>
        /// <param name="path">路径</param>
        /// <returns>转弯次数</returns>
        public int CalculatePathComplexity(List<Point3D> path)
        {
            if (path == null || path.Count < 3)
                return 0;

            int turns = 0;
            const double angleThreshold = TURN_ANGLE_THRESHOLD_RADIANS; // 弧度

            for (int i = 1; i < path.Count - 1; i++)
            {
                var v1 = new GridPoint2D(
                    (int)(path[i].X - path[i - 1].X),
                    (int)(path[i].Y - path[i - 1].Y));
                var v2 = new GridPoint2D(
                    (int)(path[i + 1].X - path[i].X),
                    (int)(path[i + 1].Y - path[i].Y));

                double angle = Math.Atan2(v2.Y, v2.X) - Math.Atan2(v1.Y, v1.X);
                if (Math.Abs(angle) > angleThreshold)
                {
                    turns++;
                }
            }

            return turns;
        }


        /// <summary>
        /// 修正路径的起点和终点为原始用户指定的坐标，避免网格转换造成的错位
        /// </summary>
        /// <param name="path">原始路径</param>
        /// <param name="originalStart">原始起点坐标</param>
        /// <param name="originalEnd">原始终点坐标</param>
        /// <returns>修正后的路径</returns>
        private List<Point3D> CorrectStartEndPoints(List<Point3D> path, Point3D originalStart, Point3D originalEnd, GridMap gridMap)
        {
            if (path == null || path.Count == 0)
                return path;

            try
            {
                LogManager.Info($"[坐标修正] 开始修正路径起终点坐标");
                var correctedPath = new List<Point3D>(path);

                // 修正起点：保持原始宿主水平位置，复用路径求得的高程
                if (correctedPath.Count > 0)
                {
                    double startElevation = gridMap.GetWorldElevation(correctedPath[0]);
                    var correctedStart = gridMap.SetWorldElevation(originalStart, startElevation);
                    LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({correctedStart.X:F3}, {correctedStart.Y:F3}, {correctedStart.Z:F3}), 高程={startElevation:F3}");
                    correctedPath[0] = correctedStart;
                }

                // 修正终点：保持原始宿主水平位置，复用路径求得的高程
                if (correctedPath.Count > 1)
                {
                    var lastIndex = correctedPath.Count - 1;
                    double endElevation = gridMap.GetWorldElevation(correctedPath[lastIndex]);
                    var correctedEnd = gridMap.SetWorldElevation(originalEnd, endElevation);
                    LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({correctedEnd.X:F3}, {correctedEnd.Y:F3}, {correctedEnd.Z:F3}), 高程={endElevation:F3}");
                    correctedPath[lastIndex] = correctedEnd;
                }

                LogManager.Info($"[坐标修正] 路径起终点坐标修正完成");
                return correctedPath;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[坐标修正] 修正路径起终点坐标失败: {ex.Message}，使用原始路径");
                return path;
            }
        }

        /// <summary>
        /// 检查指定点在给定高度约束下是否可通行
        /// </summary>
        /// <param name="gridPos">网格坐标</param>
        /// <param name="point">检查点（世界坐标）</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <returns>是否可通行</returns>
        private bool IsPointPassableAtHeight(GridPoint2D gridPos, Point3D point, GridMap gridMap, double objectHeight)
        {
            try
            {
                // 使用传入的网格坐标，避免重复转换
                int gridX = gridPos.X;
                int gridY = gridPos.Y;

                // 确保坐标在有效范围内
                if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height)
                {
                    LogManager.Warning($"[高度检查] 网格坐标超出范围：({gridX}, {gridY})，网格大小：{gridMap.Width}x{gridMap.Height}");
                    return false;
                }

                var cell = gridMap.Cells[gridX, gridY];
                // 检查基本可行走性
                if (!cell.HasAnyWalkableLayer())
                {
                    LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - HasWalkableLayer={cell.HasAnyWalkableLayer()}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行");
                    return false;
                }

                // 检查高度层约束
                if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
                {
                    // 查找包含指定Z坐标且满足物体高度的层
                    var matchingLayer = gridMap.FindLayerContainingElevation(gridPos, gridMap.GetWorldElevation(point), tolerance: 0.5);

                    if (matchingLayer.HasValue)
                    {
                        bool heightOk = matchingLayer.Value.PassableHeight.GetSpan() >= objectHeight;
                        if (heightOk)
                        {
                            return true;
                        }
                        else
                        {
                            LogManager.Warning($"[高度检查] ❌ 找到匹配高程={gridMap.GetWorldElevation(point):F2}的层高程={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}");
                        }
                    }
                    else
                    {
                        LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含高程={gridMap.GetWorldElevation(point):F2}的层");
                    }
                    return false;
                }

                LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})没有高度层信息");
                return false;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[高度约束检查] 检查点可通行性失败: {ex.Message}");
                return false;
            }
        }

        /// <summary>
        /// 根据网格高度区间调整路径点Z坐标
        /// </summary>
        /// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        /// <param name="originalStart">用户指定的原始起点（保留原始Z坐标）</param>
        /// <param name="originalEnd">用户指定的原始终点（保留原始Z坐标）</param>
        /// <returns>调整后的路径</returns>
        private List<Point3D> ApplyGridHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double objectHeight, Point3D originalStart, Point3D originalEnd)
        {
            LogManager.Info($"[智能选层] 开始为路径点选择最优高度层，路径点数: {pathWithGridCoords.Count}");

            if (pathWithGridCoords.Count == 0)
                return new List<Point3D>();

            var adjustedPath = new List<Point3D>();

            // 1. 使用用户指定的原始起点和终点高程（而不是从转换后的路径中提取）
            double startZ = gridMap.GetWorldElevation(originalStart);
            double endZ = gridMap.GetWorldElevation(originalEnd);

            // 对比日志：显示原始坐标与路径坐标的差异
            var pathStartZ = gridMap.GetWorldElevation(pathWithGridCoords[0].point);
            var pathEndZ = gridMap.GetWorldElevation(pathWithGridCoords[pathWithGridCoords.Count - 1].point);

            LogManager.Info($"[智能选层] 原始起点高程={startZ:F3}, 原始终点高程={endZ:F3}");
            LogManager.Info($"[智能选层] 路径起点高程={pathStartZ:F3}, 路径终点高程={pathEndZ:F3}");
            LogManager.Info($"[智能选层] 使用原始高程进行智能选层，高程变化={endZ - startZ:F3}");

            // 2. 为每个路径点选择高度层
            for (int i = 0; i < pathWithGridCoords.Count; i++)
            {
                var (point, gridPos) = pathWithGridCoords[i];

                // 检查网格位置有效性
                if (!gridMap.IsValidGridPosition(gridPos))
                {
                    adjustedPath.Add(point);
                    continue;
                }

                var cell = gridMap.Cells[gridPos.X, gridPos.Y];

                // 如果没有高度层，使用原始点
                if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
                {
                    adjustedPath.Add(point);
                    continue;
                }

                // 起点和终点：锁定到包含其Z坐标的层
                if (i == 0)
                {
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, startZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
                    if (selectedLayer.HasValue)
                    {
                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
                        LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
                    }
                    else
                    {
                        adjustedPath.Add(point);
                    }
                }
                else if (i == pathWithGridCoords.Count - 1)
                {
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, endZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
                    if (selectedLayer.HasValue)
                    {
                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
                        LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
                    }
                    else
                    {
                        adjustedPath.Add(point);
                    }
                }
                else
                {
                    // 中间点：根据高度趋势选择最佳层
                    var prevZ = gridMap.GetWorldElevation(adjustedPath[i - 1]);
                    var selectedLayer = SelectBestLayer(cell.HeightLayers, prevZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, false);
                    
                    if (selectedLayer.HasValue)
                    {
                        adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
                    }
                    else
                    {
                        adjustedPath.Add(point);
                    }
                }
            }

            LogManager.Info($"[智能选层] 完成，调整了 {adjustedPath.Count}/{pathWithGridCoords.Count} 个点");
            return adjustedPath;
        }

        /// <summary>
        /// 使用激活层字典还原3D路径
        /// </summary>
        /// <param name="gridPath">网格路径</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="activeLayerZ">激活层字典（网格坐标→Z坐标）</param>
        /// <param name="originalStart">原始起点</param>
        /// <param name="originalEnd">原始终点</param>
        /// <returns>3D路径点列表</returns>
        private List<Point3D> ApplyActiveLayers(
            List<GridPoint2D> gridPath,
            GridMap gridMap,
            Dictionary<GridPoint2D, double> activeLayerZ,
            Point3D originalStart,
            Point3D originalEnd)
        {
            LogManager.Info($"[3D还原] 开始使用激活层还原3D路径，路径点数: {gridPath.Count}，激活层记录数: {activeLayerZ.Count}");

            var path3D = new List<Point3D>();

            if (gridPath.Count == 0)
                return path3D;

            for (int i = 0; i < gridPath.Count; i++)
            {
                var gridPos = gridPath[i];
                double elevation;

                // 起点：使用原始起点Z
                if (i == 0)
                {
                    elevation = gridMap.GetWorldElevation(originalStart);
                    LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始起点)");
                }
                // 终点：使用原始终点Z
                else if (i == gridPath.Count - 1)
                {
                    elevation = gridMap.GetWorldElevation(originalEnd);
                    LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始终点)");
                }
                // 中间点：使用A*阶段记录的激活层
                else if (activeLayerZ.TryGetValue(gridPos, out double activeZ))
                {
                    elevation = activeZ;
                    LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (激活层)");
                }
                else
                {
                    // 降级：无激活层记录，使用前一点高度
                    elevation = gridMap.GetWorldElevation(path3D[i - 1]);
                    LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点高程={elevation:F2}m");
                }

                path3D.Add(gridMap.CreateWorldPoint(gridPos, elevation));
            }

            LogManager.Info($"[3D还原] 完成，生成了 {path3D.Count} 个3D路径点");
            return path3D;
        }

        /// <summary>
        /// 为路径点选择最佳高度层
        /// </summary>
        /// <param name="layers">可用的高度层列表</param>
        /// <param name="referenceZ">参考Z坐标（起点/终点为目标Z，中间点为前一点Z）</param>
        /// <param name="objectHeight">物体高度</param>
        /// <param name="startZ">路径起点Z</param>
        /// <param name="endZ">路径终点Z</param>
        /// <param name="currentIndex">当前点索引</param>
        /// <param name="totalPoints">总点数</param>
        /// <param name="exactMatch">是否精确匹配（起点/终点使用）</param>
        /// <returns>选中的高度层，如果没有合适的返回null</returns>
        private HeightLayer? SelectBestLayer(List<HeightLayer> layers, double referenceZ, double objectHeight, 
            double startZ, double endZ, int currentIndex, int totalPoints, bool exactMatch)
        {
            if (layers == null || layers.Count == 0)
                return null;

            // 只有一层，直接返回
            if (layers.Count == 1)
            {
                var layer = layers[0];
                if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight)
                    return layer;
                return null;
            }

            // 确定目标Z坐标
            double targetZ;
            if (exactMatch)
            {
                // 精确匹配：直接使用参考Z
                targetZ = referenceZ;
            }
            else
            {
                // 中间点：根据高度趋势计算目标Z
                int remainingSteps = totalPoints - currentIndex - 1;
                if (remainingSteps <= 0)
                {
                    // 没有剩余步骤，使用参考Z
                    targetZ = referenceZ;
                }
                else
                {
                    // 计算期望的高度变化率
                    double remainingHeight = endZ - referenceZ;
                    double desiredChange = remainingHeight / remainingSteps;
                    targetZ = referenceZ + desiredChange;
                }
            }

            // 选择最接近目标Z且满足物体高度要求的层
            HeightLayer? bestLayer = null;
            double minDistance = double.MaxValue;

            foreach (var layer in layers)
            {
                if (!layer.IsWalkable)
                    continue;

                if (layer.PassableHeight.GetSpan() < objectHeight)
                    continue;

                double distance = Math.Abs(layer.Z - targetZ);
                if (distance < minDistance)
                {
                    minDistance = distance;
                    bestLayer = layer;
                }
            }

            return bestLayer;
        }

        /// <summary>
        /// 将A*返回的米坐标转换为网格坐标（统一方法）
        /// </summary>
        /// <param name="astarPosition">A*位置（米坐标）</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>网格坐标</returns>
        private GridPoint2D ConvertAStarPositionToGrid(Position astarPosition, GridMap gridMap)
        {
            double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
            int gridX = (int)Math.Round(astarPosition.X / cellSizeInMeters);
            int gridY = (int)Math.Round(astarPosition.Y / cellSizeInMeters);

            return new GridPoint2D(gridX, gridY);
        }

        /// <summary>
        /// 执行A*算法的核心逻辑
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="astarGrid">A*网格</param>
        /// <returns>路径查找结果</returns>
        private PathFindingResult ExecuteAStarAlgorithm(Point3D start, Point3D end, GridMap gridMap, Grid astarGrid)
        {
            try
            {
                // 转换起点和终点到网格坐标
                var startGrid = gridMap.WorldToGrid(start);
                var endGrid = gridMap.WorldToGrid(end);

                LogManager.Info($"[A*执行] 网格坐标转换 - 起点: ({startGrid.X}, {startGrid.Y}), 终点: ({endGrid.X}, {endGrid.Y})");

                // 获取单位转换因子
                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);

                // 正确：直接使用网格索引（RoyT.AStar内部处理米坐标转换）
                var startPos = new GridPosition(startGrid.X, startGrid.Y);
                var endPos = new GridPosition(endGrid.X, endGrid.Y);

                // 执行A*算法
                var pathfinder = new PathFinder();
                var astarPath = pathfinder.FindPath(startPos, endPos, astarGrid);

                if (astarPath != null && astarPath.Edges.Count > 0)
                {
                    // 转换回世界坐标
                    var worldPath = ConvertPathToWorldCoordinates(astarPath, gridMap);

                    // 创建结果对象
                    var result = new PathFindingResult
                    {
                        PathPoints = worldPath,
                        OriginalEndPoint = end
                    };

                    // 检查路径类型
                    if (astarPath.Type == Roy_T.AStar.Paths.PathType.ClosestApproach)
                    {
                        LogManager.Info($"[A*执行] 找到部分路径（最近接近），包含 {astarPath.Edges.Count + 1} 个网格点");
                        LogManager.Warning($"[A*执行] 无法完全到达目标点，已找到最接近的可达点");

                        // 计算完成百分比
                        var lastNode = astarPath.Edges.Last().End;
                        var actualEndGrid = ConvertAStarPositionToGrid(lastNode.Position, gridMap);
                        var actualEndCell = gridMap.GetCell(actualEndGrid);
                        var actualEndWorld = gridMap.GridToWorld3D(actualEndGrid);

                        var originalEndCell = gridMap.GetCell(endGrid);
                        var originalEndWorld = gridMap.GridToWorld3D(endGrid);
                        var startWorldCell = gridMap.GetCell(startGrid);
                        var startWorld = gridMap.GridToWorld3D(startGrid);

                        var totalDistance = CalculateDistance(startWorld, originalEndWorld);
                        var actualDistance = CalculateDistance(startWorld, actualEndWorld);
                        var completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;

                        // 设置部分路径信息
                        result.IsComplete = false;
                        result.ActualEndPoint = actualEndWorld;
                        result.CompletionPercentage = completionPercentage;

                        LogManager.Info($"[A*执行] 路径完成度: {completionPercentage:F1}% (实际到达点: {actualEndWorld.X:F2}, {actualEndWorld.Y:F2}, {actualEndWorld.Z:F2})");
                    }
                    else
                    {
                        LogManager.Info($"[A*执行] 找到完整路径，包含 {astarPath.Edges.Count + 1} 个网格点");
                        result.IsComplete = true;
                        result.ActualEndPoint = end;
                        result.CompletionPercentage = 100.0;
                    }

                    return result;
                }
                else
                {
                    LogManager.Warning("[A*执行] 未找到路径");
                    return PathFindingResult.Failure("A*算法未找到路径，起点和终点之间被障碍物阻断");
                }
            }
            catch (Exception ex)
            {
                LogManager.Error($"[A*执行] A*算法执行失败: {ex.Message}");
                throw new AutoPathPlanningException($"A*算法执行失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 验证路径中所有点的高度约束（特别检查门网格）
        /// </summary>
        /// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
        /// <param name="gridMap">网格地图</param>
        /// <param name="objectHeight">物体高度（模型单位）</param>
        private void ValidatePathHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double objectHeight)
        {
            LogManager.Info($"[路径高度验证] 开始验证 {pathWithGridCoords.Count} 个路径点的高度约束");

            int checkedPoints = 0;
            int passedPoints = 0;
            int failedPoints = 0;
            int doorPoints = 0;

            for (int i = 0; i < pathWithGridCoords.Count; i++)
            {
                var (point, gridPos) = pathWithGridCoords[i];

                if (gridMap.IsValidGridPosition(gridPos))
                {
                    var cell = gridMap.Cells[gridPos.X, gridPos.Y];
                    checkedPoints++;

                    // 特别标记门网格点
                    if (cell.CellType == "门")
                    {
                        doorPoints++;
                        LogManager.Info($"[路径高度验证] 检查路径点{i}（门网格）: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})");
                    }

                    // 进行高度约束检查
                    bool isPassable = IsPointPassableAtHeight(gridPos, point, gridMap, objectHeight);
                    if (isPassable)
                    {
                        passedPoints++;
                    }
                    else
                    {
                        failedPoints++;
                        LogManager.Warning($"[路径高度验证] ❌ 路径点{i}高度约束检查失败！");
                    }
                }
            }

            LogManager.Info($"[路径高度验证] 完成 - 检查点数: {checkedPoints}, 通过: {passedPoints}, 失败: {failedPoints}, 门网格点: {doorPoints}");

            if (failedPoints > 0)
            {
                LogManager.Warning($"[路径高度验证] 警告：发现 {failedPoints} 个点不满足高度约束！");
            }
        }

        /// <summary>
        /// 计算两点间的3D距离
        /// </summary>
        private double CalculateDistance(Point3D point1, Point3D point2)
        {
            var dx = point2.X - point1.X;
            var dy = point2.Y - point1.Y;
            var dz = point2.Z - point1.Z;
            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        /// <summary>
        /// 优化路径（去除共线点等）
        /// </summary>
        /// <param name="pathResult">原始路径查找结果</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>优化后的路径查找结果</returns>
        private PathFindingResult OptimizePath(PathFindingResult pathResult, GridMap gridMap = null)
        {
            if (pathResult == null || pathResult.PathPoints.Count <= 2)
            {
                return pathResult;
            }

            try
            {
                // 创建临时路径用于优化
                var tempRoute = new PathRoute("临时优化路径");
                for (int i = 0; i < pathResult.PathPoints.Count; i++)
                {
                    var point = pathResult.PathPoints[i];
                    var pathPoint = new PathPoint
                    {
                        Position = point,
                        Index = i,
                        Type = i == 0 ? PathPointType.StartPoint :
                               i == pathResult.PathPoints.Count - 1 ? PathPointType.EndPoint :
                               PathPointType.WayPoint,
                        Name = $"路径点{i}"
                    };
                    tempRoute.Points.Add(pathPoint);
                }

                // 使用基于网格的路径优化算法
                // 使用基于网格的路径优化算法
                if (gridMap != null)
                {
                    // 创建路径优化器并执行优化
                    var optimizerConfig = new PathOptimizer.OptimizationConfig
                    {
                        EnableSimplification = true,
                        CollinearTolerance = 0.01
                    };
                    var optimizer = new PathOptimizer(optimizerConfig);
                    var optimizedRoute = optimizer.OptimizePath(tempRoute, gridMap);

                    if (optimizedRoute != null && optimizedRoute.Points.Count > 0)
                    {
                        // 提取优化后的点位置
                        var optimizedPoints = optimizedRoute.Points.Select(p => p.Position).ToList();

                        LogManager.Info($"[路径优化] 点数变化：{pathResult.PathPoints.Count} -> {optimizedPoints.Count}");

                        // 更新路径结果
                        pathResult.PathPoints = optimizedPoints;

                        var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap);

                        if (diagonalOptimizedPoints.Count != optimizedPoints.Count)
                        {
                            LogManager.Info($"[斜线优化] 二次优化点数变化：{optimizedPoints.Count} -> {diagonalOptimizedPoints.Count}");
                            pathResult.PathPoints = diagonalOptimizedPoints;
                        }
                        else
                        {
                            LogManager.Info($"[斜线优化] 斜线优化未能进一步减少点数，保持现有结果");
                        }
                    }
                }
                else
                {
                    LogManager.Info($"[路径生成] 未提供网格地图，跳过路径优化，点数: {pathResult.PathPoints.Count}");
                }

                // 打印最终返回给用户的路径点详情
                LogManager.Info($"[最终路径详情] 共 {pathResult.PathPoints.Count} 个点：");
                for (int i = 0; i < pathResult.PathPoints.Count; i++)
                {
                    var pt = pathResult.PathPoints[i];
                    LogManager.Info($"  [最终点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
                }

                return pathResult;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[路径优化] 优化失败，使用原始路径: {ex.Message}", ex);
                return pathResult; // 出错时返回原始路径
            }
        }

        /// <summary>
        /// 安全优先的A*网格转换方法
        /// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
        /// </summary>
        private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
        {
            try
            {
                LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式");

                // 初始化激活高度层字典和目标高度
                var activeLayerZ = new Dictionary<GridPoint2D, double>();
                double targetZ = gridMap.GetWorldElevation(endPos);

                // 清空日志记录集合，确保每次执行都能看到映射关系
                _loggedDistances.Clear();

                // 1. 创建网格基础结构（参照局部直线优先）
                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
                var gridSize = new GridSize(gridMap.Width, gridMap.Height);
                var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));

                // 使用基础速度创建网格，后续会动态调整
                var baseVelocity = Velocity.FromKilometersPerHour(5);  // 基础速度
                var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, baseVelocity);

                LogManager.Info($"[安全优先] A*网格创建完成，尺寸: {gridMap.Width}x{gridMap.Height}");

                // 2. 🔥 关键步骤：断开所有连接，准备重新按安全距离连接
                LogManager.Info($"[安全优先断开诊断] 开始断开所有节点连接，网格尺寸: {gridMap.Width}x{gridMap.Height}");
                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var pos = new GridPosition(x, y);
                        grid.DisconnectNode(pos);
                    }
                }
                LogManager.Info($"[安全优先断开诊断] 所有节点连接断开完成，准备重新构建");

                // 3. 计算安全距离图
                var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
                LogManager.Info($"[安全优先] 安全距离计算完成");

                // 4. 智能重连：基于安全距离重新构建连接
                int connectedCells = 0;
                int heightConstrainedCells = 0;
                int safetyConnections = 0;
                var velocityStats = new Dictionary<float, int>();

                for (int x = 0; x < gridMap.Width; x++)
                {
                    for (int y = 0; y < gridMap.Height; y++)
                    {
                        var cell = gridMap.Cells[x, y];

                        // 检查是否有兼容的高度层
                        if (!HasCompatibleLayer(cell, objectHeight))
                        {
                            if (cell.HasAnyWalkableLayer())
                            {
                                heightConstrainedCells++;
                            }
                            continue;
                        }

                        // 选择当前网格的最佳高度层
                        var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
                        if (!currentZ.HasValue)
                        {
                            continue;
                        }

                        var pos = new GridPosition(x, y);
                        var gridPos = new GridPoint2D(x, y);
                        var currentSafetyDistance = safetyDistanceMap[x, y];

                        // 记录当前网格的激活高度层
                        activeLayerZ[gridPos] = currentZ.Value;
                        connectedCells++;

                        // 🔥 核心改动：右方向连接（基于安全距离）
                        if (x + 1 < gridMap.Width)
                        {
                            var rightCell = gridMap.Cells[x + 1, y];
                            
                            // 检查右侧网格是否有兼容的高度层
                            if (HasCompatibleLayer(rightCell, objectHeight))
                            {
                                // 选择右侧网格的最佳高度层
                                var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
                                if (rightZ.HasValue)
                                {
                                    var rightPos = new GridPosition(x + 1, y);
                                    var rightGridPos = new GridPoint2D(x + 1, y);
                                    var rightSafetyDistance = safetyDistanceMap[x + 1, y];

                                    // 记录右侧网格的激活高度层
                                    activeLayerZ[rightGridPos] = rightZ.Value;

                                    // 使用两个位置中更安全的距离
                                    var maxSafetyDistance = Math.Max(currentSafetyDistance, rightSafetyDistance);
                                    var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = baseSafetySpeed * heightMultiplier;
                                    var rightVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 统计速度分布（使用最终速度）
                                    if (velocityStats.ContainsKey(finalSpeed))
                                        velocityStats[finalSpeed]++;
                                    else
                                        velocityStats[finalSpeed] = 1;

                                    // 添加单向边
                                    grid.AddEdge(pos, rightPos, rightVelocity);
                                    safetyConnections++;
                                }
                            }
                        }

                        // 🔥 核心改动：下方向连接（基于安全距离）
                        if (y + 1 < gridMap.Height)
                        {
                            var bottomCell = gridMap.Cells[x, y + 1];
                            
                            // 检查下方网格是否有兼容的高度层
                            if (HasCompatibleLayer(bottomCell, objectHeight))
                            {
                                // 选择下方网格的最佳高度层
                                var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
                                if (bottomZ.HasValue)
                                {
                                    var bottomPos = new GridPosition(x, y + 1);
                                    var bottomGridPos = new GridPoint2D(x, y + 1);
                                    var bottomSafetyDistance = safetyDistanceMap[x, y + 1];

                                    // 记录下方网格的激活高度层
                                    activeLayerZ[bottomGridPos] = bottomZ.Value;

                                    // 使用两个位置中更安全的距离
                                    var maxSafetyDistance = Math.Max(currentSafetyDistance, bottomSafetyDistance);
                                    var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = baseSafetySpeed * heightMultiplier;
                                    var bottomVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 统计速度分布（使用最终速度）
                                    if (velocityStats.ContainsKey(finalSpeed))
                                        velocityStats[finalSpeed]++;
                                    else
                                        velocityStats[finalSpeed] = 1;

                                    // 添加单向边
                                    grid.AddEdge(pos, bottomPos, bottomVelocity);
                                    safetyConnections++;
                                }
                            }
                        }

                        // 🔥 连通性修复：左方向连接（基于安全距离）
                        if (x - 1 >= 0)
                        {
                            var leftCell = gridMap.Cells[x - 1, y];
                            
                            // 检查左侧网格是否有兼容的高度层
                            if (HasCompatibleLayer(leftCell, objectHeight))
                            {
                                // 选择左侧网格的最佳高度层
                                var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
                                if (leftZ.HasValue)
                                {
                                    var leftPos = new GridPosition(x - 1, y);
                                    var leftGridPos = new GridPoint2D(x - 1, y);
                                    var leftSafetyDistance = safetyDistanceMap[x - 1, y];

                                    // 记录左侧网格的激活高度层
                                    activeLayerZ[leftGridPos] = leftZ.Value;

                                    // 使用两个位置中更安全的距离
                                    var maxSafetyDistance = Math.Max(currentSafetyDistance, leftSafetyDistance);
                                    var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = baseSafetySpeed * heightMultiplier;
                                    var leftVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 统计速度分布（使用最终速度）
                                    if (velocityStats.ContainsKey(finalSpeed))
                                        velocityStats[finalSpeed]++;
                                    else
                                        velocityStats[finalSpeed] = 1;

                                    // 添加单向边
                                    grid.AddEdge(pos, leftPos, leftVelocity);
                                    safetyConnections++;
                                }
                            }
                        }

                        // 🔥 连通性修复：上方向连接（基于安全距离）
                        if (y - 1 >= 0)
                        {
                            var topCell = gridMap.Cells[x, y - 1];
                            
                            // 检查上方网格是否有兼容的高度层
                            if (HasCompatibleLayer(topCell, objectHeight))
                            {
                                // 选择上方网格的最佳高度层
                                var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
                                if (topZ.HasValue)
                                {
                                    var topPos = new GridPosition(x, y - 1);
                                    var topGridPos = new GridPoint2D(x, y - 1);
                                    var topSafetyDistance = safetyDistanceMap[x, y - 1];

                                    // 记录上方网格的激活高度层
                                    activeLayerZ[topGridPos] = topZ.Value;

                                    // 使用两个位置中更安全的距离
                                    var maxSafetyDistance = Math.Max(currentSafetyDistance, topSafetyDistance);
                                    var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);

                                    // 添加高度惩罚：离目标高度越远，速度越慢
                                    double distanceToTarget = Math.Abs(topZ.Value - targetZ);
                                    float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
                                    float finalSpeed = baseSafetySpeed * heightMultiplier;
                                    var topVelocity = Velocity.FromKilometersPerHour(finalSpeed);

                                    // 统计速度分布（使用最终速度）
                                    if (velocityStats.ContainsKey(finalSpeed))
                                        velocityStats[finalSpeed]++;
                                    else
                                        velocityStats[finalSpeed] = 1;

                                    // 添加单向边
                                    grid.AddEdge(pos, topPos, topVelocity);
                                    safetyConnections++;
                                }
                            }
                        }
                    }
                }

                // 输出统计信息
                LogManager.Info($"[安全优先] 网格重建完成，连接统计:");
                LogManager.Info($"[安全优先]   - 连接的节点: {connectedCells}");
                LogManager.Info($"[安全优先]   - 被高度约束排除: {heightConstrainedCells}");
                LogManager.Info($"[安全优先]   - 安全连接数: {safetyConnections}");
                LogManager.Info($"[安全优先]   - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
                LogManager.Info($"[安全优先]   - 激活层记录数: {activeLayerZ.Count}");

                // 输出速度统计信息
                if (velocityStats.Count > 0)
                {
                    var sortedVelocityStats = velocityStats.OrderBy(kv => kv.Key);
                    var velocityStatsText = string.Join(", ", sortedVelocityStats.Select(kv => $"{kv.Key:F1}km/h:{kv.Value}条边"));
                    LogManager.Info($"[安全优先速度统计] {velocityStatsText}");
                }

                return (grid, activeLayerZ);
            }
            catch (Exception ex)
            {
                LogManager.Error($"[安全优先] 网格转换失败: {ex.Message}");
                throw new AutoPathPlanningException($"安全优先网格转换失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 计算安全距离图
        /// 直接使用GridMap的CellType，将障碍物和Unknown（边界）都视为不安全区域
        /// </summary>
        private int[,] CalculateSafetyDistanceFromGridMap(GridMap gridMap)
        {
            var distanceMap = new int[gridMap.Width, gridMap.Height];

            // 统计网格类型分布
            int obstacleCount = 0, unknownCount = 0, walkableCount = 0;

            // 初始化距离矩阵：障碍物和Unknown=0，可通行=无穷大
            for (int x = 0; x < gridMap.Width; x++)
            {
                for (int y = 0; y < gridMap.Height; y++)
                {
                    var cell = gridMap.Cells[x, y];
                    // 🔥 核心修改：直接使用IsWalkable判断，所有不可通行的网格都视为障碍物
                    bool isUnsafeArea = !cell.HasAnyWalkableLayer();

                    distanceMap[x, y] = isUnsafeArea ? 0 : int.MaxValue;

                    // 统计网格类型
                    if (cell.CellType == "障碍物")
                        obstacleCount++;
                    else if (cell.CellType == "空洞")
                        unknownCount++;
                    else if (cell.HasAnyWalkableLayer())
                        walkableCount++;
                }
            }

            LogManager.Info($"[安全距离计算] 网格类型统计: 障碍物={obstacleCount}, Unknown(边界)={unknownCount}, 可通行={walkableCount}, 总计={gridMap.Width * gridMap.Height}");
            LogManager.Info($"[安全距离计算] 不安全区域(障碍物+边界)={obstacleCount + unknownCount}, 占比={(double)(obstacleCount + unknownCount) / (gridMap.Width * gridMap.Height) * 100:F1}%");

            // 正向扫描（从左上到右下）
            for (int x = 0; x < gridMap.Width; x++)
            {
                for (int y = 0; y < gridMap.Height; y++)
                {
                    if (distanceMap[x, y] == int.MaxValue)
                    {
                        int minDist = int.MaxValue;

                        if (x > 0 && distanceMap[x - 1, y] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1);
                        if (y > 0 && distanceMap[x, y - 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1);
                        if (x > 0 && y > 0 && distanceMap[x - 1, y - 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + 1);
                        if (x > 0 && y < gridMap.Height - 1 && distanceMap[x - 1, y + 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + 1);

                        if (minDist != int.MaxValue)
                            distanceMap[x, y] = minDist;
                    }
                }
            }

            // 反向扫描（从右下到左上）
            for (int x = gridMap.Width - 1; x >= 0; x--)
            {
                for (int y = gridMap.Height - 1; y >= 0; y--)
                {
                    if (distanceMap[x, y] != 0 && distanceMap[x, y] != int.MaxValue)
                    {
                        int minDist = distanceMap[x, y];

                        if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1);
                        if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1);
                        if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1);
                        if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1);

                        distanceMap[x, y] = minDist;
                    }
                    else if (distanceMap[x, y] == int.MaxValue)
                    {
                        int minDist = int.MaxValue;

                        if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1);
                        if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1);
                        if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1);
                        if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue)
                            minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1);

                        if (minDist != int.MaxValue)
                            distanceMap[x, y] = minDist;
                    }
                }
            }

            // 统计安全距离分布
            var distanceHistogram = new Dictionary<int, int>();
            int totalWalkableCells = 0;
            int totalDistanceSum = 0;

            for (int x = 0; x < gridMap.Width; x++)
            {
                for (int y = 0; y < gridMap.Height; y++)
                {
                    var distance = distanceMap[x, y];
                    if (distance != int.MaxValue && distance > 0)
                    {
                        totalWalkableCells++;
                        totalDistanceSum += distance;

                        // 分组统计：1,2,3,4,5-9,10+
                        int distanceGroup;
                        if (distance <= 4)
                            distanceGroup = distance;
                        else if (distance <= 9)
                            distanceGroup = 5; // 代表5-9
                        else
                            distanceGroup = 10; // 代表10+

                        if (distanceHistogram.ContainsKey(distanceGroup))
                            distanceHistogram[distanceGroup]++;
                        else
                            distanceHistogram[distanceGroup] = 1;
                    }
                }
            }

            if (totalWalkableCells > 0)
            {
                double averageDistance = (double)totalDistanceSum / totalWalkableCells;
                var sortedStats = distanceHistogram.OrderBy(kv => kv.Key);
                var statsText = string.Join(", ", sortedStats.Select(kv =>
                    kv.Key <= 4 ? $"距离{kv.Key}:{kv.Value}格" :
                    kv.Key == 5 ? $"距离5-9:{kv.Value}格" :
                    $"距离10+:{kv.Value}格"
                ));

                LogManager.Info($"[安全距离分布] {statsText}");
                LogManager.Info($"[安全距离分析] 平均距离: {averageDistance:F2}格, 可通行网格总数: {totalWalkableCells}");
            }

            return distanceMap;
        }


        // 静态集合用于记录已打印的速度映射，避免重复日志
        private static readonly HashSet<int> _loggedDistances = new HashSet<int>();

        /// <summary>
        /// 根据障碍物距离计算安全速度值（浮点数）
        /// </summary>
        private float CalculateSpeedBySafetyDistance(int safetyDistance)
        {
            float finalSpeed;

            // 🔧 温和映射：参考局部直线优先算法，使用5-35km/h范围，减少网格敏感性
            if (safetyDistance >= 5)
                finalSpeed = 35.0f;   // 35 km/h（最高速度，安全区域）
            else if (safetyDistance >= 3)
                finalSpeed = 25.0f;   // 25 km/h
            else if (safetyDistance >= 2)
                finalSpeed = 15.0f;   // 15 km/h
            else if (safetyDistance >= 1)
                finalSpeed = 10.0f;   // 10 km/h
            else
                finalSpeed = 5.0f;    // 5 km/h（最低速度，但不是极端值）

            // 只在每种距离第一次出现时记录速度映射
            if (!_loggedDistances.Contains(safetyDistance))
            {
                _loggedDistances.Add(safetyDistance);
            }

            return finalSpeed;
        }

        /// <summary>
        /// 获取GridMap缓存统计信息
        /// </summary>
        /// <returns>缓存统计报告</returns>
        public static string GetGridMapCacheStatistics()
        {
            return GridMapGenerator.GetCacheStatistics();
        }

        /// <summary>
        /// 获取GridMap缓存详细统计报告
        /// </summary>
        /// <returns>详细统计报告</returns>
        public static string GetDetailedGridMapCacheReport()
        {
            return GridMapGenerator.GetDetailedCacheReport();
        }

        /// <summary>
        /// 清除GridMap缓存
        /// </summary>
        public static void ClearGridMapCache()
        {
            GridMapGenerator.ClearCache();
        }

        /// <summary>
        /// 记录路径规划完成时的缓存统计
        /// </summary>
        private void LogCacheStatistics()
        {
            var stats = GlobalGridMapCache.Instance.Statistics;
            if (stats.HitCount + stats.MissCount > 0)
            {
                LogManager.Info($"[GridMap缓存统计] {stats.GenerateReport()}");
            }
        }
    }
}
