using DotRecast.Core.Numerics;
using DotRecast.Detour;
using DotRecast.Recast;
using System.Buffers; // 引入 ArrayPool
using System.Collections.Concurrent;
using System.Numerics;
using XNet.Business.Parser;

namespace XNet.Business.PathNavigation
{
    public class NavMeshManager
    {
        // --- 核心类：地图资源模板 (共享) ---
        private class NavMeshTemplate : IDisposable
        {
            public string TemplateId { get; }
            public DtNavMesh NavMesh { get; }
            public DtNavMeshQuery Query { get; }
            public ReaderWriterLockSlim RwLock { get; }

            private bool _disposed = false;

            public NavMeshTemplate(string id, DtNavMesh mesh, DtNavMeshQuery query)
            {
                TemplateId = id;
                NavMesh = mesh;
                Query = query;
                RwLock = new ReaderWriterLockSlim();
            }

            public void Dispose()
            {
                Dispose(true);
                GC.SuppressFinalize(this);
            }

            protected virtual void Dispose(bool disposing)
            {
                if (_disposed) return;

                if (disposing)
                {
                    // 释放托管资源
                    RwLock?.Dispose();
                    //// 释放DotRecast的非托管资源
                    //Query?.Dispose();
                    //NavMesh?.Dispose();
                }

                _disposed = true;
            }

            ~NavMeshTemplate()
            {
                Dispose(false);
            }
        }

        // --- 数据存储 ---
        private readonly ConcurrentDictionary<string, NavMeshTemplate> _templates = new();
        private readonly ConcurrentDictionary<string, string> _instanceMap = new();

        // 寻路配置 (SceneAgent 需要访问，改为 Public)
        private readonly DtQueryDefaultFilter _filter;
        private readonly RcVec3f _extents = new RcVec3f(20.0f, 20.0f, 20.0f);

        public DtQueryDefaultFilter Filter => _filter;
        public RcVec3f Extents => _extents;

        public NavMeshManager()
        {
            _filter = new DtQueryDefaultFilter(
                includeFlags: 0xFFFF,
                excludeFlags: 0,
                areaCost: new float[] { 1f, 1f, 1f, 1f, 1f, 1f }
            );
        }

        // ==========================================
        // 1. 资源管理
        // ==========================================

        public void LoadTemplate(string templateId, string objFilePath)
        {
            Console.WriteLine($"[NavMesh] 开始构建模板: {templateId} ...");
            NavMeshTemplate newTemplate = null;
            try
            {
                newTemplate = BuildTemplateInternal(templateId, objFilePath);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"[Error] 模板 {templateId} 构建失败: {ex.Message}");
                return;
            }

            _templates.AddOrUpdate(templateId,
                addValueFactory: (k) =>
                {
                    Console.WriteLine($"[NavMesh] 模板 {k} 加载完成。");
                    return newTemplate;
                },
                updateValueFactory: (k, old) =>
                {
                    Console.WriteLine($"[NavMesh] 模板 {k} 更新 (Reload)。");
                    return newTemplate;
                });
        }

        public void UnloadTemplate(string templateId)
        {
            if (_templates.TryRemove(templateId, out var template))
            {
                template.Dispose();
                Console.WriteLine($"[NavMesh] 模板 {templateId} 已卸载。");
            }
        }

        // ==========================================
        // 2. 副本实例管理
        // ==========================================

        public bool CreateInstance(string roomId, string templateId)
        {
            if (!_templates.ContainsKey(templateId))
            {
                Console.WriteLine($"[Error] 无法创建实例 {roomId}, 资源 {templateId} 不存在。");
                return false;
            }
            _instanceMap[roomId] = templateId;
            return true;
        }

        public void RemoveInstance(string roomId)
        {
            _instanceMap.TryRemove(roomId, out _);
        }

        public void ClearInstanceMap()
        {
            _instanceMap.Clear();
        }

        // ==========================================
        // 3. 【新增】供 SceneAgent 使用的辅助方法
        // ==========================================

        /// <summary>
        /// 获取指定副本实例底层的 NavMesh 和 Query 对象
        /// 注意：返回的对象是共享资源，请勿 Dispose，也请勿修改其拓扑结构
        /// </summary>
        public bool GetNavMeshAndQuery(string roomId, out DtNavMesh navMesh, out DtNavMeshQuery query)
        {
            navMesh = null!;
            query = null!;

            if (!_instanceMap.TryGetValue(roomId, out var templateId)) return false;
            if (!_templates.TryGetValue(templateId, out var template)) return false;

            navMesh = template.NavMesh;
            query = template.Query;
            return true;
        }

