import java.util.Scanner;
import java.util.Stack;

public class AStarPathFinder {

    private WeightedDigraph graph;
    private double[][] coordinates;

    public AStarPathFinder(WeightedDigraph g, double[][] coordinates) {
        this.graph = g;
        this.coordinates = coordinates.clone();
    }

    public double h(int v, int w) {
        double vx = coordinates[v][0];
        double vy = coordinates[v][1];
        double wx = coordinates[w][0];
        double wy = coordinates[w][1];
        return Math.sqrt((vx-wx)*(vx-wx) + (vy-wy)*(vy-wy));
    }

    public Stack<DirectedEdge> pathBetween(int source, int destination) {
        DirectedEdge[] edgeTo;
        double[] distTo;
        IndexMinPQ<Double> pq;

        // construct necessary arrays, priority queues..
        edgeTo = new DirectedEdge[graph.numVertices()];
        distTo = new double[graph.numVertices()];
        pq = new IndexMinPQ<Double>(graph.numVertices());

        // initialize distances to source..
        for (int v = 0; v < graph.numVertices(); v++) {
            distTo[v] = Double.POSITIVE_INFINITY;
        }

        distTo[source] = 0.0;
        pq.insert(source, 0.0);

        while (!pq.isEmpty()) {
            int v = pq.delMin();
            if (v == destination) // found the shortest path there -- we are done!
                break;

            for (DirectedEdge e : graph.adj(v)) {
                //relax distance to w, if appropriate...
                int w = e.to();
                if (distTo[w] > distTo[v] + e.weight()) {
                    distTo[w] = distTo[v] + e.weight();
                    edgeTo[w] = e;
                    if (pq.contains(w)) {
                        pq.decreaseKey(w, distTo[w] + h(w, destination));
                    } else {
                        pq.insert(w, distTo[w] + h(w, destination));
                    }
                }
            }
        }

        // now use the edgeTo information to build the path between 
        // source and destination...
        Stack<DirectedEdge> edges = new Stack<DirectedEdge>();
        int w = destination;
        while (w != source) {
            DirectedEdge edge = edgeTo[w];
            edges.push(edge);
            w = edge.from();
        }

        return edges;
    }

    public static void main(String[] args) {

        WeightedDigraph g = new WeightedDigraph(13);
        g.addEdge(new DirectedEdge(0, 3, 3));
        g.addEdge(new DirectedEdge(3, 0, 3));

        g.addEdge(new DirectedEdge(0, 2, 2));
        g.addEdge(new DirectedEdge(2, 0, 2));

        g.addEdge(new DirectedEdge(0, 1, 7));
        g.addEdge(new DirectedEdge(1, 0, 7));

        g.addEdge(new DirectedEdge(1, 2, 3));
        g.addEdge(new DirectedEdge(2, 1, 3));

        g.addEdge(new DirectedEdge(2, 4, 4));
        g.addEdge(new DirectedEdge(4, 2, 4));

        g.addEdge(new DirectedEdge(1, 4, 4));
        g.addEdge(new DirectedEdge(4, 1, 4));

        g.addEdge(new DirectedEdge(4, 6, 5));
        g.addEdge(new DirectedEdge(6, 4, 5));

        g.addEdge(new DirectedEdge(6, 8, 3));
        g.addEdge(new DirectedEdge(8, 6, 3));

        g.addEdge(new DirectedEdge(2, 8, 1));
        g.addEdge(new DirectedEdge(8, 2, 1));

        g.addEdge(new DirectedEdge(8, 7, 2));
        g.addEdge(new DirectedEdge(7, 8, 2));

        g.addEdge(new DirectedEdge(7, 5, 2));
        g.addEdge(new DirectedEdge(5, 7, 2));

        g.addEdge(new DirectedEdge(5, 11, 5));
        g.addEdge(new DirectedEdge(11, 5, 5));

        g.addEdge(new DirectedEdge(9, 11, 4));
        g.addEdge(new DirectedEdge(11, 9, 4));

        g.addEdge(new DirectedEdge(10, 11, 4));
        g.addEdge(new DirectedEdge(11, 10, 4));

        g.addEdge(new DirectedEdge(9, 10, 6));
        g.addEdge(new DirectedEdge(10, 9, 6));

        g.addEdge(new DirectedEdge(12, 9, 4));
        g.addEdge(new DirectedEdge(9, 12, 4));

        g.addEdge(new DirectedEdge(12, 10, 4));
        g.addEdge(new DirectedEdge(10, 12, 4));

        g.addEdge(new DirectedEdge(12, 3, 2));
        g.addEdge(new DirectedEdge(3, 12, 2));
        
        double[][] coordinates = {{2,5},{1,4},{3,4},{5,5},{1,3},{7,1},{2,2},{5,2},{3,3},{6,3},{8,3},{8,2},{6,4}};

        System.out.println(g);

        AStarPathFinder pathFinder = new AStarPathFinder(g,coordinates);
        coordinates = new double[1][1];
        Iterable<DirectedEdge> path = pathFinder.pathBetween(0, 5);

        System.out.println("\nshortest path between " + 0 + " to " + 5 + ":");
        for (DirectedEdge e : path) {
            System.out.println(e);
        }
    }
}