public void search(double goal) {
        Point2D position = null;// = new Point2D.Double(0, 0);
        //saveState(position, position, 0);

        while (!queue.isEmpty()) {
            position = queue.poll();
            if (goal == position.getX() || goal == position.getY()) {
                //last = position;
                break;
            }

            //System.out.println("[current] = " + position.toString());
            calculateX(position);
            calculateY(position);
            //System.out.println("-----------------------");
        }

        //System.out.println(uniqueLeafs.get(position).toString());
        //System.out.println("[out] = " + position.toString());
        //System.out.println(this.treeRoadMap.toString());
        if (!queue.isEmpty()) {
            List<Point2DComment> T2 = treeRoadMap.getAncestors(new Point2DComment(position));
            Collections.reverse(T2);
            for (Point2DComment p : T2) {
                System.out.println(p.toString() + " | " + p.getComment());
            }
        }
    }