using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 3D体素网格路径查找器
    /// 基于A*算法的真3D路径规划实现
    /// </summary>
    public class VoxelPathFinder
    {
        private readonly VoxelGrid _voxelGrid;

        /// <summary>
        /// 邻域类型
        /// </summary>
        public enum NeighborType
        {
            /// <summary>6邻域（上下左右前后）</summary>
            Six = 6,

            /// <summary>26邻域（包括对角线）</summary>
            TwentySix = 26
        }

        /// <summary>
        /// 启发式函数类型
        /// </summary>
        public enum HeuristicType
        {
            /// <summary>欧几里得距离</summary>
            Euclidean,

            /// <summary>曼哈顿距离</summary>
            Manhattan,

            /// <summary>切比雪夫距离</summary>
            Chebyshev
        }

        /// <summary>
        /// 路径查找配置
        /// </summary>
        public class PathFindingConfig
        {
            /// <summary>邻域类型（默认26邻域）</summary>
            public NeighborType NeighborType { get; set; } = NeighborType.TwentySix;

            /// <summary>启发式函数类型（默认欧几里得）</summary>
            public HeuristicType HeuristicType { get; set; } = HeuristicType.Euclidean;

            /// <summary>启发式权重（>1加速搜索但可能不是最优路径）</summary>
            public double HeuristicWeight { get; set; } = 1.0;

            /// <summary>垂直移动额外成本系数（鼓励水平移动）</summary>
            public double VerticalCostFactor { get; set; } = 1.5;

            /// <summary>对角线移动成本系数</summary>
            public double DiagonalCostFactor { get; set; } = Math.Sqrt(2);

            /// <summary>最大搜索节点数（防止无限循环）</summary>
            public int MaxSearchNodes { get; set; } = 100000;
        }

        /// <summary>
        /// A*节点
        /// </summary>
        private class AStarNode : IComparable<AStarNode>
        {
            public int X { get; set; }
            public int Y { get; set; }
            public int Z { get; set; }

            /// <summary>从起点到当前节点的实际成本</summary>
            public double GCost { get; set; }

            /// <summary>从当前节点到终点的启发式估计成本</summary>
            public double HCost { get; set; }

            /// <summary>总成本 F = G + H</summary>
            public double FCost => GCost + HCost;

            /// <summary>父节点</summary>
            public AStarNode Parent { get; set; }

            public int CompareTo(AStarNode other)
            {
                int fCompare = FCost.CompareTo(other.FCost);
                if (fCompare != 0) return fCompare;
                // F相同时，比较H（优先选择离终点近的）
                return HCost.CompareTo(other.HCost);
            }

            public override bool Equals(object obj)
            {
                if (obj is AStarNode other)
                {
                    return X == other.X && Y == other.Y && Z == other.Z;
                }
                return false;
            }

            public override int GetHashCode()
            {
                // .NET Framework 4.8不支持HashCode.Combine，使用传统方式
                unchecked
                {
                    int hash = 17;
                    hash = hash * 31 + X.GetHashCode();
                    hash = hash * 31 + Y.GetHashCode();
                    hash = hash * 31 + Z.GetHashCode();
                    return hash;
                }
            }
        }

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="voxelGrid">体素网格</param>
        public VoxelPathFinder(VoxelGrid voxelGrid)
        {
            _voxelGrid = voxelGrid ?? throw new ArgumentNullException(nameof(voxelGrid));
        }

        /// <summary>
        /// 查找3D路径
        /// </summary>
        /// <param name="start">起点世界坐标</param>
        /// <param name="end">终点世界坐标</param>
        /// <param name="config">路径查找配置</param>
        /// <returns>路径点列表（世界坐标），如果无路径则返回null</returns>
        public List<Point3D> FindPath(Point3D start, Point3D end, PathFindingConfig config = null)
        {
            config = config ?? new PathFindingConfig();

            // 转换为体素坐标
            var startVoxel = _voxelGrid.WorldToVoxel(start);
            var endVoxel = _voxelGrid.WorldToVoxel(end);

            LogManager.Info($"[VoxelPathFinder] 开始路径规划");
            LogManager.Info($"  起点: 世界{start} -> 体素({startVoxel.x}, {startVoxel.y}, {startVoxel.z})");
            LogManager.Info($"  终点: 世界{end} -> 体素({endVoxel.x}, {endVoxel.y}, {endVoxel.z})");

            // 验证起终点
            if (!ValidatePoint(startVoxel.x, startVoxel.y, startVoxel.z, "起点"))
                return null;

            if (!ValidatePoint(endVoxel.x, endVoxel.y, endVoxel.z, "终点"))
                return null;

            // 执行A*搜索
            var pathVoxels = FindPathAStar(startVoxel, endVoxel, config);

            if (pathVoxels == null || pathVoxels.Count == 0)
            {
                LogManager.Warning("[VoxelPathFinder] 未找到路径");
                return null;
            }

            // 转换为世界坐标
            var pathWorld = pathVoxels.Select(v => _voxelGrid.VoxelToWorld(v.X, v.Y, v.Z)).ToList();

            LogManager.Info($"[VoxelPathFinder] 路径找到！长度: {pathVoxels.Count}个体素, {CalculatePathLength(pathWorld):F2}米");

            return pathWorld;
        }

        /// <summary>
        /// A*路径搜索核心算法
        /// </summary>
        private List<AStarNode> FindPathAStar((int x, int y, int z) start, (int x, int y, int z) end, PathFindingConfig config)
        {
            var startTime = System.Diagnostics.Stopwatch.StartNew();

            // 开放列表（使用SortedSet作为优先队列）
            var openSet = new SortedSet<AStarNode>();

            // 已访问节点集合（快速查找）
            var closedSet = new HashSet<(int, int, int)>();

            // 节点字典（快速访问G成本）
            var nodeDict = new Dictionary<(int, int, int), AStarNode>();

            // 创建起始节点
            var startNode = new AStarNode
            {
                X = start.x,
                Y = start.y,
                Z = start.z,
                GCost = 0,
                HCost = CalculateHeuristic(start, end, config.HeuristicType)
            };

            openSet.Add(startNode);
            nodeDict[(start.x, start.y, start.z)] = startNode;

            int nodesExpanded = 0;

            while (openSet.Count > 0)
            {
                // 检查节点数限制
                if (++nodesExpanded > config.MaxSearchNodes)
                {
                    LogManager.Warning($"[VoxelPathFinder] 搜索节点数超过限制 {config.MaxSearchNodes}");
                    break;
                }

                // 获取F值最小的节点
                var current = openSet.Min;
                openSet.Remove(current);

                // 到达终点
                if (current.X == end.x && current.Y == end.y && current.Z == end.z)
                {
                    startTime.Stop();
                    LogManager.Info($"[VoxelPathFinder] A*搜索完成: 耗时{startTime.ElapsedMilliseconds}ms, 扩展节点{nodesExpanded}个");
                    return ReconstructPath(current);
                }

                closedSet.Add((current.X, current.Y, current.Z));

                // 遍历邻居
                var neighbors = GetNeighbors(current.X, current.Y, current.Z, config.NeighborType);

                foreach (var (nx, ny, nz) in neighbors)
                {
                    // 跳过已访问的节点
                    if (closedSet.Contains((nx, ny, nz)))
                        continue;

                    // 跳过不可通行的节点
                    if (!_voxelGrid.IsPassable(nx, ny, nz))
                        continue;

                    // 计算移动成本
                    double moveCost = CalculateMoveCost(current.X, current.Y, current.Z, nx, ny, nz, config);
                    double tentativeG = current.GCost + moveCost;

                    // 检查是否找到更好的路径
                    if (!nodeDict.TryGetValue((nx, ny, nz), out var neighbor))
                    {
                        // 新节点
                        neighbor = new AStarNode
                        {
                            X = nx,
                            Y = ny,
                            Z = nz,
                            GCost = tentativeG,
                            HCost = CalculateHeuristic((nx, ny, nz), end, config.HeuristicType) * config.HeuristicWeight,
                            Parent = current
                        };

                        nodeDict[(nx, ny, nz)] = neighbor;
                        openSet.Add(neighbor);
                    }
                    else if (tentativeG < neighbor.GCost)
                    {
                        // 找到更好的路径，更新节点
                        openSet.Remove(neighbor);
                        neighbor.GCost = tentativeG;
                        neighbor.Parent = current;
                        openSet.Add(neighbor);
                    }
                }
            }

            startTime.Stop();
            LogManager.Warning($"[VoxelPathFinder] 未找到路径: 耗时{startTime.ElapsedMilliseconds}ms, 扩展节点{nodesExpanded}个");
            return null;
        }

        /// <summary>
        /// 重建路径（从终点回溯到起点）
        /// </summary>
        private List<AStarNode> ReconstructPath(AStarNode endNode)
        {
            var path = new List<AStarNode>();
            var current = endNode;

            while (current != null)
            {
                path.Add(current);
                current = current.Parent;
            }

            path.Reverse();
            return path;
        }

        /// <summary>
        /// 获取邻居节点
        /// </summary>
        private List<(int x, int y, int z)> GetNeighbors(int x, int y, int z, NeighborType neighborType)
        {
            if (neighborType == NeighborType.Six)
            {
                return _voxelGrid.GetNeighbors6(x, y, z);
            }
            else
            {
                return _voxelGrid.GetNeighbors26(x, y, z);
            }
        }

        /// <summary>
        /// 计算移动成本
        /// </summary>
        private double CalculateMoveCost(int x1, int y1, int z1, int x2, int y2, int z2, PathFindingConfig config)
        {
            double dx = x2 - x1;
            double dy = y2 - y1;
            double dz = z2 - z1;

            // 基础欧几里得距离
            double baseCost = Math.Sqrt(dx * dx + dy * dy + dz * dz);

            // 垂直移动额外成本
            if (dz != 0)
            {
                baseCost *= config.VerticalCostFactor;
            }

            // 对角线移动
            if (dx != 0 && dy != 0 && dz == 0)
            {
                baseCost *= config.DiagonalCostFactor;
            }

            // 考虑体素类型的速度限制
            var cell = _voxelGrid.GetCell(x2, y2, z2);
            if (cell.SpeedLimit > 0 && cell.SpeedLimit < 1.0)
            {
                baseCost /= cell.SpeedLimit; // 速度限制越低，成本越高
            }

            return baseCost;
        }

        /// <summary>
        /// 计算启发式估计
        /// </summary>
        private double CalculateHeuristic((int x, int y, int z) from, (int x, int y, int z) to, HeuristicType heuristicType)
        {
            int dx = Math.Abs(to.x - from.x);
            int dy = Math.Abs(to.y - from.y);
            int dz = Math.Abs(to.z - from.z);

            switch (heuristicType)
            {
                case HeuristicType.Euclidean:
                    return Math.Sqrt(dx * dx + dy * dy + dz * dz);

                case HeuristicType.Manhattan:
                    return dx + dy + dz;

                case HeuristicType.Chebyshev:
                    return Math.Max(Math.Max(dx, dy), dz);

                default:
                    return Math.Sqrt(dx * dx + dy * dy + dz * dz);
            }
        }

        /// <summary>
        /// 验证点是否有效
        /// </summary>
        private bool ValidatePoint(int x, int y, int z, string pointName)
        {
            if (!_voxelGrid.IsValidIndex(x, y, z))
            {
                LogManager.Error($"[VoxelPathFinder] {pointName}超出体素网格范围: ({x}, {y}, {z})");
                return false;
            }

            if (!_voxelGrid.IsPassable(x, y, z))
            {
                LogManager.Error($"[VoxelPathFinder] {pointName}不可通行: ({x}, {y}, {z})");
                return false;
            }

            return true;
        }

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

            double length = 0;
            for (int i = 1; i < path.Count; i++)
            {
                var p1 = path[i - 1];
                var p2 = path[i];
                double dx = p2.X - p1.X;
                double dy = p2.Y - p1.Y;
                double dz = p2.Z - p1.Z;
                length += Math.Sqrt(dx * dx + dy * dy + dz * dz);
            }

            return length;
        }
    }
}
