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

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 3D体素网格类 - 用于真3D路径规划
    /// 提供完整的3D空间离散化表示，支持任意高度的物体和通道
    /// </summary>
    public class VoxelGrid
    {
        /// <summary>
        /// 3D体素数组 [x, y, z]
        /// 注意：索引顺序为 [宽度, 深度, 高度]
        /// </summary>
        private VoxelCell[,,] cells;

        /// <summary>
        /// 网格原点（世界坐标，模型单位）
        /// 对应体素索引 (0, 0, 0) 的世界坐标位置
        /// </summary>
        public Point3D Origin { get; private set; }

        /// <summary>
        /// 单个体素的尺寸（模型单位）
        /// 所有三个维度使用相同的体素尺寸（立方体体素）
        /// </summary>
        public double VoxelSize { get; private set; }

        /// <summary>
        /// X方向（宽度）的体素数量
        /// </summary>
        public int SizeX { get; private set; }

        /// <summary>
        /// Y方向（深度）的体素数量
        /// </summary>
        public int SizeY { get; private set; }

        /// <summary>
        /// Z方向（高度）的体素数量
        /// </summary>
        public int SizeZ { get; private set; }

        /// <summary>
        /// 网格边界框（世界坐标，模型单位）
        /// </summary>
        public BoundingBox3D Bounds { get; private set; }

        /// <summary>
        /// 总体素数量
        /// </summary>
        public int TotalVoxels => SizeX * SizeY * SizeZ;

        /// <summary>
        /// 构造函数 - 创建指定尺寸的体素网格
        /// </summary>
        /// <param name="bounds">网格边界框（世界坐标，模型单位）</param>
        /// <param name="voxelSize">体素尺寸（模型单位）</param>
        public VoxelGrid(BoundingBox3D bounds, double voxelSize)
        {
            if (voxelSize <= 0)
                throw new ArgumentException("体素尺寸必须大于0", nameof(voxelSize));

            VoxelSize = voxelSize;
            Bounds = bounds;
            Origin = bounds.Min;

            // 计算每个维度需要的体素数量（向上取整以覆盖整个边界框）
            double width = bounds.Max.X - bounds.Min.X;
            double depth = bounds.Max.Y - bounds.Min.Y;
            double height = bounds.Max.Z - bounds.Min.Z;

            SizeX = (int)Math.Ceiling(width / voxelSize);
            SizeY = (int)Math.Ceiling(depth / voxelSize);
            SizeZ = (int)Math.Ceiling(height / voxelSize);

            // 初始化体素数组（所有体素默认为可通行）
            cells = new VoxelCell[SizeX, SizeY, SizeZ];
            InitializeCells();
        }

        /// <summary>
        /// 初始化所有体素单元
        /// </summary>
        private void InitializeCells()
        {
            for (int x = 0; x < SizeX; x++)
            {
                for (int y = 0; y < SizeY; y++)
                {
                    for (int z = 0; z < SizeZ; z++)
                    {
                        cells[x, y, z] = new VoxelCell();
                    }
                }
            }
        }

        /// <summary>
        /// 获取指定索引的体素单元
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>体素单元，如果索引越界返回null</returns>
        public VoxelCell GetCell(int x, int y, int z)
        {
            if (!IsValidIndex(x, y, z))
                return null;

            return cells[x, y, z];
        }

        /// <summary>
        /// 设置指定索引的体素单元
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <param name="cell">体素单元</param>
        /// <returns>是否设置成功</returns>
        public bool SetCell(int x, int y, int z, VoxelCell cell)
        {
            if (!IsValidIndex(x, y, z))
                return false;

            cells[x, y, z] = cell;
            return true;
        }

        /// <summary>
        /// 检查体素索引是否有效（在网格范围内）
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>索引是否有效</returns>
        public bool IsValidIndex(int x, int y, int z)
        {
            return x >= 0 && x < SizeX &&
                   y >= 0 && y < SizeY &&
                   z >= 0 && z < SizeZ;
        }

        /// <summary>
        /// 世界坐标转换为体素索引
        /// 注意：体素索引代表体素的左下角，而不是中心点
        /// </summary>
        /// <param name="worldPos">世界坐标（模型单位）</param>
        /// <returns>体素索引 (x, y, z)</returns>
        public (int x, int y, int z) WorldToVoxel(Point3D worldPos)
        {
            int x = (int)Math.Floor((worldPos.X - Origin.X) / VoxelSize);
            int y = (int)Math.Floor((worldPos.Y - Origin.Y) / VoxelSize);
            int z = (int)Math.Floor((worldPos.Z - Origin.Z) / VoxelSize);

            return (x, y, z);
        }

        /// <summary>
        /// 体素索引转换为世界坐标（体素的左下角坐标）
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>世界坐标（模型单位）</returns>
        public Point3D VoxelToWorld(int x, int y, int z)
        {
            double worldX = Origin.X + x * VoxelSize;
            double worldY = Origin.Y + y * VoxelSize;
            double worldZ = Origin.Z + z * VoxelSize;

            return new Point3D(worldX, worldY, worldZ);
        }

        /// <summary>
        /// 体素索引转换为世界坐标（体素的中心点坐标）
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>世界坐标（模型单位）</returns>
        public Point3D VoxelToWorldCenter(int x, int y, int z)
        {
            double halfVoxel = VoxelSize / 2.0;
            double worldX = Origin.X + x * VoxelSize + halfVoxel;
            double worldY = Origin.Y + y * VoxelSize + halfVoxel;
            double worldZ = Origin.Z + z * VoxelSize + halfVoxel;

            return new Point3D(worldX, worldY, worldZ);
        }

        /// <summary>
        /// 获取体素的6邻域（上下左右前后）
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>邻居体素索引列表</returns>
        public List<(int x, int y, int z)> GetNeighbors6(int x, int y, int z)
        {
            var neighbors = new List<(int, int, int)>();

            // 6个方向：右、左、后、前、上、下
            int[,] directions = {
                { 1, 0, 0 },   // +X 右
                { -1, 0, 0 },  // -X 左
                { 0, 1, 0 },   // +Y 后
                { 0, -1, 0 },  // -Y 前
                { 0, 0, 1 },   // +Z 上
                { 0, 0, -1 }   // -Z 下
            };

            for (int i = 0; i < 6; i++)
            {
                int nx = x + directions[i, 0];
                int ny = y + directions[i, 1];
                int nz = z + directions[i, 2];

                if (IsValidIndex(nx, ny, nz))
                {
                    neighbors.Add((nx, ny, nz));
                }
            }

            return neighbors;
        }

        /// <summary>
        /// 获取体素的26邻域（包括对角线方向）
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>邻居体素索引列表</returns>
        public List<(int x, int y, int z)> GetNeighbors26(int x, int y, int z)
        {
            var neighbors = new List<(int, int, int)>();

            // 26个方向：3x3x3立方体去掉中心点
            for (int dx = -1; dx <= 1; dx++)
            {
                for (int dy = -1; dy <= 1; dy++)
                {
                    for (int dz = -1; dz <= 1; dz++)
                    {
                        // 跳过中心点
                        if (dx == 0 && dy == 0 && dz == 0)
                            continue;

                        int nx = x + dx;
                        int ny = y + dy;
                        int nz = z + dz;

                        if (IsValidIndex(nx, ny, nz))
                        {
                            neighbors.Add((nx, ny, nz));
                        }
                    }
                }
            }

            return neighbors;
        }

        /// <summary>
        /// 获取两个体素之间的欧几里得距离（体素单位）
        /// </summary>
        /// <param name="x1">第一个体素的X索引</param>
        /// <param name="y1">第一个体素的Y索引</param>
        /// <param name="z1">第一个体素的Z索引</param>
        /// <param name="x2">第二个体素的X索引</param>
        /// <param name="y2">第二个体素的Y索引</param>
        /// <param name="z2">第二个体素的Z索引</param>
        /// <returns>欧几里得距离（体素单位）</returns>
        public double GetDistance(int x1, int y1, int z1, int x2, int y2, int z2)
        {
            int dx = x2 - x1;
            int dy = y2 - y1;
            int dz = z2 - z1;

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

        /// <summary>
        /// 获取两个体素之间的曼哈顿距离（体素单位）
        /// </summary>
        /// <param name="x1">第一个体素的X索引</param>
        /// <param name="y1">第一个体素的Y索引</param>
        /// <param name="z1">第一个体素的Z索引</param>
        /// <param name="x2">第二个体素的X索引</param>
        /// <param name="y2">第二个体素的Y索引</param>
        /// <param name="z2">第二个体素的Z索引</param>
        /// <returns>曼哈顿距离（体素单位）</returns>
        public int GetManhattanDistance(int x1, int y1, int z1, int x2, int y2, int z2)
        {
            return Math.Abs(x2 - x1) + Math.Abs(y2 - y1) + Math.Abs(z2 - z1);
        }

        /// <summary>
        /// 检查指定索引的体素是否可通行
        /// </summary>
        /// <param name="x">X方向索引</param>
        /// <param name="y">Y方向索引</param>
        /// <param name="z">Z方向索引</param>
        /// <returns>是否可通行（索引越界返回false）</returns>
        public bool IsPassable(int x, int y, int z)
        {
            var cell = GetCell(x, y, z);
            return cell != null && cell.IsPassable;
        }

        /// <summary>
        /// 检查世界坐标位置是否可通行
        /// </summary>
        /// <param name="worldPos">世界坐标（模型单位）</param>
        /// <returns>是否可通行</returns>
        public bool IsPassable(Point3D worldPos)
        {
            var (x, y, z) = WorldToVoxel(worldPos);
            return IsPassable(x, y, z);
        }

        /// <summary>
        /// 获取网格的统计信息
        /// </summary>
        /// <returns>(总体素数, 可通行体素数, 障碍物体素数)</returns>
        public (int total, int passable, int obstacle) GetStatistics()
        {
            int total = TotalVoxels;
            int passable = 0;
            int obstacle = 0;

            for (int x = 0; x < SizeX; x++)
            {
                for (int y = 0; y < SizeY; y++)
                {
                    for (int z = 0; z < SizeZ; z++)
                    {
                        if (cells[x, y, z].IsPassable)
                            passable++;
                        else
                            obstacle++;
                    }
                }
            }

            return (total, passable, obstacle);
        }

        /// <summary>
        /// 清除所有体素（重置为可通行状态）
        /// </summary>
        public void Clear()
        {
            InitializeCells();
        }

        /// <summary>
        /// 返回网格的字符串表示
        /// </summary>
        public override string ToString()
        {
            var (total, passable, obstacle) = GetStatistics();
            double passableRatio = (double)passable / total * 100.0;
            return $"VoxelGrid[{SizeX}x{SizeY}x{SizeZ}={total}体素, 可通行:{passableRatio:F1}%, 体素尺寸:{VoxelSize:F2}模型单位]";
        }

        /// <summary>
        /// 获取与包围盒相交的所有体素索引
        /// 用于标记障碍物：找出被模型对象占据的所有体素
        /// </summary>
        /// <param name="bounds">包围盒（世界坐标，模型单位）</param>
        /// <returns>相交的体素索引列表</returns>
        public List<(int x, int y, int z)> GetVoxelsInBounds(BoundingBox3D bounds)
        {
            var result = new List<(int, int, int)>();

            // 将包围盒的世界坐标转换为体素索引
            var minIndices = WorldToVoxel(bounds.Min);
            var maxIndices = WorldToVoxel(bounds.Max);

            // 遍历包围盒覆盖的所有体素
            // 注意：maxIndices 可能超出网格范围，需要裁剪
            int startX = Math.Max(0, minIndices.x);
            int startY = Math.Max(0, minIndices.y);
            int startZ = Math.Max(0, minIndices.z);

            int endX = Math.Min(SizeX - 1, maxIndices.x);
            int endY = Math.Min(SizeY - 1, maxIndices.y);
            int endZ = Math.Min(SizeZ - 1, maxIndices.z);

            for (int x = startX; x <= endX; x++)
            {
                for (int y = startY; y <= endY; y++)
                {
                    for (int z = startZ; z <= endZ; z++)
                    {
                        result.Add((x, y, z));
                    }
                }
            }

            return result;
        }

        #region 障碍物膨胀算法

        /// <summary>
        /// 障碍物膨胀 - 基于Fast Sweeping算法的3D距离变换
        /// 为障碍物体素添加安全缓冲区，确保路径规划时保持足够的安全距离
        /// </summary>
        /// <param name="inflationRadiusMeters">膨胀半径（米）</param>
        /// <param name="metersToModelUnits">米到模型单位的转换系数</param>
        /// <returns>膨胀的体素数量</returns>
        public int InflateObstacles(double inflationRadiusMeters, double metersToModelUnits)
        {
            LogManager.Info($"[体素膨胀] 开始膨胀，半径: {inflationRadiusMeters}米");

            // 1. 计算膨胀半径（模型单位）
            double inflationRadiusInModelUnits = inflationRadiusMeters * metersToModelUnits;

            if (inflationRadiusInModelUnits <= 0)
            {
                LogManager.Info($"[体素膨胀] 膨胀半径为0，跳过膨胀");
                return 0;
            }

            LogManager.Info($"[体素膨胀] 膨胀半径: {inflationRadiusInModelUnits:F2}模型单位");

            // 2. 创建距离网格（使用g4的DenseGrid3f）
            float upperBound = (SizeX + SizeY + SizeZ) * (float)VoxelSize;
            var distanceGrid = new g4.DenseGrid3f(SizeX, SizeY, SizeZ, upperBound);

            // 3. 初始化距离图：障碍物=0, 可通行区域=upperBound
            int obstacleCount = 0;

            for (int x = 0; x < SizeX; x++)
            {
                for (int y = 0; y < SizeY; y++)
                {
                    for (int z = 0; z < SizeZ; z++)
                    {
                        var cell = cells[x, y, z];

                        // 简单逻辑：只有障碍物（IsPassable=false）才设置为膨胀源（distance=0）
                        // 所有可通行区域（门、楼梯、通道等IsPassable=true）都不膨胀
                        if (!cell.IsPassable)
                        {
                            distanceGrid[x, y, z] = 0f;
                            obstacleCount++;
                        }
                        else
                        {
                            distanceGrid[x, y, z] = upperBound;
                        }
                    }
                }
            }

            LogManager.Info($"[体素膨胀] 初始化完成 - 障碍物体素: {obstacleCount}");

            // 4. 迭代膨胀：只在XY平面（水平方向）膨胀，不在Z方向膨胀
            // 使用简单的形态学膨胀，每次迭代向XY方向扩展1个体素
            LogManager.Info($"[体素膨胀] 开始迭代膨胀（只在XY平面）...");
            var inflationStopwatch = System.Diagnostics.Stopwatch.StartNew();

            int inflationRadius = (int)Math.Ceiling(inflationRadiusInModelUnits / VoxelSize);
            LogManager.Info($"[体素膨胀] 膨胀半径: {inflationRadius} 个体素");

            int totalInflated = 0;

            // 迭代膨胀 inflationRadius 次
            for (int iteration = 0; iteration < inflationRadius; iteration++)
            {
                // 记录本次迭代要膨胀的体素（避免在遍历时修改）
                var toInflate = new List<(int x, int y, int z)>();

                // 遍历所有体素，找到障碍物的XY邻域
                for (int x = 0; x < SizeX; x++)
                {
                    for (int y = 0; y < SizeY; y++)
                    {
                        for (int z = 0; z < SizeZ; z++)
                        {
                            // 只处理障碍物体素
                            if (!cells[x, y, z].IsPassable)
                            {
                                // 检查XY平面的4个邻居（上下左右），不检查Z方向
                                int[] dx = { -1, 1, 0, 0 };
                                int[] dy = { 0, 0, -1, 1 };

                                for (int dir = 0; dir < 4; dir++)
                                {
                                    int nx = x + dx[dir];
                                    int ny = y + dy[dir];
                                    int nz = z; // Z保持不变，不向上下膨胀

                                    // 检查边界
                                    if (nx >= 0 && nx < SizeX && ny >= 0 && ny < SizeY)
                                    {
                                        // 如果邻居是可通行的，标记为待膨胀
                                        if (cells[nx, ny, nz].IsPassable)
                                        {
                                            toInflate.Add((nx, ny, nz));
                                        }
                                    }
                                }
                            }
                        }
                    }
                }

                // 应用膨胀
                int iterationCount = 0;
                foreach (var (x, y, z) in toInflate)
                {
                    // 再次检查，避免重复标记
                    if (cells[x, y, z].IsPassable)
                    {
                        cells[x, y, z].SetAsObstacle();
                        cells[x, y, z].Type = CategoryAttributeManager.LogisticsElementType.障碍物;
                        iterationCount++;
                    }
                }

                totalInflated += iterationCount;
                LogManager.Info($"[体素膨胀] 第 {iteration + 1}/{inflationRadius} 次迭代：膨胀了 {iterationCount} 个体素");

                // 如果本次没有膨胀任何体素，提前结束
                if (iterationCount == 0)
                {
                    LogManager.Info($"[体素膨胀] 第 {iteration + 1} 次迭代无新增膨胀，提前结束");
                    break;
                }
            }

            inflationStopwatch.Stop();
            LogManager.Info($"[体素膨胀] 迭代膨胀完成，耗时: {inflationStopwatch.ElapsedMilliseconds}ms");
            LogManager.Info($"[体素膨胀] 总膨胀体素数: {totalInflated}");
            return totalInflated;
        }

        /// <summary>
        /// 执行Fast Sweeping算法（8方向扫描传播距离）
        /// 参考geometry4Sharp的sweep_pass实现
        /// </summary>
        /// <param name="distanceGrid">距离网格</param>
        /// <param name="cellSize">体素尺寸</param>
        private void PerformFastSweeping(g4.DenseGrid3f distanceGrid, float cellSize)
        {
            // 8方向扫描（与g4的sweep_pass相同）
            Sweep(distanceGrid, cellSize, +1, +1, +1);
            Sweep(distanceGrid, cellSize, -1, -1, -1);
            Sweep(distanceGrid, cellSize, +1, +1, -1);
            Sweep(distanceGrid, cellSize, -1, -1, +1);
            Sweep(distanceGrid, cellSize, +1, -1, +1);
            Sweep(distanceGrid, cellSize, -1, +1, -1);
            Sweep(distanceGrid, cellSize, +1, -1, -1);
            Sweep(distanceGrid, cellSize, -1, +1, +1);
        }

        /// <summary>
        /// 单向扫描 - 从g4的sweep方法移植
        /// 沿指定方向扫描网格，传播距离值
        /// </summary>
        /// <param name="grid">距离网格</param>
        /// <param name="cellSize">体素尺寸</param>
        /// <param name="di">X方向增量（+1或-1）</param>
        /// <param name="dj">Y方向增量（+1或-1）</param>
        /// <param name="dk">Z方向增量（+1或-1）</param>
        private void Sweep(g4.DenseGrid3f grid, float cellSize, int di, int dj, int dk)
        {
            // 确定扫描范围和方向
            int i0 = di > 0 ? 1 : grid.ni - 2;
            int i1 = di > 0 ? grid.ni : -1;
            int j0 = dj > 0 ? 1 : grid.nj - 2;
            int j1 = dj > 0 ? grid.nj : -1;
            int k0 = dk > 0 ? 1 : grid.nk - 2;
            int k1 = dk > 0 ? grid.nk : -1;

            // 三层循环扫描
            for (int k = k0; k != k1; k += dk)
            {
                for (int j = j0; j != j1; j += dj)
                {
                    for (int i = i0; i != i1; i += di)
                    {
                        // 检查7个邻居并更新当前点的距离
                        // 邻居方向：与扫描方向相反的7个方向
                        CheckAndUpdate(grid, cellSize, i, j, k, i - di, j, k);
                        CheckAndUpdate(grid, cellSize, i, j, k, i, j - dj, k);
                        CheckAndUpdate(grid, cellSize, i, j, k, i - di, j - dj, k);
                        CheckAndUpdate(grid, cellSize, i, j, k, i, j, k - dk);
                        CheckAndUpdate(grid, cellSize, i, j, k, i - di, j, k - dk);
                        CheckAndUpdate(grid, cellSize, i, j, k, i, j - dj, k - dk);
                        CheckAndUpdate(grid, cellSize, i, j, k, i - di, j - dj, k - dk);
                    }
                }
            }
        }

        /// <summary>
        /// 检查邻居并更新距离 - 从g4的check_neighbour移植
        /// 如果通过邻居到达当前点的距离更短，则更新当前点的距离
        /// </summary>
        /// <param name="grid">距离网格</param>
        /// <param name="cellSize">体素尺寸</param>
        /// <param name="i0">当前点X索引</param>
        /// <param name="j0">当前点Y索引</param>
        /// <param name="k0">当前点Z索引</param>
        /// <param name="i1">邻居X索引</param>
        /// <param name="j1">邻居Y索引</param>
        /// <param name="k1">邻居Z索引</param>
        private void CheckAndUpdate(g4.DenseGrid3f grid, float cellSize,
            int i0, int j0, int k0, int i1, int j1, int k1)
        {
            // 边界检查
            if (i1 < 0 || i1 >= grid.ni || j1 < 0 || j1 >= grid.nj || k1 < 0 || k1 >= grid.nk)
                return;

            // 计算从邻居到当前点的边长
            float dx = (i0 - i1) * cellSize;
            float dy = (j0 - j1) * cellSize;
            float dz = (k0 - k1) * cellSize;
            float edgeDistance = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);

            // 计算通过邻居到达当前点的新距离
            float newDistance = grid[i1, j1, k1] + edgeDistance;

            // 如果新距离更短，更新
            if (newDistance < grid[i0, j0, k0])
            {
                grid[i0, j0, k0] = newDistance;
            }
        }

        #endregion
    }
}
