package electrosphere.server.pathfinding;

import electrosphere.engine.Globals;
import electrosphere.logger.LoggerInterface;
import electrosphere.server.pathfinding.blocker.NavBlocker;
import electrosphere.server.pathfinding.navmesh.NavCube;
import electrosphere.server.pathfinding.navmesh.NavMesh;
import electrosphere.server.pathfinding.navmesh.NavShape;
import electrosphere.server.terrain.manager.ServerTerrainChunk;

import java.util.LinkedList;
import java.util.List;
import org.joml.Vector2i;
import org.joml.Vector4i;

/**
 *
 * @author satellite
 */
public class NavMeshUtils {
    
    static float NAVMESH_PHASE_ONE_DISPARITY_TOLERANCE = 0.5f;
    static float NAVMESH_PHASE_TWO_DISPARITY_TOLERANCE = 0.5f;
    public static NavMesh createMeshFromChunk(ServerTerrainChunk chunk, NavBlocker navBlocker){
        NavMesh rVal = Globals.navMeshManager.createNavMesh();
        float[][] heightMap = chunk.getWeights()[0];
        boolean[][] navMeshGeneratorMask = navBlocker.getHeightfieldBlocker();
        List<FirstPhaseBox> firstPassBoxes = new LinkedList<FirstPhaseBox>();
        int numInCurrent = 0;
        float currentMin = 0;
        float currentMax = 0;
        int startPos = 0;
        int endPos = 0;
        for(int x = 0; x < Globals.serverTerrainManager.getAugmentedChunkWidth() - 1; x++){
            numInCurrent = 0;
            currentMin = 0;
            currentMax = 0;
            for(int y = 0; y < Globals.serverTerrainManager.getAugmentedChunkWidth(); y++){
                //create node
                if(navMeshGeneratorMask[x][y]){
                    if(numInCurrent > 0){
                        firstPassBoxes.add(new FirstPhaseBox(x,startPos,endPos,currentMin,currentMax));
                    }
                    numInCurrent = 0;
                } else {
                    if(numInCurrent == 0){
                        currentMin = heightMap[x][y];
                        currentMax = heightMap[x][y];
                        startPos = y;
                        endPos = y+1;
                        numInCurrent = 1;
                    } else {
                        if(currentMin > heightMap[x][y]){
                            if(currentMax - heightMap[x][y] < NAVMESH_PHASE_ONE_DISPARITY_TOLERANCE){
                                currentMin = heightMap[x][y];
                                endPos = y;
                                numInCurrent++;
                            } else {
                                //expel previous rectangle
                                firstPassBoxes.add(new FirstPhaseBox(x,startPos,endPos,currentMin,currentMax));
                                //start new one
                                currentMin = heightMap[x][y];
                                currentMax = heightMap[x][y];
                                startPos = y;
                                endPos = y+1;
                                numInCurrent = 1;
                            }
                        } else if(currentMax < heightMap[x][y]){
                            if(heightMap[x][y] - currentMin < NAVMESH_PHASE_ONE_DISPARITY_TOLERANCE){
                                currentMin = heightMap[x][y];
                                endPos = y;
                                numInCurrent++;
                            } else {
                                //expel previous rectangle
                                firstPassBoxes.add(new FirstPhaseBox(x,startPos,endPos,currentMin,currentMax));
                                //start new one
                                currentMin = heightMap[x][y];
                                currentMax = heightMap[x][y];
                                startPos = y;
                                endPos = y+1;
                                numInCurrent = 1;
                            }
                        } else {
                            endPos = y;
                            numInCurrent++;
                        }
                    }
                }
//                if(x > 0){
//                    //add back neighbor
//                }
//                if(y > 0){
//                    //add back neighbor
//                }
            }
            
            //close off last box on row
//            FirstPhaseBox boxy = new FirstPhaseBox(1, 1, 1, 1, 1);
            firstPassBoxes.add(new FirstPhaseBox(x,startPos,endPos,currentMin,currentMax));
            
            
            
        }
        
        //phase two
        //???
        List<SecondPhaseBox> secondPhaseBoxes = new LinkedList<SecondPhaseBox>();
        List<SecondPhaseBox> toRemove = new LinkedList<SecondPhaseBox>();
        for(FirstPhaseBox firstPhaseBox : firstPassBoxes){
            SecondPhaseBox newBox = new SecondPhaseBox(
                    firstPhaseBox.x,firstPhaseBox.yStart,
                    firstPhaseBox.x+1,firstPhaseBox.yEnd,
                    firstPhaseBox.minHeight,firstPhaseBox.maxHeight);
//            System.out.println(
//                    firstPhaseBox.x + " " + 
//                    firstPhaseBox.yStart + " " +
//                    (firstPhaseBox.x+1) + " " +
//                    firstPhaseBox.yEnd + " " +
//                    firstPhaseBox.minHeight + " " +
//                    firstPhaseBox.maxHeight
//            );
//            System.out.println(
//                    "(" + (newBox.boundMinX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth()) + ", " + 
//                    (newBox.minHeight - 0.5f) + ", " + 
//                    (newBox.boundMinY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth()) + ") (" + 
//                    (newBox.boundMaxX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth()) + ", " + 
//                    (newBox.maxHeight + 0.5f) + ", " + 
//                    (newBox.boundMaxY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth()) + ")"
//            );
            toRemove.clear();
            //TODO: iterate this backwards and check for x adjacency, if no adjacency end loop early
            List<SecondPhaseBox> neighborUpdateList = new LinkedList<SecondPhaseBox>();
            for(SecondPhaseBox interim : secondPhaseBoxes){
                if(firstPhaseBox.yStart == interim.boundMinY && firstPhaseBox.yEnd == interim.boundMaxY && firstPhaseBox.x == interim.boundMaxX){
                    for(SecondPhaseBox neighbor : interim.getNeighbors()){
                        neighborUpdateList.add(neighbor);
//                        System.out.println("ADD NEIGHBOR: " + neighbor.boundMaxX + " " + neighbor.boundMaxY + " " + neighbor.boundMinX + " " + neighbor.boundMinY);
                    }
                    for(SecondPhaseBox update : neighborUpdateList){
                        update.removeNeighbor(interim);
                        interim.removeNeighbor(update);
                        update.addNeighbor(newBox);
                        newBox.addNeighbor(update);
                    }
                    toRemove.add(interim);
                    newBox.setBoundMinX(interim.getBoundMinX());
                    //TODO: calculate new min/max height
                } else {
                    //because of the way the rectangles are constructed, we can never have a neighbor along y axis
                    //only neighbors will be behind us
                    if(newBox.boundMinX == interim.boundMaxX && !toRemove.contains(interim)){
                        if(
                                (interim.boundMaxY < newBox.boundMaxY && interim.boundMaxY > newBox.boundMinY) ||
                                (interim.boundMinY < newBox.boundMaxY && interim.boundMinY > newBox.boundMinY) ||
                                (newBox.boundMaxY < interim.boundMaxY && newBox.boundMaxY > interim.boundMinY) ||
                                (newBox.boundMinY < interim.boundMaxY && newBox.boundMinY > interim.boundMinY)
                                ){
//                            System.out.println("ADD INTERIM: " + interim.boundMaxX + " " + interim.boundMaxY + " " + interim.boundMinX + " " + interim.boundMinY);
                            newBox.addNeighbor(interim);
                            interim.addNeighbor(newBox);
                        }
                    }
                }
            }
            secondPhaseBoxes.removeAll(toRemove);
            secondPhaseBoxes.add(newBox);
        }
        
        int id = 0;
        for(SecondPhaseBox box : secondPhaseBoxes){
            box.setId(id);
//            System.out.println("getId:" + box.getId());
            id++;
//            System.out.println(
//                    "(" + (box.boundMinX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth()) + ", " + 
//                    (box.minHeight - 0.5f) + ", " + 
//                    (box.boundMinY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth()) + ") (" + 
//                    (box.boundMaxX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth() - 1) + ", " + 
//                    (box.maxHeight + 0.5f) + ", " + 
//                    (box.boundMaxY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth()) + ")"
//            );
//            System.out.println(box.neighbors.size());
            //why the -1? I think the array fiddling above is causing the bounds to be off normally
            //this fixes that
            NavCube cube = new NavCube(
                    box.boundMinX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth(),
                    box.minHeight - 0.5f,
                    box.boundMinY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth(),
                    box.boundMaxX + chunk.getWorldX() * Globals.serverTerrainManager.getChunkWidth(),
                    box.maxHeight + 0.5f,
                    box.boundMaxY + chunk.getWorldY() * Globals.serverTerrainManager.getChunkWidth()
            );
            rVal.addNode(cube);
        }
        
        id = 0;
        for(NavShape shape : rVal.getNodes()){
            NavCube cube = (NavCube)shape;
            
            SecondPhaseBox currentBox = secondPhaseBoxes.get(id);
//            System.out.println("getId:" + currentBox.getId());
            id++;
            for(SecondPhaseBox neighbor : currentBox.getNeighbors()){
                //TODO: solve bug where items are getting picked up from not in second phase boxes
                if(!secondPhaseBoxes.contains(neighbor)){
                    LoggerInterface.loggerGameLogic.WARNING("Found bad neighbor adjacency in navmesh generation");
                    for(SecondPhaseBox newNeighbor : secondPhaseBoxes){
                        if(newNeighbor.boundMaxX >= neighbor.boundMaxX &&
                                newNeighbor.boundMaxY >= neighbor.boundMaxY &&
                                newNeighbor.boundMinX <= neighbor.boundMinX &&
                                newNeighbor.boundMinY <= neighbor.boundMinY
                                ){
                            cube.addNodeNeighbor(rVal.getNodes().get(newNeighbor.getId()));
                            LoggerInterface.loggerGameLogic.WARNING("Managed to replace bad neighbor");
                            break;
                        }
                    }
//                    System.out.println("ERR!");
//                    System.out.println(neighbor.boundMaxX + " " + neighbor.boundMaxY + " " + neighbor.boundMinX + " " + neighbor.boundMinY);
                } else {
                    cube.addNodeNeighbor(rVal.getNodes().get(neighbor.getId()));
                }
            }
//            System.out.println(cube.getNodeNeighbors().size());
        }
//        System.out.println();
//        System.out.println();
//        System.out.println(secondPhaseBoxes.size());
        
        return rVal;
    }
    
