Untitled

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using UnityEngine;
using static RuleOfCoolStudios.Utils.Unity.Logging;
using BlueRaja;
using Unity.VisualScripting; // Fast Priority Queue

namespace RuleOfCoolStudios.MechMice1
{
    public enum HexDirection
    {
        UpRight,
        UpLeft,
        DownLeft,
        DownRight,
        Right,
        Left,
    }

    /// <summary>
    /// Simple reference type for a grid point for internal use. Includes translations for hex based
    /// grid points based on: https://www.redblobgames.com/grids/hexagons/
    /// Unity's convention for tile mapping: odd-R; Cube/Axial Coordinates are provided (Q, R, S).
    /// In order to use the helper methods to find the world position, the "size" must be specified (radius).
    /// </summary>
    public struct HexPoint : IEquatable<HexPoint>
    {
        public readonly int X { get; }

        public readonly int Y { get; }

        //private static List<HexPoint> _
        private static PathNodeStructure _open = new(331);
        private static PathNodeStructure _closed = new(331);

        public const float Size = 2.05f;

        /// <summary>
        /// If searching for paths or radius greater than this number, the methods will return default values.
        /// </summary>
        public const int MaxSearchRange = 50;

        // Fixed locations
        public static HexPoint Origin => new(0, 0);
        public static HexPoint None => new(-999999, -999999);

        // Hex adjacency using hex coordinate constructor
        public readonly HexPoint UpRightHex => new(Q + 1, R - 1, S);
        public readonly HexPoint UpLeftHex => new(Q, R - 1, S + 1);
        public readonly HexPoint DownLeftHex => new(Q - 1, R + 1, S);
        public readonly HexPoint DownRightHex => new(Q, R + 1, S - 1);
        public readonly HexPoint RightHex => new(Q + 1, R, S - 1);
        public readonly HexPoint LeftHex => new(Q - 1, R, S + 1);

        // Cartesian adjacency
        public readonly HexPoint Up => new(X, Y + 1);
        public readonly HexPoint Down => new(X, Y - 1);
        public readonly HexPoint Right => new(X + 1, Y);
        public readonly HexPoint Left => new(X - 1, Y);

        // Cube/Axial coordinates
        public readonly int Q => X - ((Y - (Y & 1)) / 2); // Positive to the right or upright

        public readonly int R => Y; // Positive to the downright or downleft

        public readonly int S => 0 - Q - R; // Positive to the left or upleft

        /// <summary>
        /// Construct a point with hex (cube/axial) coordinates.
        /// S must be included as two integers matches the signature of offset constructor: (x, y).
        /// </summary>
        public HexPoint(int q, int r, int s)
        {
            if (s + r + q != 0) throw new ArgumentException($"Invalid S provided to HexPoint constructor. (q:{q}, r:{r}, s:{s}).");
            X = q + ((r - (r & 1)) / 2); // Bitwise conversion to X using unity Odd-R convention
            Y = r;
        }

        public HexPoint(HexPoint hp, HexDirection direction)
        {
            HexPoint tmp = direction switch
            {
                HexDirection.UpRight => hp.UpRightHex,
                HexDirection.UpLeft => hp.UpLeftHex,
                HexDirection.DownLeft => hp.DownLeftHex,
                HexDirection.DownRight => hp.DownRightHex,
                HexDirection.Right => hp.RightHex,
                HexDirection.Left => hp.LeftHex,
                _ => throw new NotImplementedException(),
            };
            X = tmp.X;
            Y = tmp.Y;
        }

        public HexPoint(int x, int y) { X = x; Y = y; }

        public HexPoint(HexPoint p) { X = p.X; Y = p.Y; }

        public readonly Vector3 WorldPosition => GetWorldPosition();

        private const float Sqrt3 = 1.73205080757f;

        public readonly Vector3 GetWorldPosition()
        {
            float x = Size * Sqrt3 * (X + (0.5f * (Y & 1)));
            float y = Size * 3f / 2f * Y;
#pragma warning disable S2234 // Parameters should be passed in the correct order
            return new(y, 0, x); // X,Y to unity Z,X
#pragma warning restore S2234 // Parameters should be passed in the correct order
        }

