如何在Java中实现A*？

import java.util.*;

class Node {
    int x;
    int y;
    int cost;
    int heuristic;
    Node parent;

    public Node(int x, int y, int cost, int heuristic, Node parent) {
        this.x = x;
        this.y = y;
        this.cost = cost;
        this.heuristic = heuristic;
        this.parent = parent;
    }
}

public class AStar {
    private static final int[][] GRID = {
        {0, 0, 0, 0, 0},
        {0, 1, 1, 1, 0},
        {0, 1, 0, 1, 0},
        {0, 1, 1, 1, 0},
        {0, 0, 0, 0, 0}
    };

    private static final int[][] DIRECTIONS = {
        {-1, 0}, {1, 0}, {0, -1}, {0, 1}
    };

    private static int calculateHeuristic(int x, int y, int targetX, int targetY) {
        return Math.abs(targetX - x) + Math.abs(targetY - y);
    }

    public static List<Node> findPath(int startX, int startY, int targetX, int targetY) {
        PriorityQueue<Node> openSet = new PriorityQueue<>(Comparator.comparingInt(a -> a.cost + a.heuristic));
        Set<Node> closedSet = new HashSet<>();

        Node startNode = new Node(startX, startY, 0, calculateHeuristic(startX, startY, targetX, targetY), null);
        openSet.add(startNode);

        while (!openSet.isEmpty()) {
            Node currentNode = openSet.poll();

            if (currentNode.x == targetX && currentNode.y == targetY) {
                // 构建路径
                List<Node> path = new ArrayList<>();
                Node node = currentNode;
                while (node != null) {
                    path.add(node);
                    node = node.parent;
                }
                Collections.reverse(path);
                return path;
            }

            closedSet.add(currentNode);

            for (int[] direction : DIRECTIONS) {
                int nextX = currentNode.x + direction[0];
                int nextY = currentNode.y + direction[1];

                if (nextX < 0 || nextX >= GRID.length || nextY < 0 || nextY >= GRID[0].length || GRID[nextX][nextY] == 1) {
                    continue;
                }

                int nextCost = currentNode.cost + 1;
                int nextHeuristic = calculateHeuristic(nextX, nextY, targetX, targetY);
                Node nextNode = new Node(nextX, nextY, nextCost, nextHeuristic, currentNode);

                if (closedSet.contains(nextNode)) {
                    continue;
                }

                Node existingNode = findNodeInSet(openSet, nextNode);
                if (existingNode != null && nextNode.cost >= existingNode.cost) {
                    continue;
                }

                openSet.add(nextNode);
            }
        }

        return null;
    }

    private static Node findNodeInSet(Set<Node> set, Node node) {
        for (Node n : set) {
            if (n.x == node.x && n.y == node.y) {
                return n;
            }
        }
        return null;
    }

    public static void main(String[] args) {
        List<Node> path = findPath(0, 0, 4, 4);
        if (path != null) {
            for (Node node : path) {
                System.out.println("(" + node.x + ", " + node.y + ")");
            }
        } else {
            System.out.println("No path found.");
        }
    }
}