PathFinder.java

import java.util.*;

public class PathFinder {

    private Grid grid;

    private int nodesExplored;

    public PathFinder(Grid grid) {
        this.grid = grid;
        this.nodesExplored = 0;
    }

    private int calculateHeuristic(Node current, Node goal) {

        return Math.abs(current.row - goal.row)
                + Math.abs(current.column - goal.column);
    }

    public List<Node> findPath(
            int startRow,
            int startColumn,
            int goalRow,
            int goalColumn) {

        nodesExplored = 0;

        Node start = new Node(startRow, startColumn);
        Node goal = new Node(goalRow, goalColumn);

        PriorityQueue<Node> openList =
                new PriorityQueue<>(
                        Comparator.comparingInt(Node::getFCost)
                );

        Set<String> closedList =
                new HashSet<>();

        Map<String, Integer> bestCosts =
                new HashMap<>();

        start.gCost = 0;
        start.hCost =
                calculateHeuristic(start, goal);

        openList.add(start);

        bestCosts.put(
                start.row + "," + start.column,
                0
        );

        while (!openList.isEmpty()) {

            Node current = openList.poll();

            String currentKey =
                    current.row + "," + current.column;

            if (closedList.contains(currentKey)) {
                continue;
            }

            closedList.add(currentKey);

            nodesExplored++;

            // Goal reached
            if (current.row == goal.row &&
                current.column == goal.column) {

                return reconstructPath(current);
            }

            int[][] directions = {
                    {-1, 0},
                    {1, 0},
                    {0, -1},
                    {0, 1}
            };

            for (int[] direction : directions) {

                int newRow =
                        current.row + direction[0];

                int newColumn =
                        current.column + direction[1];

                if (!grid.isValidPosition(
                        newRow,
                        newColumn)) {

                    continue;
                }

                if (grid.isObstacle(
                        newRow,
                        newColumn)) {

                    continue;
                }

                String neighborKey =
                        newRow + "," + newColumn;

                if (closedList.contains(neighborKey)) {
                    continue;
                }

                int newGCost =
                        current.gCost + 1;

                Integer previousBest =
                        bestCosts.get(neighborKey);

                if (previousBest != null &&
                    newGCost >= previousBest) {

                    continue;
                }

                Node neighbor =
                        new Node(
                                newRow,
                                newColumn
                        );

                neighbor.gCost = newGCost;

                neighbor.hCost =
                        calculateHeuristic(
                                neighbor,
                                goal
                        );

                neighbor.parent = current;

                bestCosts.put(
                        neighborKey,
                        newGCost
                );

                openList.add(neighbor);
            }
        }

        return new ArrayList<>();
    }

    private List<Node> reconstructPath(Node goal) {

        List<Node> path =
                new ArrayList<>();

        Node current = goal;

        while (current != null) {

            path.add(current);

            current = current.parent;
        }

        Collections.reverse(path);

        return path;
    }

    public int getNodesExplored() {

        return nodesExplored;
    }
}