import java.util.*;
import java.util.function.BiPredicate;
import java.util.function.Function;
import java.util.function.Predicate;
import java.util.stream.Stream;

public class AStarPathingStrategy implements PathingStrategy {

    /**
     * Return a list containing a single point representing the next step toward a goal
     * If the start is within reach of the goal, the returned list is empty.
     *
     * @param start the point to begin the search from
     * @param end the point to search for a point within reach of
     * @param canPassThrough a function that returns true if the given point is traversable
     * @param withinReach a function that returns true if both points are within reach of each other
     * @param potentialNeighbors a function that returns the neighbors of a given point, as a stream
     */
    public List<Point> computePath(
            Point start,
            Point end,
            Predicate<Point> canPassThrough,
            BiPredicate<Point, Point> withinReach,
            Function<Point, Stream<Point>> potentialNeighbors
    )

    {
        List<Point> openSet =  new ArrayList<>();
        Map<Point, Point> cameFrom = new HashMap<>();
        Map<Point, Integer> gscore = new HashMap<>();
        Map<Point, Integer> fscore = new HashMap<>();
        Set<Point> closedSet = new HashSet<>();


        gscore.put(start, 0);
        openSet.add(start);

        while(!openSet.isEmpty()) {
            Point current = openSet.get(0);
            int bestFScore = gscore.get(current) + manhattan(current, end);

            for (Point p : openSet){
                int pfscore = gscore.get(p) + manhattan(p, end);
                if (pfscore < bestFScore){
                    bestFScore = pfscore;
                    current = p;
                }
            }

            openSet.remove(current);

            if (withinReach.test(current, end)){
                return reconstructPath(cameFrom, current);
            }

            closedSet.add(current);

            List<Point> neighbors = potentialNeighbors.apply(current)
                    .filter(canPassThrough)
                    .filter(p -> !closedSet.contains(p))
                    .toList();

            for (Point neighbor : neighbors){
                int NGScore = gscore.get(current) + 1;
                int NFScore = NGScore + manhattan(neighbor, end);

                if (NGScore < gscore.getOrDefault(neighbor, Integer.MAX_VALUE)) {
                    cameFrom.put(neighbor, current);
                    gscore.put(neighbor, NGScore);

                    if (!openSet.contains(neighbor)) {
                        openSet.add(neighbor);
                    }
                }

            }
        }

        return List.of();
    }


    private int manhattan(Point p1, Point p2){
        int xdist = Math.abs(p1.x- p2.x);
        int ydist = Math.abs(p1.y- p2.y);
        return xdist + ydist;
    }

    private List<Point> reconstructPath(Map<Point, Point> cameFrom, Point current){
        List <Point> path = new LinkedList<>();

        while (cameFrom.containsKey(current)){
            path.add(0, current);
            current = cameFrom.get(current);
        }

        return path;
    }
}


