package engine.gameManager;

import engine.math.Vector3fImmutable;
import engine.objects.*;

import java.util.ArrayList;


public class NavigationManager {

    private static final int cellGap = 1;
    private static final int stepHeight = 2;

    public static void pathfind(AbstractCharacter character, Vector3fImmutable goal){
        //try {
        //    ArrayList<Vector3fImmutable> path = getOptimizedPath(getPath(character.loc, goal), getPath(goal, character.loc));
        //    if (path.isEmpty()) {
        //        character.destination = goal;
        //        return; //no points to walk to
        //    }

        //    character.destination = path.get(0);

        //} catch(Exception e){
            //something failed
        //}
    }

    public static ArrayList<Vector3fImmutable> getOptimizedPath(ArrayList<Vector3fImmutable> startToGoal, ArrayList<Vector3fImmutable> goalToStart) {
        ArrayList<Vector3fImmutable> optimalPath = new ArrayList<>();
        optimalPath.add(startToGoal.get(0));
        for(Vector3fImmutable point : startToGoal)
        {
            if(!goalToStart.contains(point))
            {
                continue;
            }
            optimalPath.add(point);
        }

        //optimize the path to its smallest possible amount of points



        return optimalPath;
    }

    private static ArrayList<Vector3fImmutable> getPath(Vector3fImmutable start, Vector3fImmutable goal) {
        ArrayList<Vector3fImmutable> path = new ArrayList<>();
        path.add(start);
        Vector3fImmutable current = start;
        boolean obstructed = false;
        int count = 0;
        while (current.distanceSquared(goal) > 9 && count < 250)
        {
            //gather the 8 cells around the player
            ArrayList<Vector3fImmutable> surroundingCells = new ArrayList<>();
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, 0)));
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(0, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, 0)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, -cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(0, 0, -cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(-cellGap, 0, cellGap)));
            surroundingCells.add(current.add(new Vector3fImmutable(cellGap, 0, -cellGap)));
            Vector3fImmutable cheapest = new Vector3fImmutable(Vector3fImmutable.ZERO);
            for (Vector3fImmutable point : surroundingCells)
            {
                count++;

                if (path.contains(point))
                    continue;

                if (pointIsBlocked(point)) {
                    obstructed = true;
                    continue;
                }

                if (getCost(cheapest, current, goal) > getCost(point, current, goal))
                    cheapest = point;
            }
            current = cheapest;
            path.add(cheapest);
        }
        if(obstructed) {
            return path;
        }else {
            ArrayList<Vector3fImmutable> goalPath = new ArrayList<>();
            goalPath.add(start);
            goalPath.add(goal);
            return goalPath; //if the path isn't obstructed we can walk directly from start to the goal
        }
    }

    public static float getCost(Vector3fImmutable point, Vector3fImmutable start, Vector3fImmutable goal) {
        float gCost = start.distanceSquared(point);
        float hCost = goal.distanceSquared(point);
        return gCost + hCost;
    }

    public static boolean pointIsBlocked(Vector3fImmutable point) {

        Building building = BuildingManager.getBuildingAtLocation(point);
        if(building != null)
            for(Regions region : building.getBounds().getRegions())
                if(region.isPointInPolygon(point))
                    if(Math.abs(region.lerpY(point) - point.y) > stepHeight) // get the height distance between current height and target location height
                        return true;

        Zone currentZone = ZoneManager.findSmallestZone(point);
        if(currentZone == null)
            return false;
        else
            return currentZone.navMesh.contains(point.x,point.z);
    }
}