public class AStarPathingStrategy implements PathingStrategy {


public int manhattan(Point start, Point end) {

        int m = (Math.abs(start.x - end.x) + Math.abs(start.y - end.y));
        return m;
        }

    public List<Point> computePath(Point start, Point end,
                                   Predicate<Point> canPassThrough,
                                   BiPredicate<Point, Point> withinReach,
                                   Function<Point, Stream<Point>> potentialNeighbors) {

        List<Point> pth = new ArrayList<>();
        HashMap<Point, Point> parent = new HashMap<>();
        HashMap<Point, Integer> length = new HashMap<>();
        PriorityQueue<Cmp> q = new PriorityQueue<>(100000);
        q.add(new Cmp(manhattan(start,end),start));
        parent.put(start, start);
        length.put(start, 0);
        Cmp cur = null;
        //System.out.println(q);
        while (q.peek() != null) {
            cur = q.poll();
            System.out.println(cur);
            if (cur.getP() == end) {
                break;
            }
            List<Point> neighbors = potentialNeighbors.apply(cur.getP())
                    .filter(canPassThrough)
                    .filter(p -> !p.equals(start) && !p.equals(end) && !parent.containsKey(p)).collect(Collectors.toList());
            //System.out.println(neighbors);
            for (Point p : neighbors) {
                if ((parent.get(p) == null || (length.get(cur.getP())) < length.get(parent.get(p)))) {
                    parent.put(p, cur.getP());
                    length.put(p, length.get(cur.getP()) + 1);
                    q.add(new Cmp(manhattan(p, end) + length.get(p), p));
                    //System.out.println(q);
                }
            }
        }

        Point p = cur.getP();
        if (p == end){return pth;}
        pth.add(0, p);
            while (parent.get(p) != p) {
                p = parent.get(p);
                pth.add(0, p);
            }

            return pth;
        }
}