Алгоритм Java A * не находит пути

Пытаюсь создать программу, которая берет изображение лабиринта и выводит лабиринт с выделенным решением, но моя реализация A* ошибочна.

Я основываю алгоритм на псевдокоде Википедии и Coding Train послужила источником вдохновения для этого проекта. Я сравнил код алгоритма и ничего необычного не заметил.

public class Main {
    public static void main(String[] args) {
        ImageConverter a = new ImageConverter("file path");

        Node[][] nodes = a.to2Darray();
        Solver solve = new Solver(nodes);
        ArrayList<Node> solution = solve.aStar(new Node(0,1, 0),new Node(14,13,0));
        System.out.println(solution);
        a.toImage(nodes, solution);

    }
}



public class Solver {

private Node[][] graph;

public Solver(Node[][] graph) {
    this.graph = graph;
}

public ArrayList<Node> aStar(Node start, Node finish){ // solves maze using A*
    ArrayList<Node> closeSet = new ArrayList<>();
    ArrayList<Node> openSet = new ArrayList<>();
    openSet.add(start);
    ArrayList<Node> path = new ArrayList<>();
    while (openSet.size()>0){
        int bestF = 0;
        for (int i = 0; i < openSet.size(); i++){ // find next least costly node
            if (openSet.get(i).getF() < openSet.get(bestF).getF()){
                bestF = i;
            }
        }

        Node current = openSet.get(bestF);

        if (current.equals(finish)){ // check if current node is end node
            Node temp = current;
            path.add(temp);
            while(temp.getPrevious()!=null){
                path.add(temp.getPrevious());
                temp = temp.getPrevious();
            }
            return path;
        }

        openSet.remove(current);
        closeSet.add(current);

        ArrayList<Node> neighbors = current.getNeighbors();
        for (int i = 0; i < neighbors.size(); i++){ // check neighbors
            Node n = neighbors.get(i);
            boolean isNewPath = false;

            if (!closeSet.contains(n) && n.getState()!=1){
                double tempG = current.getG()+heuristic(n,current);
                if (openSet.contains(n)){
                    if (tempG < n.getG()){
                        n.setG(tempG);
                        isNewPath = true;
                    }
                    else{
                        n.setG(tempG);
                        openSet.add(n);
                        isNewPath = true;
                    }
                }
                if (isNewPath) {
                    n.setH(heuristic(n, finish));
                    n.setF(n.getG() + n.getH());
                    n.setPrevious(current);
                }
            }
        }
    }
    return null; // no solution
}

private static double heuristic(Node end, Node finish) {
    int y1 = end.getCol();
    int x1 = end.getRow();
    int y2 = finish.getCol();
    int x2 = finish.getRow();
    return Math.sqrt((x1-x2)*(x1-x2)+(y2-y1)*(y2-y1)); // order doesn't matter in because of squaring

    }
}

public class Node {

private double f, g, h;
private int row, col; // row and col
private int state;
private ArrayList<Node> neighbors = new ArrayList<>();
private Node previous = null;

public Node(int r, int c, int state) {
    this.state = state;
    row = r;
    col = c;
}

public int getRow() {
    return row;
}

public int getCol() {
    return col;
}

public double getF() {
    return f;
}

public double getG() {
    return g;
}

public double getH() {
    return h;
}

public void setF(double f) {
    this.f = f;
}

public void setG(double g) {
    this.g = g;
}

public void setH(double h) {
    this.h = h;
}

public int getState() {
    return state;
}

public void setState(int state) {
    this.state = state;
}

public void addNeighbor(Node b){
    neighbors.add(b);
}

public void setPrevious(Node n){
    previous = n;
}

public Node getPrevious(){
    return previous;
}

@Override
public String toString(){
    return "["+row+"]["+col+"]";
}

public ArrayList<Node> getNeighbors(){
    return neighbors;
}

@Override
public boolean equals(Object o){
    if (o ==this){
        return true;
    }
    if (!(o instanceof Node)){
        return false;
    }
    Node n = (Node) o;
    return row == n.getRow() && col==n.getRow();
}
}