        public RcVec3f? GetClosetPoint(string roomId, Vector3 originPt, out long startRef)
        {
            startRef = 0;
            if (!_instanceMap.TryGetValue(roomId, out var templateId)) return null;
            if (!_templates.TryGetValue(templateId, out var template)) return null;
            var query = template.Query;
            var startPos = new RcVec3f(originPt.X, originPt.Y, originPt.Z);
            query.FindNearestPoly(startPos, _extents, _filter, out startRef, out var startPt, out var _);

            return startPt;
        }

        // ==========================================
        // 4. 高性能寻路
        // ==========================================

        public List<Vector3>? FindPath(string roomId, Vector3 start, Vector3 end)
        {
            if (!_instanceMap.TryGetValue(roomId, out var templateId)) return null;
            if (!_templates.TryGetValue(templateId, out var template)) return null;

            const int MAX_POLYS = 256;
            const int MAX_SMOOTH = 256;

            long[] polyBuffer = ArrayPool<long>.Shared.Rent(MAX_POLYS);
            DtStraightPath[] straightPathBuffer = ArrayPool<DtStraightPath>.Shared.Rent(MAX_SMOOTH);

            template.RwLock.EnterReadLock();
            try
            {
                var query = template.Query;
                var startPos = new RcVec3f(start.X, start.Y, start.Z);
                var endPos = new RcVec3f(end.X, end.Y, end.Z);

                query.FindNearestPoly(startPos, _extents, _filter, out long startRef, out var startPt, out var _);
                query.FindNearestPoly(endPos, _extents, _filter, out long endRef, out var endPt, out var _);

                if (startRef == 0 || endRef == 0) return null;

                var status = query.FindPath(
                    startRef, endRef, startPt, endPt, _filter,
                    polyBuffer, out int pathCount, MAX_POLYS
                );

                if (status.Failed() || pathCount == 0) return null;

                query.FindStraightPath(
                    startPt, endPt, polyBuffer.AsSpan(0, pathCount), pathCount,
                    straightPathBuffer, out int straightPathCount, MAX_SMOOTH, 0
                );

                var result = new List<Vector3>(straightPathCount);
                for (int i = 0; i < straightPathCount; i++)
                {
                    var pos = straightPathBuffer[i].pos;
                    result.Add(new Vector3(pos.X, pos.Y, pos.Z));
                }
                return result;
            }
            finally
            {
                template.RwLock.ExitReadLock();
                ArrayPool<long>.Shared.Return(polyBuffer, clearArray: true);
                ArrayPool<DtStraightPath>.Shared.Return(straightPathBuffer, clearArray: true);
            }
        }