        public readonly bool Equals(HexPoint p) => X == p.X && Y == p.Y;

        public override readonly bool Equals(object obj) => obj is HexPoint other && Equals(other);

        public static bool operator ==(HexPoint lhs, HexPoint rhs) => lhs.Equals(rhs);

        public static bool operator !=(HexPoint lhs, HexPoint rhs) => !(lhs == rhs);

        public override readonly int GetHashCode() => (X, Y).GetHashCode();

        /// <summary>
        /// Get hexes within range (0 to MaxSearchRange). Range 0 returns 1 tile, Range 1 returns 7 tiles, Range 2
        /// returns 19 tiles, etc.
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly bool TryGetHexesInRange(int range, out List<HexPoint> resultList)
        {
            int capacity = range switch
            {
                0 => 1,
                1 => 7,
                2 => 19,
                3 => 37,
                4 => 61,
                5 => 91,
                6 => 127,
                7 => 169,
                8 => 217,
                9 => 271,
                10 => 331,
                _ => 0, // error cases return an empty list, so set the capacity to 0
            };

            resultList = new(capacity);
            if (range < 0) return false;
            if (range > MaxSearchRange) return false;
            if (this == None) return false;
            int negativeRange = range * -1;
            for (int q = negativeRange; q <= range; q++)
            {
                int min = Math.Max(negativeRange, 0 - q - range);
                int max = Math.Min(range, 0 - q + range);
                for (int r = min; r <= max; r++)
                {
                    HexPoint newPoint = new(Q + q, R + r, S - q - r);
                    if (resultList.Capacity == resultList.Count) { e($"resultList capacity ({resultList.Capacity}) reached."); }

                    resultList.Add(newPoint);
                }
            }
            return true;
        }

        /// <summary>
        /// Integer distance by "hexwalking" to another HexPoint (ie, UpLeft, UpRight, UpLeft, UpRight).
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly int DistanceTo(HexPoint dest)
        {
            int q = Math.Abs(Q - dest.Q);
            int r = Math.Abs(R - dest.R);
            int s = Math.Abs(S - dest.S);
            return (q + r + s) / 2;
        }

        private class PathNode : FastPriorityQueueNode
        {
            public HexPoint Parent;
            public float F;
            public int H;
            public int G;
            public HexPoint Location;
            public override string ToString()
            {
                return $"{Location}, FGH:({F},{G},{H}), Parent:{Parent}";
            }
        }

        private class PathNodeStructure
        {
            public PathNodeStructure(int maxSize)
            {
                HPPriorityQueue = new(maxSize);
            }

            public FastPriorityQueue<PathNode> HPPriorityQueue;

            public Dictionary<HexPoint, PathNode> HPDictionary = new(22);

            public PathNode this[HexPoint hp] => HPDictionary[hp];

            public bool Contains(HexPoint hexPoint) => HPDictionary.ContainsKey(hexPoint);

            public void Clear()
            {
                HPPriorityQueue.Clear();
                HPDictionary.Clear();
            }

            public PathNode Dequeue()
            {
                PathNode ret = HPPriorityQueue.Dequeue();
                HPPriorityQueue.ResetNode(ret);
                HPDictionary.Remove(ret.Location);
                return ret;
            }

            public void Enqueue(PathNode node, float priority)
            {
                //if (node.Location == node.Parent)
                //{
                //    e($"Tried to enqueue a node ({node.Location}) with itself as the parent. This is a bug.");
                //    return;
                //}
                HPPriorityQueue.Enqueue(node, priority);
                HPDictionary.Add(node.Location, node);
            }

            public void Remove(PathNode node)
            {
                HPDictionary.Remove(node.Location);
                HPPriorityQueue.Remove(node);
                HPPriorityQueue.ResetNode(node);
            }
        }

