Pathfinding in game (Android)

    private void calcGoing() {
        listClosedGo.clear();// откуда можно атаковать
        listClosedGoShot.clear();// откуда можно атаковать
        ArrayList<Node> listOpen = new ArrayList<Node>();// стек
        int haveP = currentUnit.getPassability();
        int currP;
        int maxShot = currentUnit.getShotMaxPassability();
        Node startNode = new Node(null, currentUnit.getX(), currentUnit.getY(), 0);
        listOpen.add(startNode);
        int x, y;
        while (!listOpen.isEmpty()) {
            Node n = listOpen.get(0);
            listOpen.remove(n);
            x = n.getX();
            y = n.getY();
            currP = n.getPassability();
            if (currP > haveP) {
                continue;// нить окончена
            }
            if (maxShot >= currP) {
                if (listClosedGoShot.contains(n)) {
                    Node exist = listClosedGoShot.get(listClosedGoShot.indexOf(n));
                    if (exist.getPassability() > n.getPassability()) {
                        listClosedGoShot.remove(exist);
                        listClosedGoShot.add(n);
                    } else {
                        continue;
                    }
                } else if (listClosedGo.contains(n)) {
                    listClosedGo.remove(n);
                    listClosedGoShot.add(n);
                } else {
                    Unit u = getUnit(x, y);// получаем юнита на указанной клетке или null, если нет никого
                    if (u != null && u != currentUnit) {
                        continue;
                    } else {
                        listClosedGoShot.add(n);
                    }
                }
            } else {
                if (listClosedGoShot.contains(n)) {
                    continue;
                }
                if (listClosedGo.contains(n)) {
                    Node exist = listClosedGo.get(listClosedGo.indexOf(n));
                    if (exist.getPassability() > n.getPassability()) {
                        listClosedGo.remove(exist);
                        listClosedGo.add(n);
                    } else {
                        continue;
                    }
                } else {
                    Unit u = getUnit(x, y);
                    if (u != null && u != currentUnit) {
                        continue;
                    } else {
                        listClosedGo.add(n);
                    }
                }
            }
            int[] newx = {x, x, x - 1, x + 1};
            int[] newy = {y - 1, y + 1, y, y};
            for (int i = 0; i < 4; i++) {
                int tmpx = newx[i];
                int tmpy = newy[i];
                if (tmpx < 0 || tmpx > wTiles - 1 || tmpy < 0 || tmpy > hTiles - 1) {
                    continue;
                } else {
                    int addP = currentUnit.calcPassability(gameMap.getTileCode(tmpx, tmpy));// считаем проходимость клетки-кандидата на исследование
                    Node addNode = new Node(n, tmpx, tmpy, currP + addP);
                    listOpen.add(addNode);
                }
            }
        }
        invalidate();// рисуем, можно все в Path добавить и рисовать
    }

/**
 *А вот так ходить на выбранную клетку по самому оптимальному пути:
 */

[spoiler][code=java]
    private void goTo(int x, int y) {
        ArrayList<Node> path = new ArrayList<Node>();
        ArrayList<Node> listAll = new ArrayList<Node>(listClosedGoShot);
        listAll.addAll(listClosedGo);
        Node curr = listAll.get(listAll.indexOf(new Node(null, x, y, 0)));
//      currentUnit.changePassability(curr.getPassability());
        path.add(curr);
        Node tmp;
        while ((tmp = curr.getParent()) != null) {
            path.add(tmp);
            curr = tmp;
        }
        path.remove(path.size() - 1);// юнит и так уже находится в начале пути
        Collections.reverse(path);
        currentUnit.setMove(path);// идем
    }