package electrosphere.server.pathfinding;

import electrosphere.entity.Entity;
import electrosphere.entity.EntityCreationUtils;
import electrosphere.entity.EntityUtils;
import electrosphere.server.pathfinding.navmesh.NavCube;
import electrosphere.server.pathfinding.navmesh.NavMesh;
import electrosphere.server.pathfinding.navmesh.NavShape;
import electrosphere.server.pathfinding.path.Waypoint;

import java.awt.geom.Line2D;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import org.joml.Vector3d;

/**
 * Navmesh pathfinding functions
 */
public class NavMeshPathfinder {
    
    //TODO: add movement type mask to this function
    public static List<Waypoint> navigatePointToPointInMesh(NavMesh mesh, Vector3d start, Vector3d end){
        
        List<Waypoint> rVal = new LinkedList<Waypoint>();
        
        NavShape startNode = null;
        NavShape endNode = null;
        
        for(NavShape node : mesh.getNodes()){
            if(node.containsPoint(start.x, start.y, start.z)){
                startNode = node;
            }
            if(node.containsPoint(end.x, end.y, end.z)){
                endNode = node;
            }
            if(startNode != null && endNode != null){
                break;
            }
        }
        //if the start and end nodes aren't both present, exit
        //need to do broadphase still
        if(startNode == null || endNode == null){
            return null;
        }
        
        //theta *
        List<NavShape> openSet = new LinkedList<NavShape>();
        List<NavShape> closedSet = new LinkedList<NavShape>();
        PriorityQueue<SetItem> pathItemsQueue = new PriorityQueue<SetItem>();
        Map<NavShape,SetItem> pathItems = new HashMap<NavShape,SetItem>();
        
        openSet.add(startNode);
        SetItem startSetItem = new SetItem(null,startNode,0,start);
        startSetItem.currentPos = start;
        pathItems.put(startNode, startSetItem);
        pathItemsQueue.add(startSetItem);
        
        while(!openSet.isEmpty()){
            //get lowest cost item
            SetItem currentItem = pathItemsQueue.poll();
            NavShape currentNode = currentItem.getNode();
            openSet.remove(currentNode);
            
            //return path if found end
            if(currentNode == endNode){
                // System.out.println("Found path!");
                //TODO: return path
                while(currentItem != null){
                    if(currentItem.node == endNode){
                        Entity waypoint = EntityCreationUtils.createClientSpatialEntity();
                        EntityCreationUtils.makeEntityDrawable(waypoint, "Models/waypoint1.fbx");
                        // System.out.println(end);
                        EntityUtils.getPosition(waypoint).set(end.x,end.y + 1,end.z);
                        EntityUtils.getRotation(waypoint).rotateLocalX(-(float)Math.PI/2.0f);
                    } else {
                        Entity waypoint = EntityCreationUtils.createClientSpatialEntity();
                        EntityCreationUtils.makeEntityDrawable(waypoint, "Models/waypoint1.fbx");
                        // System.out.println(currentItem.currentPos);
                        EntityUtils.getPosition(waypoint).set(currentItem.currentPos.x,currentItem.currentPos.y + 1,currentItem.currentPos.z);
                        EntityUtils.getRotation(waypoint).rotateLocalX(-(float)Math.PI/2.0f);
                    }
                    currentItem = currentItem.parent;
                }
                break;
            }
            closedSet.add(currentNode);
            for(NavShape neighbor : currentNode.getNodeNeighbors()){
                if(!closedSet.contains(neighbor)){
                    if(!openSet.contains(neighbor)){
                        Vector3d centerPoint = calculateCenterOfShape(neighbor);
                        SetItem newSetItem = new SetItem(currentItem,neighbor,currentItem.getCost() + (float)currentItem.currentPos.distance(centerPoint),centerPoint);
                        pathItems.put(neighbor, newSetItem);
                        pathItemsQueue.add(newSetItem);
                        openSet.add(neighbor);
                    }
                    //update vertex
                    updateVertices(currentItem, pathItems.get(neighbor),pathItemsQueue);
                }
            }
        }
        return null;
    }
    