public class ImageConverter {

private BufferedImage image;
private int x;
private int y;
private String path;

public ImageConverter(String path) {
    try {
        image = ImageIO.read(new FileInputStream(path));
    } 
    catch (IOException e) {
        e.printStackTrace();
    }
    this.path = path;
    this.x = image.getWidth(); // done for readability of to2Darray()
    this.y = image.getHeight();
}

public Node[][] to2Darray() { // nested loop does [j][i] as [i][j] reflects along line from top left to bot right
    Node[][] nodes = new Node[x][y];

    for (int i = 0; i < nodes.length; i++){ // inital assignment/null pointer
        for (int j = 0; j < nodes.length; j++){
            nodes[i][j] = new Node(i,j,0); // the [j][i] thing doesn't matter here
        }
    }

    for (int i = 0; i < x; i++) {
        for (int j = 0; j < y; j++) {
            Color t = new Color(image.getRGB(i, j));
            if (t.equals(Color.BLACK)) {
                nodes[j][i].setState(1); //black pixels are walls
            } 
            else if (t.equals(Color.WHITE)) {
                nodes[j][i].setState(0); //white pixels are paths
            } 
            else { // is not black or white
                try {
                    throw new Exception("Pixel at [" + i + "][" + j + "]" + " is not black or white");
                } 
                catch (Exception e) {
                    System.out.println("Java threw an exception while throwing an exception. God help you" +
                            " if you ever see this. But if you do, there might be a pixel in the maze that is not b/w");
                }
            }
        }
    }

    for (int row = 0; row < x; row++){ // add neighbors, if neighbor does not exist (out of bounds) it makes it null
        for (int col = 0; col < y; col++){
            try{
                nodes[row][col].addNeighbor(nodes[row-1][col]); // Node to the top
            }
            catch (IndexOutOfBoundsException e){
                nodes[row][col].addNeighbor(null);
            }

            try{
                nodes[row][col].addNeighbor(nodes[row][col+1]); // Node to the right
            }
            catch (IndexOutOfBoundsException e){
                nodes[row][col].addNeighbor(null);
            }

            try{
                nodes[row][col].addNeighbor(nodes[row+1][col]); // Node to the bottom
            }
            catch (IndexOutOfBoundsException e){
                nodes[row][col].addNeighbor(null);
            }

            try{
                nodes[row][col].addNeighbor(nodes[row][col-1]); // Node to the left
            }
            catch (IndexOutOfBoundsException e){
                nodes[row][col].addNeighbor(null);
            }
        }
    }

    return nodes;
}

public void toImage(Node[][] graph, ArrayList<Node> solution) { // converts to image and saves it at location from constructor
    BufferedImage imageCopy = this.image;
    int index = path.lastIndexOf("\\"); // change this to \\ if on Windows
    File file = new File(path.substring(0, index) + "\\solved.png"); // remove the filename from filepath

    final int RED = new Color(255, 0, 0).getRGB(); // for readability
    final int BLACK = new Color(0, 0, 0).getRGB();
    final int WHITE = new Color(255, 255, 255).getRGB();

    /*for (int i = 0; i < x; i++) { // convert to BufferedImage
        for (int j = 0; j < y; j++) {
            if (graph[i][j].getState() == 0) { // empty path
                image.setRGB(j, i, WHITE);
            }
            else if (graph[i][j].getState() == 1) { // wall
                image.setRGB(j, i, BLACK);
            }
            if
        }
    }*/
    for (int i = 0; i < x; i++) { // convert to BufferedImage
        for (int j = 0; j < y; j++) {
            System.out.println(i+" "+j);
            if (solution.contains(graph[j][i])){
                imageCopy.setRGB(i,j,RED);
            }
        }
    }

    try {
        ImageIO.write(image, "png", file);
    } 
    catch (IOException e) {
        e.printStackTrace();
    }
}
}

В Main solution должно иметь ArrayList<Node> объектов Node, которые создают лучший путь, однако он возвращает null, показывая, что не находит решения.