        // ==========================================
        // 5. 内部构建逻辑
        // ==========================================
        private NavMeshTemplate BuildTemplateInternal(string id, string objFilePath)
        {
            var objData = SimpleObjParser.Parse(objFilePath);

            var geom = new SimpleInputGeomProvider(objData.Vertices, objData.Indices);

            // --- 2. 配置烘焙参数 ---
            var walkableAreaMod = new RcAreaModification(63);
            var config = new RcConfig(
                useTiles: false,             // 单块 Mesh
                tileSizeX: 0,
                tileSizeZ: 0,
                borderSize: 0,
                partition: RcPartition.WATERSHED,
                cellSize: 0.1f,
                cellHeight: 0.1f,
                agentMaxSlope: 45f,       // 允许 45 度坡
                agentHeight: 0.3f,
                agentRadius: 0.1f,         // 极小半径
                agentMaxClimb: 0.5f,
                minRegionArea: 0.0f,        // 不过滤小区域
                mergeRegionArea: 0.0f,
                edgeMaxLen: 12.0f,
                edgeMaxError: 1.3f,
                vertsPerPoly: 6,
                detailSampleDist: 6.0f,
                detailSampleMaxError: 1.0f,
                filterLowHangingObstacles: false, // 过滤开关 
                filterLedgeSpans: false,
                filterWalkableLowHeightSpans: false,
                walkableAreaMod: walkableAreaMod,
                buildMeshDetail: true
            );

            // --- 3. 构建 NavMesh ---
            RcBuilder builder = new RcBuilder();
            RcVec3f bmin = geom.GetMeshBoundsMin();
            RcVec3f bmax = geom.GetMeshBoundsMax();

            // 【关键修复】确保高度
            if (Math.Abs(bmax.Y - bmin.Y) < 0.001f)
            {
                Console.WriteLine("Flat mesh detected. Increasing Bounds Height...");
                bmax.Y += 5.0f;
                bmin.Y -= 0.5f;
            }
            else // 即使不是绝对平面，如果太薄，也强制加高
            {
                bmax.Y += 5.0f;
                bmin.Y -= 0.5f;
            }

            //Console.WriteLine($"Adjusted Bounds: Min({bmin.X},{bmin.Y},{bmin.Z}) Max({bmax.X},{bmax.Y},{bmax.Z})");
            //Console.WriteLine($"Adjusted Size: {bmax.X - bmin.X}, {bmax.Y - bmin.Y}, {bmax.Z - bmin.Z}");

            var builderConfig = new RcBuilderConfig(config, bmin, bmax);
            if (builderConfig.width <= 0 || builderConfig.height <= 0)
            {
                throw new Exception($"网格尺寸计算错误！Width: {builderConfig.width}, Height: {builderConfig.height}.");
            }

            RcBuilderResult result = builder.Build(geom, builderConfig, false);

            var meshData = result.Mesh;
            // 检查点：npolys 应该是 > 0
            if (meshData.npolys == 0)
            {
                throw new Exception($"Recast 构建完成，但生成了 0 个多边形。请检查 OBJ 数据和参数。");
            }

            // 【强制修复】设置 Flags 为 1 (Walkable)
            for (int i = 0; i < meshData.npolys; ++i)
            {
                meshData.flags[i] = 1;
            }

            // --- 4. 转换为 Detour 数据 ---
            var meshDetail = result.MeshDetail;
            var navMeshDataParams = new DtNavMeshCreateParams
            {
                verts = meshData.verts,
                vertCount = meshData.nverts,
                polys = meshData.polys,
                polyFlags = meshData.flags,
                polyAreas = meshData.areas,
                polyCount = meshData.npolys,
                nvp = meshData.nvp,
                detailMeshes = meshDetail?.meshes ?? new int[0],
                detailVerts = meshDetail?.verts ?? new float[0],
                detailVertsCount = meshDetail?.nverts ?? 0,
                detailTris = meshDetail?.tris ?? new int[0],
                detailTriCount = meshDetail?.ntris ?? 0,
                offMeshConVerts = new float[0],
                offMeshConRad = new float[0],
                offMeshConDir = new int[0],
                offMeshConAreas = new int[0],
                offMeshConFlags = new int[0],
                offMeshConUserID = new int[0], // 确保是 uint[]
                offMeshConCount = 0,
                bmin = meshData.bmin,
                bmax = meshData.bmax,
                walkableHeight = config.WalkableHeightWorld,
                walkableRadius = config.WalkableRadiusWorld,
                walkableClimb = config.WalkableClimbWorld,
                cs = config.Cs,
                ch = config.Ch,
                buildBvTree = true
            };

            var navMeshData = DtNavMeshBuilder.CreateNavMeshData(navMeshDataParams);
            if (navMeshData == null) throw new Exception("Failed to create Detour NavMeshData.");


            // 1. 初始化一个空的 NavMesh 容器
            var dtParams = new DtNavMeshParams();
            dtParams.orig = navMeshData.header.bmin; // 设置原点
            dtParams.tileWidth = navMeshData.header.bmax.X - navMeshData.header.bmin.X; // 设置块宽
            dtParams.tileHeight = navMeshData.header.bmax.Z - navMeshData.header.bmin.Z; // 设置块高
            dtParams.maxTiles = 1; // 只有一块
            dtParams.maxPolys = navMeshData.header.polyCount;

            var navMesh = new DtNavMesh();
            // 使用 Init 方法加载数据

            navMesh.Init(dtParams, 6);
            // 关键：添加导航数据Tile到NavMesh
            navMesh.AddTile(navMeshData, 0, 0, out long tileRef);
            if (tileRef == 0)
            {
                throw new Exception("Failed to add tile to NavMesh!");
            }
            var navMeshQuery = new DtNavMeshQuery(navMesh);

            return new NavMeshTemplate(id, navMesh, navMeshQuery);
        }
    }
}