using System;
using System.Collections.Generic;
using UnityEngine;

// Filename : CMoveAgent.cs
// Creator  : ported/simplified from C++ (AutoPFImp/AutoMoveImp/MoveAgent.*)
// Date     : 2026/01/09

namespace AutoMove
{
    /// <summary>
    /// Minimal MoveAgent: loads movemap and provides A* path on layer0 rmap.
    /// 最小 MoveAgent：加载 movemap，并在第0层 rmap 上执行 A* 路径搜索。
    /// </summary>
    public class CMoveAgent
    {
        // Debug switch (enable temporarily while validating routes).
        // 调试开关（验证寻路时可临时开启）。
        // NOTE: must not be const, otherwise Unity compiler warns about unreachable code.
        // 注意：不要用 const，否则 Unity 编译会报“不可达代码”警告。
        private static bool DEBUG_AUTOPF = false;
        public abstract class BrushTest
        {
            // from.y/to.y store DH in original engine.
            // 原版中 from.y/to.y 存储 DH（相对地形高度差）。
            public abstract bool Collide(Vector3 from, Vector3 to);
        }

        private readonly CMoveMap m_pMoveMap = new CMoveMap();
        private Vector3 m_vOriginOverride;

        private Vector2Int m_ptStart;
        private Vector2Int m_ptGoal;
        private int m_iLayerStart;
        private int m_iLayerGoal;

        private readonly List<Vector3> m_path3D = new List<Vector3>(1024);

        public bool Load(string basePathNoExt, Func<string, byte[]> resolver, Vector3? originOverride)
        {
            // basePathNoExt corresponds to "maps\\<map>\\movemap\\r.._..-c.._..-l0" in C++.
            // basePathNoExt 对应 C++ 的 "maps\\<map>\\movemap\\r.._..-c.._..-l0"。
            string cfgName = basePathNoExt;
            if (!cfgName.EndsWith(".cfg", StringComparison.OrdinalIgnoreCase))
            {
                cfgName += ".cfg";
            }

            byte[] cfgBytes = resolver(cfgName);
            if (cfgBytes == null)
            {
                return false;
            }

            // Resolver for referenced files: in cfg, references are baseName + ".prmap" etc.
            // cfg 内引用：baseName + ".prmap" 等。
            bool ok = m_pMoveMap.Load(cfgBytes, resolver, basePathNoExt);
            if (!ok)
            {
                return false;
            }

            if (originOverride.HasValue)
            {
                m_vOriginOverride = originOverride.Value;
                m_pMoveMap.SetOrigin(m_vOriginOverride);
            }

            return true;
        }

        public bool IsReady()
        {
            // Original C++ requires MultiCluGraph; here we only require layer0 RMap.
            // 原版 C++ 依赖 MultiCluGraph；这里最小实现只要求第0层 RMap。
            var layer0 = m_pMoveMap.GetLayer(0);
            return layer0 != null && layer0.GetRMap() != null;
        }

        public bool IsContain(Vector3 vWld)
        {
            var p = TransWld2Map(vWld);
            int w = m_pMoveMap.GetMapWidth();
            int h = m_pMoveMap.GetMapLength();
            return p.x >= 0 && p.x < w && p.y >= 0 && p.y < h;
        }

        public Vector2Int TransWld2Map(Vector3 vWld)
        {
            return m_pMoveMap.TransWld2Map(vWld.x, vWld.z);
        }

        public Vector3 Map2Wld(Vector2Int ptMap)
        {
            Vector2 xz = m_pMoveMap.TransMap2Wld(ptMap.x, ptMap.y);
            return new Vector3(xz.x, 0.0f, xz.y);
        }

        public int WhichLayer(Vector3 vWld, float dH, out float layerDist)
        {
            // Minimal: single layer 0 if passable.
            // 最小实现：如果可通行则使用单层0。
            layerDist = 0.0f;
            var pt = TransWld2Map(vWld);
            var layer0 = m_pMoveMap.GetLayer(0);
            if (layer0 == null || layer0.GetRMap() == null) return -1;
            return layer0.GetRMap().GetPixel(pt.x, pt.y) ? 0 : -1;
        }

        public bool SetStartEnd(Vector3 vStart, int iLayerStart, Vector3 vGoal, int iLayerGoal, BrushTest brushTest = null)
        {
            m_ptStart = TransWld2Map(vStart);
            m_ptGoal = TransWld2Map(vGoal);
            m_iLayerStart = iLayerStart;
            m_iLayerGoal = iLayerGoal;
            return true;
        }

        public int GetPathCount() => m_path3D.Count;