        /// <summary>
        /// Return a set of hexpoints with the integer path cost from the origin to that location.
        /// </summary>
        public static Dictionary<HexPoint, int> GetMovementRange(HexPoint origin, Dictionary<HexPoint, TerrainType> map, int range, HashSet<HexPoint> omitLocations)
        {
            Dictionary<HexPoint, int> ret = new(22);
            List<TerrainType> pathTerrain = new(22);
            if (!origin.TryGetHexesInRange(range, out List<HexPoint> hexesInRange)) return ret;
            foreach (HexPoint hp in hexesInRange)
            {
                if (!TryGetPath(origin, hp, map, out List<HexPoint> path, omitLocations))
                {
                    continue; // No path.
                }
                pathTerrain.Clear();
                foreach (HexPoint p in path)
                {
                    if (pathTerrain.Capacity == pathTerrain.Count) { e($"pathTerrain capacity ({pathTerrain.Capacity}) reached."); }
                    pathTerrain.Add(map[p]);
                }
                int pathCost = pathTerrain.GetPathCost();
                if (pathCost <= range)
                {
                    ret.Add(hp, pathCost);
                }
            }
            return ret;
        }

        /// <summary>
        /// Gets a path from one point to another on a map. Returns false if no path is found.
        /// Path does not include the from point. OmitDestinations is a set of points that are not allowed to be the destination.
        /// </summary>
        public static bool TryGetPath(HexPoint from, HexPoint to, Dictionary<HexPoint, TerrainType> map, out List<HexPoint> path, HashSet<HexPoint> omitDestinations)
        {
            HashSet<HexPoint> _omitDestinations = omitDestinations ?? new(21); // 15 bugs, 5 mice, and a mech
            if (map == null)
            {
                path = null;
                return false;
            }

            if (from == None)
            {
                path = null;
                return false;
            }

            if (to == None)
            {
                path = null;
                return false;
            }

            if (!map.Keys.Contains(from))
            {
                path = null;
                return false;
            }

            if (!map.Keys.Contains(to))
            {
                path = null;
                return false;
            }

            if (from == to)
            {
                path = new(1) { from };
                return true;
            }

            if (map[to].GetMoveCost() == TerrainBehaviour.Infinite)
            {
                path = null;
                return false;
            }

            if (_omitDestinations.Contains(to))
            {
                path = null;
                return false;
            }

            int mapCount = map.Count;
            if (mapCount == 0)
            {
                path = null;
                return false;
            }

            //PathNodeStructure open = new(mapCount);
            //PathNodeStructure closed = new(mapCount);
            _open.Clear();
            _closed.Clear();

            int G = 0; // Cost so far
            int H = from.DistanceTo(to); // Heuristic
            float F = G + H; // Estimate of remaining cost
            PathNode node = new()
            {
                Parent = None,
                F = F,
                G = G,
                H = H,
                Location = from,
            };

            if (node.Parent == node.Location)
            {
                e($"1: Tried to add a self-referential node. Parent:{node.Parent} Location:{node.Location} F:{F} G:{G} H:{H} from:{from} to:{to}.");
                path = null;
                return false;
            }

            _open.Enqueue(node, F);
            bool isDone = false;

            // Find a path
            while (!isDone)
            {
                PathNode current;
                try
                {
                    current = _open.Dequeue();
                }
                catch (InvalidOperationException)
                {
                    // This happens when there's no path to the intended destination, even if there's a manhattan distance (ie, units blocking the way).
                    // Skip this path.
                    path = null;
                    return false;
                }

                if (current.Parent == current.Location)
                {
                    e($"2: Tried to add a self-referential node. Parent:{current.Parent} Location:{current.Location} F:{current.F} G:{current.G} H:{current.H} from:{from} to:{to}.");
                    path = null;
                    return false;
                }
                _closed.Enqueue(current, current.F);
                foreach (HexPoint hp in current.Location.GetAdjacentHexes())
                {
                    if (!map.Keys.Contains(hp)) continue;
                    int moveCost = map[hp].GetMoveCost();

                    // Don't walk through other spots
                    if (_omitDestinations.Contains(hp)) continue;

                    if (moveCost == TerrainBehaviour.Infinite) continue;
                    G = current.G + moveCost;
                    if (hp == to)
                    {
                        isDone = true;
                        PathNode neighbor = new()
                        {
                            Location = hp,
                            Parent = current.Location,
                            H = 0,
                            G = G,
                            F = G + H, // aka G - we're there.
                        };
                        if (neighbor.Parent == neighbor.Location)
                        {
                            e($"3: Tried to add a self-referential node. Parent:{neighbor.Parent} Location:{neighbor.Location} F:{neighbor.F} G:{neighbor.G} H:{neighbor.H} from:{from} to:{to}.");
                            path = null;
                            return false;
                        }
                        _closed.Enqueue(neighbor, neighbor.F); // should be highest priority in list.
                        break;
                    }
                    if (_open.Contains(hp))
                    {
                        PathNode neighbor = _open[hp];
                        if (G < neighbor.G)
                        {
                            _open.Remove(neighbor);
                        }
                    }
                    if (_closed.Contains(hp))
                    {
                        PathNode neighbor = _closed[hp];
                        if (G < neighbor.G)
                        {
                            _closed.Remove(neighbor);
                        }
                    }
                    if (!_open.Contains(hp) && !_closed.Contains(hp))
                    {
                        H = hp.DistanceTo(to);
                        F = G + H;
                        PathNode neighbor = new()
                        {
                            Location = hp,
                            Parent = current.Location,
                            G = G,
                            H = H,
                            F = F,
                        };
                        if (neighbor.Parent == neighbor.Location)
                        {
                            e($"4: Tried to add a self-referential node. Parent:{neighbor.Parent} Location:{neighbor.Location} F:{neighbor.F} G:{neighbor.G} H:{neighbor.H} from:{from} to:{to}.");
                            path = null;
                            return false;
                        }
                        _open.Enqueue(neighbor, F);
                    }
                }
            }

            // Backtrack, build path, return it.
            path = new(30);
            PathNode backtrackCurrent = _closed[to]; // Shouldn't fail.


            while (backtrackCurrent.Parent != None)
            {
                if (path.Capacity == path.Count)
                {
                    e($"Path capacity ({path.Capacity}) reached. From:{from} To:{to}");
                    e($"Path:[{GetPathString(path)}]");
                    return false;
                }
                if (path.Count > 50)
                {
                    e($"Path broke. From:{from} To:{to} Path:[{GetPathString(path)}]");
                    return false;
                }
                path.Add(backtrackCurrent.Location);
                backtrackCurrent = _closed[backtrackCurrent.Parent];
            }
            path.Reverse();
            return true;
        }

