private Path aStar() {

    init();

    // While there are still node to explore
    while (!openSet.isEmpty()) {
      // Get the node with the lowest f_score value
      Node currentNode = openSet.poll();
      Square currentSquare = currentNode.getSquare();

      //            System.out.println(currentSquare.toString() + " openSet");

      // If current square is the destination square, return the reconstructed path
      if (currentSquare.equals(endPosition)) {
        return reconstructPath(currentSquare);
      }

      closedSet.add(currentSquare);
      piece.move(currentSquare);

      //            for (Square neighbourSquare : piece.getValidMoves()) {
      //                System.out.print(neighbourSquare + " ");
      //            }
      //            System.out.println();

      // Explore the neighbours of the current square
      for (Square neighbourSquare : piece.getValidMoves()) {
        //                System.out.println(neighbourSquare.toString() + " neighbour");

        // If the current neighbour square has already been explored, pass
        if (closedSet.contains(neighbourSquare)) {
          continue;
        }

        // Get a tentative gScore, default the distance from 'piece' to 'neighbourSquare' to 1
        // (step).
        int tentativeGScore = gScore.get(currentSquare) + 1;

        if ((!openSet.contains(neighbourSquare)) || tentativeGScore < gScore.get(neighbourSquare)) {

          cameFrom.put(neighbourSquare, currentSquare);
          gScore.put(neighbourSquare, tentativeGScore);
          fScore.put(
              neighbourSquare,
              (tentativeGScore
                  + ((AStarPiece) piece).getHeuristicMoves(neighbourSquare, endPosition)));

          // Add this neighbour to the open set
          Node newNode = new Node(neighbourSquare, fScore.get(neighbourSquare));
          openSet.add(newNode);
        }
      }
    }

    // If no path is found, return null
    return null;
  }