import java.util.TreeMap;
import java.util.HashMap;
import java.util.Comparator;
import java.util.LinkedList;
import java.util.ListIterator;

public class Graph
{
    public Graph( Map _map )
    {
        map = _map;
    }

    Open open;
    Closed closed;
    private Map map;

    LinkedList aStarSearch( Location startLocation, Location goalLocation )
    {
        open = new Open();
        closed = new Closed();

        Node startNode = new Node();

        startNode.location = startLocation;
        startNode.costFromStart = 0;
        startNode.costToGoal = map.pathCostEstimate( startLocation, goalLocation );
        startNode.parent = null;
        startNode.update();
        
        open.put( startNode );
        System.out.println("Startnode -> " + startNode );
        System.out.println("Goal location -> " + goalLocation );

        while( open.size() > 0 )
        {
            Node node = open.pop();
            //System.out.println("Checking -> " + node.location + "\t" + node );
            if( node.costToGoal == 0 )
            {
                LinkedList answer = new LinkedList();
                do {
                    answer.addFirst( node.location );
                    node = node.parent;
                } while( node != null );
                System.out.println( "Route found" );
                
                return answer;
            }
            else
            {
                LinkedList list = map.findNeighbors ( node.location );
                ListIterator it = list.listIterator(0);
                while( it.hasNext() )
                {
                    Location location = (Location) it.next();
                    Node newnode = new Node();
                    newnode.location = location;

                    float newcost = node.costFromStart + map.traverseCost( node.location, newnode.location );

                    Node existing = open.get( newnode.location );
                    if( existing == null )
                        existing = closed.get( newnode );
                    if( existing != null )
                        if( existing.costFromStart <= newcost )
                            continue;

                    newnode.parent = node;
                    newnode.costFromStart = newcost;
                    newnode.costToGoal = map.pathCostEstimate( newnode.location, goalLocation );
                    newnode.update();

                    closed.remove( newnode );
                    open.put( newnode );
                }
                open.remove( node );
                closed.put( node );
            }
        }
        return null;
    }

    public Open getOpen() { return open; }
    public Closed getClosed() { return closed; }
}