Невозможно реализовать A Star на java

Я весь день пытался запустить и запустить этот алгоритм, но я не могу этого сделать. Я прочитал много руководств в сети и исходный код на AS3, javascript и C ++; но я не могу адаптировать то, что вижу, к своему собственному коду.

Я создал класс AStar, у которого есть вложенный класс с именем Node. Карта представляет собой двумерный массив с именем MAP.

Самая большая проблема, с которой я столкнулся, - это получение значения F в функции pathfind.

Я реализовал F = G + H, моя проблема заключается в реальном алгоритме AStar. Может кто-нибудь, пожалуйста, помогите, вот как далеко я продвинулся на данный момент:

import java.util.ArrayList;

public class AStar
{
    int MAP[][];

    Node startNode, endNode;

    public AStar(int MAP[][], int startXNode, int startYNode,
                              int endXNode, int endYNode)
    {
        this.MAP = MAP;
        startNode = new Node(startXNode, startYNode);
        endNode = new Node(endXNode, endYNode);
    }

    public void pathfinder()
    {
        ArrayList openList = new ArrayList();
        ArrayList closedList = new ArrayList();



    }

    public int F(Node startNode, Node endNode)
    {
        return (H(startNode, endNode) + G(startNode));
    }

    //H or Heuristic part of A* algorithm
    public int H(Node startNode, Node endNode)
    {
        int WEIGHT = 10;
        int distance = (Math.abs(startNode.getX() - endNode.getX()) + Math.abs(startNode.getY() - endNode.getY()));

        return (distance * WEIGHT);
    }

    public int G(Node startNode)
    {
        if(MAP[startNode.getX() - 1][startNode.getY()] != 1)
        {
            return 10;
        }

        if(MAP[startNode.getX() + 1][startNode.getY()] != 1)
        {
            return 10;
        }

        if(MAP[startNode.getX()][startNode.getY() -1] != 1)
        {
            return 10;
        }

        if(MAP[startNode.getX()][startNode.getY() + 1] != 1)
        {
            return 0;
        }

        return 0;
    }

    public class Node
    {
        private int NodeX;
        private int NodeY;

        private int gScore;
        private int hScore;
        private int fScore;

        public Node(int NodeX, int NodeY)
        {
            this.NodeX = NodeX;
            this.NodeY = NodeY;
        }

        public int getX()
        {
            return NodeX;
        }

        public int getY()
        {
            return NodeY;
        }

        public int getG()
        {
            return gScore;
        }

        public void setG(int gScore)
        {
            this.gScore = gScore;
        }

        public int getH()
        {
            return hScore;
        }

        public void setH(int hScore)
        {
            this.hScore = hScore;
        }

        public int getF()
        {
            return fScore;
        }

        public void setF(int fScore)
        {
            this.fScore = fScore;
        }
    }
}

Это самое дальнее, чего я когда-либо мог достичь с функцией поиска пути:

   public void pathfinder()
    {
        LinkedList<Node> openList = new LinkedList();
        LinkedList<Node> closedList = new LinkedList();

        Node currentNode;

        openList.add(startNode);

        while(openList.size() > 0)
        {
            currentNode = (Node) openList.get(0);
            closedList.add(currentNode);


            for(int i = 0; i < openList.size(); i++)
            {
                int cost = F(currentNode, endNode);

            }
        }

    }