A* implementation

gistfile1.cs

        private IPath AStar(SquareGridWorldLocation start, SquareGridWorldLocation goal, NetworkType validNodeTypes)
        {
            var closedSet = new Dictionary<int, Node>();
            var openSet = new Dictionary<int, Node> {{start.GetHashCode(), new Node() {WorldLocation = start}}};
            var cameFrom = new Dictionary<int, Node>();

            while (openSet.Count > 0)
            {
                var current = Lowest(openSet);
                if (current.WorldLocation.Equals(goal))
                {
                    return ReconstructPath(cameFrom, goal, start);
                }
                openSet.Remove(current.WorldLocation.GetHashCode());
                closedSet.Add(current.WorldLocation.GetHashCode(), current);

                foreach (var neighbour in GetNeighbours(current, validNodeTypes))
                {
                    var neighbourIndex = SquareGridWorldLocation.GenerateHashCode(neighbour[0], neighbour[1]);
                    var closedContainsNeighbour = closedSet.ContainsKey(neighbourIndex);
                    var openContainsNeighbour = openSet.ContainsKey(neighbourIndex);

                    Node neighbourNode;
                    if (closedContainsNeighbour)
                        neighbourNode = closedSet[neighbourIndex];
                    else if (openContainsNeighbour)
                        neighbourNode = openSet[neighbourIndex];
                    else
                        neighbourNode = new Node()
                        {
                            WorldLocation = new SquareGridWorldLocation(neighbour[0], neighbour[1])
                        };
                    var tentativeGScore = current.GScore + DistanceBetween((SquareGridWorldLocation)current.WorldLocation, (SquareGridWorldLocation)neighbourNode.WorldLocation);
                    if (closedContainsNeighbour &&
                        tentativeGScore >= neighbourNode.GScore)
                        continue;
                    
                    if (!openContainsNeighbour || tentativeGScore < neighbourNode.GScore)
                    {
                        cameFrom[neighbourIndex] = current;
                        neighbourNode.GScore = tentativeGScore;
                        neighbourNode.FScore = neighbourNode.GScore + AStarHeuristic((SquareGridWorldLocation)neighbourNode.WorldLocation, goal);
                        if (!openContainsNeighbour)
                            openSet.Add(neighbourIndex, neighbourNode);
                    }
                }
            }
            return null;
       	}