import org.bukkit.Location;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;

public class PathFinder {

    private final Location startLoc;
    private final Location endLoc;
    private final boolean canDiagonal;

    public PathFinder(Location startLoc, Location endLoc, boolean canDiagonal) {
        this.startLoc = startLoc;
        this.endLoc = endLoc;
        this.canDiagonal = canDiagonal;
    }

    public LinkedList<Location> makePath() {
        LinkedList<Location> path = new LinkedList<>();

        Location curLocation = startLoc;
        path.add(curLocation);
        while (!curLocation.equals(endLoc)) {
            List<Location> neighbors = getNeighbors(curLocation);
            Location nearest = getNearest(neighbors, endLoc);
            curLocation = nearest;
            path.add(curLocation);
        }
        return path;
    }

    public Location getNearest(List<Location> locs, Location newLoc) {
        Location nearest = null;
        double nearestDist = Double.MAX_VALUE;
        for (Location loc : locs) {
            double dist = getDist(loc, newLoc);
            if(dist < nearestDist) {
                nearest = loc;
                nearestDist = dist;
            }
        }
        return nearest;
    }

    public double getDist(Location curLoc, Location newLoc) {
        return Math.sqrt(Math.pow((newLoc.getBlockX() - curLoc.getBlockX()),2) + Math.pow((newLoc.getBlockY() - curLoc.getBlockY()),2) + Math.pow((newLoc.getBlockZ() - curLoc.getBlockZ()),2));
    }

    public List<Location> getNeighbors(Location loc) {
        List<Location> neighbors = new ArrayList<>();

        //X
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY(), loc.getBlockZ()));
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY(), loc.getBlockZ()));

        //Y
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() + 1, loc.getBlockZ()));
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() - 1, loc.getBlockZ()));

        //Z
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY(), loc.getBlockZ() + 1));
        neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY(), loc.getBlockZ() - 1));

        if (canDiagonal) {
            //XY
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY() + 1, loc.getBlockZ()));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() - 1, loc.getBlockZ()));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1 , loc.getBlockY() - 1, loc.getBlockZ()));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() + 1, loc.getBlockZ()));

            //ZY
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() + 1, loc.getBlockZ() + 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() - 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() + 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX(), loc.getBlockY() - 1, loc.getBlockZ() + 1));

            //XZ
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY(), loc.getBlockZ() + 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY(), loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY(), loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY(), loc.getBlockZ() + 1));

            //XZY+
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY() + 1, loc.getBlockZ() + 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() + 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY() + 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() + 1, loc.getBlockZ() + 1));

            //XZY-
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY() - 1, loc.getBlockZ() + 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() - 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() + 1, loc.getBlockY() - 1, loc.getBlockZ() - 1));
            neighbors.add(new Location(loc.getWorld(), loc.getBlockX() - 1, loc.getBlockY() - 1, loc.getBlockZ() + 1));
        }
        return neighbors;
    }