        private static string GetPathString(List<HexPoint> path)
        {
            StringBuilder sb = new();
            bool isFirst = true;
            foreach (HexPoint p in path)
            {
                if (!isFirst) sb.Append(", ");
                sb.Append(p);
                isFirst = false;
            }
            return sb.ToString();
        }

        /// <summary>
        /// (X,Y)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly override string ToString() => ToString(true);

        /// <summary>
        /// (X,Y) for cartesian coordinates, (Q,R,S) for hex coordinates. Note that Unity will still
        /// use cartesian coordinates for hex based Tile Grids (using "odd-R" convention - odd rows are
        /// shoved left by one half unit).
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly string ToString(bool isCartesian)
        {
            StringBuilder sb = new(7);
            sb.Append('(');
            if (isCartesian)
            {
                sb.Append(X);
                sb.Append(",");
                sb.Append(Y);
            }
            else
            {
                sb.Append(Q);
                sb.Append(",");
                sb.Append(R);
                sb.Append(",");
                sb.Append(S);
            }
            sb.Append(')');
            return sb.ToString();
        }
    }

    public static class HexPointExtensions
    {
        /// <summary>
        /// Get the 6 adjacent hexes to this hex.
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static List<HexPoint> GetAdjacentHexes(this HexPoint hp)
        {
            return new List<HexPoint>()
            {
                new(hp.Q, hp.R - 1, hp.S + 1),
                new(hp.Q + 1, hp.R - 1, hp.S),
                new(hp.Q + 1, hp.R, hp.S - 1),
                new(hp.Q, hp.R + 1, hp.S - 1),
                new(hp.Q - 1, hp.R + 1, hp.S),
                new(hp.Q - 1, hp.R, hp.S + 1),
            };
        }