    static class FirstPhaseBox {
        int x;
        int yStart;
        int yEnd;
        float minHeight;
        float maxHeight;

        public FirstPhaseBox(int x, int yStart, int yEnd, float minHeight, float maxHeight) {
            this.x = x;
            this.yStart = yStart;
            this.yEnd = yEnd;
            this.minHeight = minHeight;
            this.maxHeight = maxHeight;
        }

        public int getX() {
            return x;
        }

        public int getyStart() {
            return yStart;
        }

        public int getyEnd() {
            return yEnd;
        }

        public float getMinHeight() {
            return minHeight;
        }

        public float getMaxHeight() {
            return maxHeight;
        }
        
        
        
    }
    
    static class SecondPhaseBox {
        
        float minHeight;
        float maxHeight;
        int boundMinX;
        int boundMinY;
        int boundMaxX;
        int boundMaxY;
        List<SecondPhaseBox> neighbors = new LinkedList<SecondPhaseBox>();
        int id = -1;
        
        SecondPhaseBox(int boundMinX, int boundMinY, int boundMaxX, int boundMaxY, float minHeight, float maxHeight){
            this.boundMinX = boundMinX;
            this.boundMinY = boundMinY;
            
            this.boundMaxX = boundMaxX;
            this.boundMaxY = boundMaxY;
            
            this.minHeight = minHeight;
            this.maxHeight = maxHeight;
        }
        
