public Double aStarSearch(Vertex[] vertices, int start, int goal) {
    PriorityQueue<Vertex> vertexQueue = new PriorityQueue<Vertex>();

    vertices[start].gScore = 0.0;
    vertices[goal].isGoal = true;
    vertexQueue.add(vertices[start]);

    while (!vertexQueue.isEmpty()) {

      Vertex current = vertexQueue.poll();
      if (current.isGoal) {
        return vertices[goal].gScore;
      }

      current.visited = true;
      vertices[current.vertexID].visited = true;

      // for all vertex adjacent to current do
      for (int n = 0; n < vertices.length; n++) {
        if (current.adjList.contains(n) && !vertices[n].visited) {
          Vertex adj = vertices[n];
          Double gScore = current.gScore + euclidianDistance(current, adj);

          // if adj not in Q or gScore < adj.gScore 
          if (!vertexQueue.contains(adj) || gScore < adj.gScore) {
            // Update the Scores for the adjacent vertex.
            vertexQueue.remove(adj);
            adj.gScore = gScore;
            adj.hScore = euclidianDistance(adj, vertices[goal]);
            adj.fScore = adj.gScore + adj.hScore;
            // Add this vertex back into the priority queue with updated information.
            vertexQueue.add(adj);
          }
        }
      }
    }

    // Failed.
    return Double.POSITIVE_INFINITY; 
  }