A Star with sorting by full path C# Unity

AStar.cs

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

// Similar to https://lsreg.ru/realizaciya-algoritma-poiska-a-na-c/

public class AStar
{

    public static List<Vector2Int> FindPath(
        Vector2Int start,
        Vector2Int goal,
        int calculationLimit,
        CanMove canMove
    )
    {
        var closedSet = new PathNodeCollection();
        var openSet = new PathNodeCollection();

        PathNode startNode = new PathNode(
            Position: start,
            CameFrom: null,
            PathLengthFromStart: 0,
            HeuristicEstimatePathLength: GetHeuristicPathLength(start, goal)
        );
        openSet.Add(startNode);
        int currIteration = 0;

        while (openSet.Count() > 0)
        {
            currIteration++;
            if (currIteration >= calculationLimit)
            {
                return null;
            }

            var currentNode = openSet.FindShortestFullPathLength();

            if (currentNode.Position == goal)
            {
                return GetPathForNode(currentNode);
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            foreach (var neighbourNode in Get4Neighbours(currentNode, goal, canMove))
            {
                if (closedSet.HasPosition(neighbourNode.Position))
                {
                    continue;
                }
                var openNode = openSet.FindForPosition(neighbourNode.Position);

                if (openNode == null)
                {
                    openSet.Add(neighbourNode);
                }
                else
                    if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
                {
                    openSet.ModifyCameFrom(
                        openNode,
                        NewCameFrom: currentNode,
                        NewPathLengthFromStart: neighbourNode.PathLengthFromStart
                    );
                }
            }
        }
        return null;
    }

    private static List<PathNode> Get4Neighbours(PathNode pathNode, Vector2Int goal, CanMove canMove)
    {
        var result = new List<PathNode>(4);

        Vector2Int[] neighbourPoints = new Vector2Int[4];
        neighbourPoints[0] = new Vector2Int(pathNode.Position.x + 1, pathNode.Position.y);
        neighbourPoints[1] = new Vector2Int(pathNode.Position.x - 1, pathNode.Position.y);
        neighbourPoints[2] = new Vector2Int(pathNode.Position.x, pathNode.Position.y + 1);
        neighbourPoints[3] = new Vector2Int(pathNode.Position.x, pathNode.Position.y - 1);

        foreach (Vector2Int point in neighbourPoints)
        {
            if (!canMove(point))
            {
                continue;
            }

            var neighbourNode = new PathNode(
                Position: point,
                CameFrom: pathNode,
                PathLengthFromStart: pathNode.PathLengthFromStart + GetDistanceBetweenNeighbours(),
                HeuristicEstimatePathLength: GetHeuristicPathLength(point, goal)
            );
            result.Add(neighbourNode);
        }
        return result;
    }

    private static int GetDistanceBetweenNeighbours()
    {
        return 1;
    }

    private static int GetHeuristicPathLength(Vector2Int from, Vector2Int to)
    {
        return Math.Abs(from.x - to.x) + Math.Abs(from.y - to.y);
    }

    private static List<Vector2Int> GetPathForNode(PathNode pathNode)
    {
        var result = new List<Vector2Int>();
        var currentNode = pathNode;
        while (currentNode != null)
        {
            result.Add(currentNode.Position);
            currentNode = currentNode.CameFrom;
        }
        result.Reverse();
        return result;
    }

    public delegate bool CanMove(Vector2Int cell);
}

class PathNodeCollection
{

    private readonly SortedListWithDuplicates<int, PathNode> NodesByFullPath =
        new SortedListWithDuplicates<int, PathNode>();
    private readonly Dictionary<Vector2Int, PathNode> PositionToNode =
        new Dictionary<Vector2Int, PathNode>();

    public int Count() => NodesByFullPath.Count();

    public void Add(PathNode element)
    {
        NodesByFullPath.Add(element.FullPathLength, element);
        PositionToNode[element.Position] = element;
    }

    public void Remove(PathNode element)
    {
        NodesByFullPath.Remove(element.FullPathLength, element);
        PositionToNode.Remove(element.Position);
    }

    public void ModifyCameFrom(PathNode element, PathNode NewCameFrom, int NewPathLengthFromStart)
    {
        PathNode newElement = new PathNode(
            Position: element.Position,
            CameFrom: NewCameFrom,
            PathLengthFromStart: NewPathLengthFromStart,
            HeuristicEstimatePathLength: element.HeuristicEstimatePathLength
        );
        Remove(element);
        Add(newElement);
    }

    public PathNode FindShortestFullPathLength()
    {
        return NodesByFullPath.GetFirst();
    }

    public bool HasPosition(Vector2Int pos)
    {
        return PositionToNode.ContainsKey(pos);
    }

    public PathNode FindForPosition(Vector2Int pos)
    {
        PathNode result = null;
        PositionToNode.TryGetValue(pos, out result);
        return result;
    }
}

public class PathNode
{

    public readonly Vector2Int Position;

    public readonly int PathLengthFromStart;

    public readonly PathNode CameFrom;

    public readonly int HeuristicEstimatePathLength;

    public readonly int FullPathLength;

    public PathNode(
        Vector2Int Position,
        int PathLengthFromStart,
        PathNode CameFrom,
        int HeuristicEstimatePathLength
    )
    {
        this.Position = Position;
        this.PathLengthFromStart = PathLengthFromStart;
        this.CameFrom = CameFrom;
        this.HeuristicEstimatePathLength = HeuristicEstimatePathLength;
        FullPathLength = PathLengthFromStart + HeuristicEstimatePathLength;
    }
}

class SortedListWithDuplicates<K, V>
{

    private readonly SortedList<K, List<V>> list = new SortedList<K, List<V>>();

    public void Add(K key, V value)
    {
        List<V> bucket = null;
        list.TryGetValue(key, out bucket);
        if (bucket == null)
        {
            bucket = new List<V>();
            list[key] = bucket;
        }
        bucket.Add(value);
    }

    public void Remove(K key, V value)
    {
        List<V> bucket = null;
        list.TryGetValue(key, out bucket);
        if (bucket == null)
        {
            return;
        }
        bucket.Remove(value);
        if (bucket.Count == 0) 
        {
            list.Remove(key);
        }
    }

    public int Count() => list.Count;

    public V GetFirst()
    {
        if (list.Count == 0)
        {
            return default(V);
        }
        List<V> bucket = list.Values[0];
        if (bucket.Count == 0)
        {
            return default(V);
        }
        return bucket[0];
    }
}