public struct PathFindJob : IJob
{
    private HexUnit _origin;
    private HexUnit _destination;
    private Stack<HexUnit> _path;
    private HashSet<PathUnit> _explored;
    private SortedSet<PathUnit> _toExplore;
    public PathFindJob(HexUnit origin, HexUnit destination)
    {
        _origin = origin;
        _destination = destination;
        _path = new Stack<HexUnit>();
        _explored = new HashSet<PathUnit>();
        _toExplore = new SortedSet<PathUnit> { new(origin) };
    }
    public void Execute()
    {
        while (_toExplore.Count != 0)
        {
            PathUnit cur = _toExplore.Min;
            if (cur.hexUnit.coordinate == _destination.coordinate)
            {
                while (cur!.hexUnit.coordinate != _origin.coordinate)
                {
                    _path.Push(cur.hexUnit);
                    cur = cur.prior;
                }
                return;
            }
            _toExplore.Remove(cur);
            _explored.Add(cur);
            foreach (PathUnit u in cur.GetNeighbours())
            {
                if (u == null) continue;
                if (!u.hexUnit.walkable || _explored.Contains(u)) continue;
                bool alreadyScheduled = _toExplore.Contains(u);
                if (!alreadyScheduled || cur.G + 1 < u.G)
                {
                    u.G = cur.G + 1;
                    u.H = HexUnit.GetDistance(u.hexUnit.coordinate, _destination.coordinate);
                    u.prior = cur;
                    if (!alreadyScheduled)
                        _toExplore.Add(u);
                }
            }
        }
    }
    public Stack<HexUnit> GetPath() => _path;
}


public class PathUnit : IComparable<PathUnit>, IEquatable<PathUnit>
{
    public int F => G + H;
    public int G;
    public int H;
    [CanBeNull]
    public PathUnit prior;
    public readonly HexUnit hexUnit;
    private HexUnit _destination;
    public PathUnit(HexUnit hexUnit)
    {
        this.hexUnit = hexUnit;
        G = 0;
        H = 0;
        prior = null;
    }
    public int CompareTo(PathUnit other)
    {
        if (hexUnit.GetHashCode() == other.hexUnit.GetHashCode()) return 0;
        if (F < other.F) return -1;
        if (F == other.F && H < other.H) return -1;
        return 1;
    }
    public bool Equals(PathUnit other) =>
        other != null && other.CompareTo(this) == 0;
    public override int GetHashCode() =>
        hexUnit.GetHashCode();
    public override bool Equals(object obj) =>
        (PathUnit)obj != null && ((PathUnit)obj).CompareTo(this) == 0;
    public List<PathUnit> GetNeighbours()
    {
        HexMap m = EnvironmentManager.Instance.targetMap;
        Vector3Int c = hexUnit.coordinate;
        return new List<PathUnit>
        {
            new(m.GetUnit(c + new Vector3Int(1, 0, -1))),
            new(m.GetUnit(c + new Vector3Int(0, 1, -1))),
            new(m.GetUnit(c + new Vector3Int(-1, 1, 0))),
            new(m.GetUnit(c + new Vector3Int(-1, 0, 1))),
            new(m.GetUnit(c + new Vector3Int(0, -1, 1))),
            new(m.GetUnit(c + new Vector3Int(1, -1, 0)))
        };
    }
}