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

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

namespace AutoMove
{
    /// <summary>
    /// MoveAgent: A* on layer0 rmap + COptimizePath (matches C++ CMoveAgent flow).
    /// </summary>
    public class CMoveAgent
    {
        private static bool DEBUG_AUTOPF = false;

        public abstract class BrushTest
        {
            public abstract bool Collide(Vector3 from, Vector3 to);
        }

        private readonly CMoveMap m_pMoveMap = new CMoveMap();
        private COptimizePath m_pPathOptimizer;

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

        public bool Load(string basePathNoExt, Func<string, byte[]> resolver, Vector3? originOverride)
        {
            string cfgName = basePathNoExt;
            if (!cfgName.EndsWith(".cfg", StringComparison.OrdinalIgnoreCase))
                cfgName += ".cfg";

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

            bool ok = m_pMoveMap.Load(cfgBytes, resolver, basePathNoExt);
            if (!ok)
                return false;

            if (originOverride.HasValue)
                m_pMoveMap.SetOrigin(originOverride.Value);

            CreateOptimizer();
            return true;
        }

        void CreateOptimizer()
        {
            m_pPathOptimizer = new COptimizePath();
        }

        public bool IsReady()
        {
            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)
        {
            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 List<PathNode> Get2DPath()
        {
            return m_pPathOptimizer?.GetPath();
        }

        public int GetPathCount()
        {
            if (m_pPathOptimizer != null)
            {
                var path = m_pPathOptimizer.GetPath();
                if (path != null)
                    return path.Count;
            }
            return 0;
        }

        public int GetOptimizeCatchCount()
        {
            return m_pPathOptimizer != null ? m_pPathOptimizer.GetCatchCount() : 0;
        }

        public Vector3 Get2DPathNode(int index)
        {
            var path2d = Get2DPath();
            if (path2d == null || index < 0 || index >= path2d.Count)
                return Vector3.zero;
            PathNode pathNode = path2d[index];
            Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
            return new Vector3(ptWld.x, 0f, ptWld.y);
        }

        public Vector3 Get3DPathNode(int index)
        {
            var path2d = Get2DPath();
            if (path2d == null || index < 0 || index >= path2d.Count)
                return Vector3.zero;
            PathNode pathNode = path2d[index];
            Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
            float dh = m_pMoveMap.GetDH(pathNode.layer, (int)pathNode.ptMap.x, (int)pathNode.ptMap.y);
            return new Vector3(ptWld.x, dh, ptWld.y);
        }

        /// <summary>Ported from C++ CMoveAgent::Optimize.</summary>
        public bool Optimize(int moveIndex)
        {
            if (m_pPathOptimizer == null)
                return false;
            if (!m_pPathOptimizer.NeedOptimize(moveIndex))
                return false;
            m_pPathOptimizer.StepOptimize();
            return true;
        }

        public List<Vector3> GetFullPath()
        {
            var path = new List<Vector3>();
            var path2d = Get2DPath();
            if (path2d == null)
                return path;
            for (int i = 0; i < path2d.Count; i++)
                path.Add(Get2DPathNode(i));
            return path;
        }

        public bool Search(int nMaxExpand = -1)
        {
            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 (!rmap.GetPixel(m_ptStart.x, m_ptStart.y))
            {
                if (!TryFindNearestPassable(rmap, m_ptStart, w, h, 64, out var newStart))
                    return false;
                m_ptStart = newStart;
            }
            if (!rmap.GetPixel(m_ptGoal.x, m_ptGoal.y))
            {
                if (!TryFindNearestPassable(rmap, m_ptGoal, w, h, 64, out var newGoal))
                    return false;
                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;
            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;

            if (origin.x >= 0 && origin.x < w && origin.y >= 0 && origin.y < h && rmap.GetPixel(origin.x, origin.y))
            {
                best = origin;
                return true;
            }

            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);

                for (int x = minX; x <= maxX; x++)
                {
                    TryConsider(x, minY);
                    TryConsider(x, maxY);
                }
                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)
        {
            var rev = new List<Vector2Int>(1024) { cur };
            while (cameFrom.TryGetValue(cur, out var prev))
            {
                cur = prev;
                rev.Add(cur);
            }
            rev.Reverse();

            var initPath = new List<PathNode>(rev.Count);
            for (int i = 0; i < rev.Count; i++)
            {
                initPath.Add(new PathNode
                {
                    ptMap = new Vector2(rev[i].x, rev[i].y),
                    layer = m_iLayerStart,
                });
            }

            if (m_pPathOptimizer == null)
                CreateOptimizer();

            // C++: m_pPathOptimizer->SetupOptimize(GetMoveMap(), path);
            m_pPathOptimizer.SetupOptimize(m_pMoveMap, initPath);
        }

        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)
        {
            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)
        {
            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()
        {
            m_ptStart = Vector2Int.zero;
            m_iLayerStart = -1;
            m_ptGoal = Vector2Int.zero;
            m_iLayerGoal = -1;
            m_pPathOptimizer?.Reset();
        }

        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;
                }
            }
        }
    }
}