        float getMinHeight(){
            return minHeight;
        }
        float getMaxHeight(){
            return maxHeight;
        }

        public int getBoundMinX() {
            return boundMinX;
        }

        public int getBoundMinY() {
            return boundMinY;
        }

        public int getBoundMaxX() {
            return boundMaxX;
        }

        public int getBoundMaxY() {
            return boundMaxY;
        }

        public void setMinHeight(float minHeight) {
            this.minHeight = minHeight;
        }

        public void setMaxHeight(float maxHeight) {
            this.maxHeight = maxHeight;
        }

        public void setBoundMinX(int boundMinX) {
            this.boundMinX = boundMinX;
        }

        public void setBoundMinY(int boundMinY) {
            this.boundMinY = boundMinY;
        }

        public void setBoundMaxX(int boundMaxX) {
            this.boundMaxX = boundMaxX;
        }

        public void setBoundMaxY(int boundMaxY) {
            this.boundMaxY = boundMaxY;
        }

        public List<SecondPhaseBox> getNeighbors() {
            return neighbors;
        }
        
        public void addNeighbor(SecondPhaseBox neighbor){
            neighbors.add(neighbor);
        }
        
        public void removeNeighbor(SecondPhaseBox neighbor){
            neighbors.remove(neighbor);
        }

        public int getId() {
            return id;
        }

        public void setId(int id) {
            this.id = id;
        }
        
    }
    
}