        /// <summary>
        /// Path cost for a list of terrain types.
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int GetPathCost(this List<TerrainType> path)
        {
            int cost = 0;
            int tileCost = 0;
            int pathLength = path.Count;
            for (int i = 0; i < pathLength; i++)
            {
                tileCost = path[i].GetMoveCost();
                if (tileCost == TerrainBehaviour.Infinite)
                {
                    return TerrainBehaviour.Infinite;
                }
                cost += tileCost;
            }
            return cost;
        }

        /// <summary>
        /// Path cost with a from and to and map. Returns TerrainBehaviour.Infinite if no path is found. NOTE: Make sure to
        /// properly do TO and FROM (since the omitlocations often contains the "from" location).
        /// </summary>
        public static int GetPathCost(this HexPoint from, HexPoint to, Dictionary<HexPoint, TerrainType> map, HashSet<HexPoint> omitLocations)
        {
            bool hasPath = HexPoint.TryGetPath(from, to, map, out List<HexPoint> path, omitLocations);
            if (!hasPath)
            {
                return TerrainBehaviour.Infinite;
            }
            List<TerrainType> pathTerrain = new(path.Count);
            foreach (HexPoint p in path)
            {
                pathTerrain.Add(map[p]);
            }
            return pathTerrain.GetPathCost();
        }

        /// <summary>
        /// Gets the farthest point along a path from a starting point to the destination with a given movement budget. Will
        /// return a point that is 1 away from the target if we could reach it. Returns HexPoint.None if there is no path. Could return
        /// "from" if movementBudget is 0 or there are no hexes in the path the unit can move to (ie, surrounded by 2 movement cost
        /// squares with 1 movement budget). Won't land on omitLocations.
        /// </summary>
        public static HexPoint GetClosestPointTo(this HexPoint from, HexPoint to, Dictionary<HexPoint, TerrainType> map, int movementBudget, HashSet<HexPoint> omitLocations)
        {
            HashSet<HexPoint> omitLocationsWithoutTarget = new(omitLocations ?? new(22));
            omitLocationsWithoutTarget.Remove(to);
            bool hasPath = HexPoint.TryGetPath(from, to, map, out List<HexPoint> path, omitLocationsWithoutTarget);
            if (!hasPath)
            {
                return HexPoint.None;
            }

            HexPoint target = from;
            int currentBudget = movementBudget;
            //v($"Move budget is {currentBudget}");
            foreach (HexPoint p in path)
            {
                if (p == to)
                {
                    return target; // don't advance onto the target.
                }
                int cost = map[p].GetMoveCost();
                //v($"Subtracting {cost}. New budget is {currentBudget}. Current target is {target}.");
                currentBudget -= cost;
                if (currentBudget < 0)
                {
                    return target;
                }
                target = p;
            }
            return target; // made it all the way
        }

        /// <summary>
        /// True if hexes are adjacent.
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool IsAdjacentTo(this HexPoint hp, HexPoint other)
        {
            return
                (hp.Q == other.Q && hp.R == other.R - 1 && hp.S == other.S + 1) ||
                (hp.Q == other.Q + 1 && hp.R == other.R - 1 && hp.S == other.S) ||
                (hp.Q == other.Q + 1 && hp.R == other.R && hp.S == other.S - 1) ||
                (hp.Q == other.Q && hp.R == other.R + 1 && hp.S == other.S - 1) ||
                (hp.Q == other.Q -1 && hp.R == other.R + 1 && hp.S == other.S) ||
                (hp.Q == other.Q -1 && hp.R == other.R && hp.S == other.S + 1);
        }
    }
}