    static void updateVertices(SetItem current, SetItem neighbor, PriorityQueue<SetItem> queue){
        // This part of the algorithm is the main difference between A* and Theta*
        if(current.getParent() != null){
            LOSResult losResult = null;
            Vector3d neighborCenter = calculateCenterOfShape(neighbor.node);
            if(current.getParent().currentPos == null){
                current.getParent().currentPos = calculateCenterOfShape(current.getParent().node);
            }
            Vector3d parentPos = current.getParent().currentPos;
            if((losResult = lineOfSight(current.getParent(), current, neighbor))==null){
//                Vector3d neighborCenter = calculateCenterOfShape(neighbor.node);
//                Vector3d parentPos = current.getParent().currentPos;
                float dist = (float)neighborCenter.distance(parentPos);
                // If there is line-of-sight between parent(s) and neighbor
                // then ignore s and use the path from parent(s) to neighbor 
                if(current.getParent().cost + dist < neighbor.cost){
                    // c(s, neighbor) is the Euclidean distance from s to neighbor
                    neighbor.cost = current.getParent().cost + dist;
                    neighbor.parent = current.getParent();

                    //update prio q
                    queue.remove(neighbor);
                    queue.add(neighbor);
//                    if(neighbor in open){
//                        open.remove(neighbor);
//                    }
//                    open.insert(neighbor, gScore(neighbor) + heuristic(neighbor));
                }
            } else {
                //find midpoint
//                Vector3d neighborCenter = calculateCenterOfShape(neighbor.node);
//                Vector3d parentPos = current.getParent().currentPos;
                if(losResult.hit){
                    float distToMidpoint = (float)neighborCenter.distance(losResult.currentPos);
                    float distToParent = (float)losResult.currentPos.distance(losResult.parentPos);
                    float dist = distToMidpoint + distToParent;
                    // If the length of the path from start to s and from s to 
                    // neighbor is shorter than the shortest currently known distance
                    // from start to neighbor, then update node with the new distance
                    if(current.cost + dist < neighbor.cost){
                        neighbor.cost = current.cost + dist;
                        neighbor.parent = current;

                        //update prio q
                        queue.remove(neighbor);
                        queue.add(neighbor);

                        current.getParent().currentPos = losResult.parentPos;
                        current.currentPos = losResult.currentPos;

                    }
                } else {
                    float dist = (float)neighborCenter.distance(parentPos);
                    // If there is line-of-sight between parent(s) and neighbor
                    // then ignore s and use the path from parent(s) to neighbor 
                    if(current.getParent().cost + dist < neighbor.cost){
                        // c(s, neighbor) is the Euclidean distance from s to neighbor
                        neighbor.cost = current.getParent().cost + dist;
                        neighbor.parent = current.getParent();

                        //update prio q
                        queue.remove(neighbor);
                        queue.add(neighbor);
                    }
                }
            }
        }
    }
    
    static class LOSResult{
        Vector3d parentPos;
        Vector3d currentPos;
        boolean hit;
    }
    
    //calculates the line of sight ACROSS current FROM PARENT TO NEIGHBOR
    static LOSResult lineOfSight(SetItem parent, SetItem current, SetItem neighbor){
        if(parent.node instanceof NavCube && current.node instanceof NavCube && neighbor.node instanceof NavCube){
            //get points on border of current and neighbor, this is one of the lines
            double borderMinX = 0;
            double borderMinZ = 0;
            double borderMaxX = 0;
            double borderMaxZ = 0;
            NavCube parentCube = (NavCube)parent.node;
            NavCube currentCube = (NavCube)current.node;
            NavCube neighborCube = (NavCube)neighbor.node;
            //resolve min/max pos
//            Vector3d currentMin = currentCube.getMinPoint();
//            Vector3d currentMax = currentCube.getMaxPoint();
//            Vector3d neighborMin = neighborCube.getMinPoint();
//            Vector3d neighborMax = neighborCube.getMaxPoint();
            
            BoxInternalBorder parentCurrentBorder = getBoxInternalBorder(parentCube.getMinPoint(),parentCube.getMaxPoint(),currentCube.getMinPoint(),currentCube.getMaxPoint());
            BoxInternalBorder currentChildBorder = getBoxInternalBorder(currentCube.getMinPoint(),currentCube.getMaxPoint(),neighborCube.getMinPoint(),neighborCube.getMaxPoint());
            
            boolean intersectsParentBorder = Line2D.linesIntersect(
                    parentCurrentBorder.minPoint.x, parentCurrentBorder.minPoint.z,
                    parentCurrentBorder.maxPoint.x, parentCurrentBorder.maxPoint.z,
                    neighbor.currentPos.x, neighbor.currentPos.z,
                    parent.currentPos.x, parent.currentPos.z
            );
            boolean intersectsChildBorder = Line2D.linesIntersect(
                    currentChildBorder.minPoint.x, currentChildBorder.minPoint.z,
                    currentChildBorder.maxPoint.x, currentChildBorder.maxPoint.z,
                    neighbor.currentPos.x, neighbor.currentPos.z,
                    parent.currentPos.x, parent.currentPos.z
            );
            
            LOSResult rVal = new LOSResult();
            
            if(intersectsParentBorder && intersectsChildBorder){
                rVal.hit = false;
            } else {
                rVal.hit = true;
//                rVal.currentPos = new Vector3d();
                if(neighbor.currentPos.distance(currentChildBorder.minPoint) < neighbor.currentPos.distance(currentChildBorder.maxPoint)){
                    rVal.currentPos = currentChildBorder.minPoint;
                } else {
                    rVal.currentPos = currentChildBorder.maxPoint;
                }
                if(parent.currentPos.distance(parentCurrentBorder.minPoint) < parent.currentPos.distance(parentCurrentBorder.maxPoint)){
                    rVal.parentPos = parentCurrentBorder.minPoint;
                } else {
                    rVal.parentPos = parentCurrentBorder.maxPoint;
                }
            }
            
            return rVal;
            
            //the other line comes from earlier
//            if(Line2D.linesIntersect(borderMinX, borderMinZ, borderMaxX, borderMaxZ, rayStart.x, rayStart.z, rayEnd.x, rayEnd.z)){
//                return null;
//            } else {
//                double distMin = Line2D.ptLineDist(rayStart.x, rayStart.z, rayEnd.x, rayEnd.z, borderMinX, borderMinZ);
//                double distMax = Line2D.ptLineDist(rayStart.x, rayStart.z, rayEnd.x, rayEnd.z, borderMaxX, borderMaxZ);
//                if(distMin < distMax){
//                    return new Vector3d(borderMinX, 0, borderMinZ);
//                } else {
//                    return new Vector3d(borderMaxX, 0, borderMaxZ);
//                }
//            }
        }
        return null;
    }
    
