Pathfinding

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



public class PathFinding{

    public static List<Node> reconstruct_path(Dictionary<Node, Node> cameFrom, Node current){
        List<Node> total_path=new List<Node>();
        total_path.Add(current);
        while (cameFrom.ContainsKey(current)){
            current=cameFrom[current];
            total_path.Add(current);
        }
        total_path.Reverse();
        return total_path;
    }


    public static List<Node> Astar(Node start, Node goal, Func<Node,Node,double> costEstimate){
        // The set of nodes already evaluated
        List<Node> closedSet=new List<Node>();
        // The set of currently discovered nodes still to be evaluated.
        // Initially, only the start node is known.
        List<Node> openSet=new List<Node>();
        openSet.Add(start);
        // For each node, which node it can most efficiently be reached from.
        // If a node can be reached from many nodes, cameFrom will eventually contain the
        // most efficient previous step.
        Dictionary<Node, Node> cameFrom = new Dictionary<Node,Node>();
        // For each node, the cost of getting from the start node to that node.
        Dictionary<Node, Double> gScore=new Dictionary<Node, double>();
        double DefaultScore=Double.PositiveInfinity;
        gScore[start]=0;
        // For each node, the total cost of getting from the start node to the goal
        // by passing by that node. That value is partly known, partly heuristic.
        Dictionary<Node, double> fScore=new Dictionary<Node, double>();
        //for the first node, that value is completely heuristic
        fScore[start]=costEstimate(start,goal);
        while (openSet.Count!=0){
            //the current node is the one in openSet with the lowest fScore value
            Node current=start;
            Double minScore=DefaultScore;
            foreach (Node n in openSet){
                Double fValue=DefaultScore;
                fScore.TryGetValue(n,out fValue);
                if (fValue<minScore){
                    current=n;
                    minScore=fValue;
                }
            }
            if (current==goal){
                List<Node> path=reconstruct_path(cameFrom,current);
                return path;
            }
            openSet.Remove(current);
            closedSet.Add(current);
            foreach (Node neighbour in current.neighbours()){
                if(neighbour==null){
                    continue;
                }
                if (closedSet.Contains(neighbour)){
                    continue;
                    // Ignore the neighbor which is already evaluated.
                }
                // The distance from start to a neighbor
                double tentative_gScore = gScore[current] + current.weight(neighbour);
                if (!openSet.Contains(neighbour)){
                    openSet.Add(neighbour);
                }else if (tentative_gScore >= gScore[neighbour]){
                    continue;       // This is not a better path.
                }
                // This path is the best until now. Record it!
                cameFrom[neighbour] = current;
                gScore[neighbour] = tentative_gScore;
                fScore[neighbour] = gScore[neighbour] + costEstimate(neighbour, goal);
            }
        }
        //failure
        return new List<Node>();
    }

}