        /// <summary>
        /// Matches C++ CMoveAgent::GetOptimizeCatchCount — lookahead window for path following.
        /// 对应 C++：无 PathOptimizer 时为 0，FindNearest/Farthest 仅在单步索引上工作。
        /// </summary>
        public int GetOptimizeCatchCount() => 0;

        public Vector3 Get3DPathNode(int index)
        {
            if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
            return m_path3D[index];
        }

        public System.Collections.Generic.List<Vector3> GetFullPath()
        {
            return new System.Collections.Generic.List<Vector3>(m_path3D);
        }

        public bool Search(int nMaxExpand = -1)
        {
            // A* on layer0 rmap
            // 在第0层 rmap 上执行 A*
            m_path3D.Clear();

            var layer0 = m_pMoveMap.GetLayer(0);
            var rmap = layer0?.GetRMap();
            if (rmap == null) return false;

            rmap.GetImageSize(out int w, out int h);
            if (w <= 0 || h <= 0) return false;

            if (!InBounds(m_ptStart, w, h) || !InBounds(m_ptGoal, w, h))
            {
                return false;
            }

            // If goal or start is not passable, try to find nearest passable (original MoveAgent does this).
            // 如果起点或终点不可通行，尝试寻找最近可通行点（原版 MoveAgent 会这样做）。
            if (!rmap.GetPixel(m_ptStart.x, m_ptStart.y))
            {
                if (!TryFindNearestPassable(rmap, m_ptStart, w, h, 64, out var newStart))
                {
                    if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] start not passable and no nearest passable found. start={m_ptStart}");
                    return false;
                }
                if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust start {m_ptStart} -> {newStart}");
                m_ptStart = newStart;
            }
            if (!rmap.GetPixel(m_ptGoal.x, m_ptGoal.y))
            {
                if (!TryFindNearestPassable(rmap, m_ptGoal, w, h, 64, out var newGoal))
                {
                    if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] goal not passable and no nearest passable found. goal={m_ptGoal}");
                    return false;
                }
                if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust goal {m_ptGoal} -> {newGoal}");
                m_ptGoal = newGoal;
            }

            var cameFrom = new Dictionary<Vector2Int, Vector2Int>(4096);
            var gScore = new Dictionary<Vector2Int, int>(4096);
            var open = new MinHeap();

            gScore[m_ptStart] = 0;
            open.Push(m_ptStart, Heuristic(m_ptStart, m_ptGoal));

            int expands = 0;
            //ToDo: need use another method to caculate the value of maxExpand
            //800000 is a magic number, need to be optimized
            int maxExpand = nMaxExpand > 0 ? nMaxExpand : 800000;

            while (open.Count > 0 && expands < maxExpand)
            {
                var cur = open.Pop();
                expands++;

                if (cur == m_ptGoal)
                {
                    ReconstructPath(cameFrom, cur);
                    return true;
                }

                int curG = gScore[cur];
                foreach (var nb in Neighbors8(cur))
                {
                    if (!InBounds(nb, w, h)) continue;
                    if (!rmap.GetPixel(nb.x, nb.y)) continue;

                    int tentative = curG + Cost(cur, nb);
                    if (!gScore.TryGetValue(nb, out int old) || tentative < old)
                    {
                        cameFrom[nb] = cur;
                        gScore[nb] = tentative;
                        int f = tentative + Heuristic(nb, m_ptGoal);
                        open.Push(nb, f);
                    }
                }
            }