    static class BoxInternalBorder {
        Vector3d minPoint = new Vector3d();
        Vector3d maxPoint = new Vector3d();
    }
    
    static BoxInternalBorder getBoxInternalBorder(Vector3d currentMin, Vector3d currentMax, Vector3d neighborMin, Vector3d neighborMax){
        BoxInternalBorder rVal = new BoxInternalBorder();
        if(currentMin.x == neighborMax.x){
            double targetX = currentMin.x;
            if(currentMin.z >= neighborMin.z && currentMax.z <= neighborMax.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            } else if(neighborMin.z >= currentMin.z && neighborMax.z <= currentMax.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = neighborMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = neighborMax.z;
            } else if(currentMin.z >= neighborMin.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = neighborMax.z;
            } else if(neighborMin.z >= currentMin.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = neighborMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            } else {
                //they all line up or something
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            }
        }
        if(currentMax.x == neighborMin.x){
            double targetX = currentMax.x;
            if(currentMin.z >= neighborMin.z && currentMax.z <= neighborMax.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            } else if(neighborMin.z >= currentMin.z && neighborMax.z <= currentMax.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = neighborMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = neighborMax.z;
            } else if(currentMin.z >= neighborMin.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = neighborMax.z;
            } else if(neighborMin.z >= currentMin.z){
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = neighborMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            } else {
                //they all line up or something
                rVal.minPoint.x = targetX;
                rVal.minPoint.z = currentMin.z;
                rVal.maxPoint.x = targetX;
                rVal.maxPoint.z = currentMax.z;
            }
        }
        if(currentMin.z == neighborMax.z){
            double targetZ = currentMin.x;
            if(currentMin.x >= neighborMin.x && currentMax.x <= neighborMax.x){
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(neighborMin.x >= currentMin.x && neighborMax.x <= currentMax.x){
                rVal.minPoint.x = neighborMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = neighborMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(currentMin.x >= neighborMin.x){
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = neighborMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(neighborMin.x >= currentMin.x){
                rVal.minPoint.x = neighborMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            } else {
                //they all line up or something
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            }
        }
        if(currentMax.z == neighborMin.z){
            double targetZ = currentMax.x;
            if(currentMin.x >= neighborMin.x && currentMax.x <= neighborMax.x){
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(neighborMin.x >= currentMin.x && neighborMax.x <= currentMax.x){
                rVal.minPoint.x = neighborMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = neighborMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(currentMin.x >= neighborMin.x){
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = neighborMax.x;
                rVal.maxPoint.z = targetZ;
            } else if(neighborMin.x >= currentMin.x){
                rVal.minPoint.x = neighborMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            } else {
                //they all line up or something
                rVal.minPoint.x = currentMin.x;
                rVal.minPoint.z = targetZ;
                rVal.maxPoint.x = currentMax.x;
                rVal.maxPoint.z = targetZ;
            }
        }
        return rVal;
    }
    
//    static float lineDistCalc(SetItem current, SetItem target){
//        float rVal = -1.0f;
//        return rVal;
//    }
    
    static Vector3d calculateCenterOfShape(NavShape shape){
        Vector3d rVal = new Vector3d();
        if(shape instanceof NavCube){
            NavCube cube = (NavCube)shape;
            rVal.add(cube.getMaxPoint()).add(cube.getMinPoint());
            rVal.mul(0.5);
        }
        return rVal;
    }
    
    
    
    static class SetItem implements Comparable<SetItem> {
        SetItem parent;
        NavShape node;
        float cost;
        //closest point from parent's parent
        Vector3d currentPos;
        
        public SetItem(SetItem parent, NavShape node, float cost, Vector3d currentPos){
            this.parent = parent;
            this.node = node;
            this.cost = cost;
            this.currentPos = currentPos;
        }

        public SetItem getParent() {
            return parent;
        }

        public NavShape getNode() {
            return node;
        }

        public float getCost() {
            return cost;
        }

        public void setCost(float cost) {
            this.cost = cost;
        }

        @Override
        public int compareTo(SetItem target) {
            if(this.cost < target.cost){
                return -1;
            }
            if(this.cost > target.cost){
                return 1;
            }
            return 0;
        }

        public Vector3d getCurrentPos() {
            return currentPos;
        }

        public void setCurrentPos(Vector3d currentPos) {
            this.currentPos = currentPos;
        }
        
        
        
        
    }
}
