/**
   * Find the path from start to goal using breadth first search
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @return The list of intersections that form the shortest (unweighted) path from start to goal
   *     (including both start and goal).
   */
  public List<GeographicPoint> bfs(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
    Queue<MapNode> q = new LinkedList<MapNode>(); // Initialize queue
    HashSet<MapNode> visited = new HashSet<MapNode>(); // Initialize visited HashSet
    HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap

    MapNode startNode = vertices.get(start); // get start node
    q.add(startNode); // enqueue start node
    visited.add(startNode); // add start node to visited

    // Hook for visualization.  See writeup.
    nodeSearched.accept(startNode.getCoordinates());

    while (q.isEmpty() == false) { // while queue is not empty

      MapNode dqNode = q.remove(); // dequeue current node from the front of the queue
      nodeSearched.accept(dqNode.getCoordinates());

      if (vertices.get(goal)
          == vertices.get(
              dqNode.getCoordinates())) { // if current node = goal, then return the parent map

        List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();
        listCoordinates =
            buildPath(
                dqNode,
                parent); // Build a list using the path found from the start node to the goal node.
        return listCoordinates;
      }

      for (MapEdge e :
          dqNode.getEdges()) { // for each of the current node's neighbors not in the visited set
        MapNode n = vertices.get(e.end);
        if (visited.contains(n) == false) {
          visited.add(n);
          parent.put(n, dqNode);
          q.add(n);
        }
      }
    } // while
    return null;
  }
  /**
   * Build a list using the path found from the start node to the goal node.
   *
   * @param dqNode The goal node
   * @param parent The hashmap of every node's parent
   * @return The list of intersections that form the shortest (unweighted) path from start to goal
   *     (including both start and goal).
   */
  private List<GeographicPoint> buildPath(MapNode dqNode, HashMap<MapNode, MapNode> parent) {
    List<MapNode> mapPath = new ArrayList<MapNode>();
    List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();

    boolean loop = true;
    while (loop) {
      mapPath.add(dqNode);
      dqNode = parent.get(dqNode);
      if (parent.get(dqNode) == null) {
        mapPath.add(dqNode);
        loop = false;
      }
    }

    Collections.reverse(mapPath);

    for (MapNode n : mapPath) {
      listCoordinates.add(n.getCoordinates());
    }
    return listCoordinates;
  }
  /**
   * Find the path from start to goal using A-Star search
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @return The list of intersections that form the shortest path from start to goal (including
   *     both start and goal).
   */
  public List<GeographicPoint> aStarSearch(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {

    // Initialize priority queue
    PriorityQueue<MapNode> pq = new PriorityQueue<MapNode>(); // Initialize priority queue

    // Initialize HashSet
    HashSet<MapNode> visited = new HashSet<MapNode>();
    HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap

    // Initialize values to infinity
    MapNode startNode = vertices.get(start);
    initializeNodes(startNode, vertices);

    // Initialize estimated distance
    double heuristicDistance = start.distance(goal);

    // Enqueue start node
    pq.add(startNode);

    // Count
    int aStarCount = 0;

    // while queue is not empty
    while (pq.isEmpty() == false) {

      // dequeue current node from the front of the queue
      // MapNode dqNode = pq.remove();
      MapNode dqNode = pq.poll();
      aStarCount++;

      // Hook for visualization.
      nodeSearched.accept(dqNode.getCoordinates());

      // if curr is not visited
      if (visited.contains(dqNode) == false) {

        // add curr to the visited set
        visited.add(dqNode);

        // for each of the current node's neighbors not in the visited set
        for (MapEdge e : dqNode.getEdges()) {

          MapNode n = vertices.get(e.end);

          if (visited.contains(n) == false) {

            // calculate the distance from the start node to the current node,
            // to each neighboring node
            // double g = dqNode.getWeight() + e.getDistance();
            double g = dqNode.getWeight() + dqNode.getCoordinates().distance(n.getCoordinates());

            // calculate the estimated distance from the neighboring node to
            // the goal
            double h = n.getCoordinates().distance(goal);

            // set the weight of the neighboring node
            n.setWeight(g + h);

            // set the current node as it's neighboring nodes' parent
            parent.put(n, dqNode);

            // If neighbor is the goal, return!
            if (vertices.get(n.getCoordinates()) == vertices.get(goal)) {
              List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();

              // Build a list using the path found from the start node to the goal node.
              listCoordinates = buildPath(n, parent);

              // Print out the visited nodes
              printPathOfCoordinates(listCoordinates);
              System.out.println("a star count: " + aStarCount);
              return listCoordinates;
            }

            // if the original heuristic distance is less than or equal
            // to the new heuristic distance, add the node to the
            // priority queue.
            if (heuristicDistance <= n.getWeight()) {
              pq.offer(n);
            }
          }
        }
      } // if
    } // while

    return null;
  }
  /**
   * Find the path from start to goal using Dijkstra's algorithm
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @return The list of intersections that form the shortest path from start to goal (including
   *     both start and goal).
   */
  public List<GeographicPoint> dijkstra(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {

    PriorityQueue<MapNode> pq = new PriorityQueue<MapNode>(); // Initialize priority queue
    HashSet<MapNode> visited = new HashSet<MapNode>();
    HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap

    // Initialize values to infinity
    MapNode startNode = vertices.get(start);
    initializeNodes(startNode, vertices);

    // Enqueue start node
    pq.add(startNode);

    int dijkstraCount = 0;
    // while queue is not empty
    while (pq.isEmpty() == false) {

      // dequeue current node from the front of the queue
      MapNode dqNode = pq.remove();
      dijkstraCount++;

      // Hook for visualization.
      nodeSearched.accept(dqNode.getCoordinates());

      // if curr is not visited
      if (visited.contains(dqNode) == false) {

        // add curr to the visited set
        visited.add(dqNode);

        // if the current node is equal to the goal node...
        if (vertices.get(dqNode.getCoordinates()) == vertices.get(goal)) {
          System.out.println("Dijkstra count: " + dijkstraCount);
          List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();

          // Build a list using the path found from the start node to the goal node.
          listCoordinates = buildPath(dqNode, parent);

          // Print out the visited nodes
          printPathOfCoordinates(listCoordinates);
          return listCoordinates;
        }

        // for each of the current node's neighbors not in the visited set
        for (MapEdge e : dqNode.getEdges()) {
          MapNode n = vertices.get(e.end);

          if (visited.contains(n) == false) {
            // visited.add(vertices.get(e.end));
            // 1. if path through curr to n is shorter
            double newWeight = dqNode.getWeight() + e.getDistance();
            if (newWeight < n.getWeight()) {
              // 2. update n's distances
              n.setWeight(newWeight);
              // 3. update curr as n's parent in parent map
              parent.put(n, dqNode);
              // 4. enqueue (n, distance) into the PQ
              pq.add(n);
            }
          }
        }
      } // if
    } // while

    return null;
  }