            return false;
        }

        private static bool TryFindNearestPassable(CBitImage rmap, Vector2Int origin, int w, int h, int maxRadius, out Vector2Int best)
        {
            best = origin;
            int bestDist2 = int.MaxValue;
            int bestX = origin.x;
            int bestY = origin.y;

            if (rmap == null) return false;

            // Check origin first
            // 先检查原点
            if (origin.x >= 0 && origin.x < w && origin.y >= 0 && origin.y < h && rmap.GetPixel(origin.x, origin.y))
            {
                best = origin;
                return true;
            }

            // Expand square rings
            // 按方形“圈”扩展搜索
            for (int r = 1; r <= maxRadius; r++)
            {
                int minX = Math.Max(0, origin.x - r);
                int maxX = Math.Min(w - 1, origin.x + r);
                int minY = Math.Max(0, origin.y - r);
                int maxY = Math.Min(h - 1, origin.y + r);

                // Top/bottom edges
                for (int x = minX; x <= maxX; x++)
                {
                    TryConsider(x, minY);
                    TryConsider(x, maxY);
                }

                // Left/right edges (excluding corners already checked)
                for (int y = minY + 1; y <= maxY - 1; y++)
                {
                    TryConsider(minX, y);
                    TryConsider(maxX, y);
                }

                if (bestDist2 != int.MaxValue)
                {
                    best = new Vector2Int(bestX, bestY);
                    return true;
                }
            }

            return false;

            void TryConsider(int x, int y)
            {
                if (!rmap.GetPixel(x, y)) return;
                int dx = x - origin.x;
                int dy = y - origin.y;
                int d2 = dx * dx + dy * dy;
                if (d2 < bestDist2)
                {
                    bestDist2 = d2;
                    bestX = x;
                    bestY = y;
                }
            }
        }

        private void ReconstructPath(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int cur)
        {
            List<Vector2Int> rev = new List<Vector2Int>(1024) { cur };
            while (cameFrom.TryGetValue(cur, out var prev))
            {
                cur = prev;
                rev.Add(cur);
            }

            rev.Reverse();
            // Convert to world positions (y will be resolved by host terrain in movement).
            // 转为世界坐标（y 由移动逻辑/地形解析）。
            for (int i = 0; i < rev.Count; i++)
            {
                m_path3D.Add(Map2Wld(rev[i]));
            }
        }

        private static bool InBounds(Vector2Int p, int w, int h) => p.x >= 0 && p.x < w && p.y >= 0 && p.y < h;

        private static int Heuristic(Vector2Int a, Vector2Int b)
        {
            // Octile distance * 10
            // 八方向启发式（octile）*10
            int dx = Mathf.Abs(a.x - b.x);
            int dy = Mathf.Abs(a.y - b.y);
            int min = Math.Min(dx, dy);
            int max = Math.Max(dx, dy);
            return 14 * min + 10 * (max - min);
        }

        private static int Cost(Vector2Int a, Vector2Int b)
        {
            // Diagonal=14, straight=10
            // 斜向=14，直向=10
            int dx = Mathf.Abs(a.x - b.x);
            int dy = Mathf.Abs(a.y - b.y);
            return (dx + dy == 2) ? 14 : 10;
        }

        private static IEnumerable<Vector2Int> Neighbors8(Vector2Int p)
        {
            yield return new Vector2Int(p.x + 1, p.y);
            yield return new Vector2Int(p.x - 1, p.y);
            yield return new Vector2Int(p.x, p.y + 1);
            yield return new Vector2Int(p.x, p.y - 1);
            yield return new Vector2Int(p.x + 1, p.y + 1);
            yield return new Vector2Int(p.x + 1, p.y - 1);
            yield return new Vector2Int(p.x - 1, p.y + 1);
            yield return new Vector2Int(p.x - 1, p.y - 1);
        }

        public void ResetSearch()
        {
            //	Çå³ýµ±Ç°ËÑË÷״̬
            //if (m_iStat == PF_STATE_UNKNOWN)
            //{
            //    return;
            //}
            //if (m_pPfAlg)
            //{
            //    m_pPfAlg->Reset();
            //}
            //if (m_pPathOptimizer)
            //{
            //    m_pPathOptimizer.Reset();
            //}
            m_ptStart.x = m_ptStart.y = 0;
            m_iLayerStart = -1;
            m_ptGoal.x = m_ptGoal.y = 0;
            m_iLayerGoal = -1;
            //m_pBrushTest = null;

            //m_iStat = PF_STATE_UNKNOWN;
            m_path3D.Clear();
        }

        /// <summary>
        /// Very small min-heap for A*.
        /// A* 用的小型最小堆。
        /// </summary>
        private sealed class MinHeap
        {
            private struct Node
            {
                public Vector2Int P;
                public int F;
            }

            private readonly List<Node> _a = new List<Node>(4096);
            public int Count => _a.Count;

            public void Push(Vector2Int p, int f)
            {
                _a.Add(new Node { P = p, F = f });
                SiftUp(_a.Count - 1);
            }

            public Vector2Int Pop()
            {
                var root = _a[0].P;
                int last = _a.Count - 1;
                _a[0] = _a[last];
                _a.RemoveAt(last);
                if (_a.Count > 0) SiftDown(0);
                return root;
            }

            private void SiftUp(int i)
            {
                while (i > 0)
                {
                    int p = (i - 1) >> 1;
                    if (_a[p].F <= _a[i].F) break;
                    (_a[p], _a[i]) = (_a[i], _a[p]);
                    i = p;
                }
            }

            private void SiftDown(int i)
            {
                int n = _a.Count;
                while (true)
                {
                    int l = (i << 1) + 1;
                    if (l >= n) break;
                    int r = l + 1;
                    int m = (r < n && _a[r].F < _a[l].F) ? r : l;
                    if (_a[i].F <= _a[m].F) break;
                    (_a[i], _a[m]) = (_a[m], _a[i]);
                    i = m;
                }
            }
        }
    }
}