Untitled

    private static FloorNode map_Dijkstra_findLowestDistanceNode(HashMap<FloorNode, Integer> distances, Set<FloorNode> unsettledNodes) {
        FloorNode lowestDistanceNode = null;
        int lowestDistance = Integer.MAX_VALUE;
        for (FloorNode node : unsettledNodes) {
            Integer nodeDistance = distances.get(node);
            if (nodeDistance == null) {
                nodeDistance = Integer.MAX_VALUE;
                distances.put(node, nodeDistance);
            }
            if (nodeDistance < lowestDistance) {
                lowestDistance = nodeDistance;
                lowestDistanceNode = node;
            }
        }
        return lowestDistanceNode;
    }

    private static void map_Dijkstra_calculateMinimumDistance(HashMap<FloorNode, Integer> distances,
                                                              HashMap<FloorNode, LinkedList<FloorNode>> shortestPaths, FloorNode evaluationNode, int edgeWeigh,
                                                              FloorNode sourceNode) {
        Integer sourceDistance = distances.get(sourceNode);
        Integer evaluationDistance = distances.get(evaluationNode);

        if (distances == null) {
            sourceDistance = Integer.MAX_VALUE;
            distances.put(sourceNode, sourceDistance);
        }
        if (evaluationDistance == null) {
            evaluationDistance = Integer.MAX_VALUE;
            distances.put(sourceNode, evaluationDistance);
        }

        if ((sourceDistance + edgeWeigh) < evaluationDistance) {
            distances.put(evaluationNode, (sourceDistance + edgeWeigh));
            LinkedList<FloorNode> sourcePathList = shortestPaths.get(sourceNode);
            if (sourcePathList == null) {
                sourcePathList = new LinkedList<>();
                shortestPaths.put(sourceNode, sourcePathList);
            }
            LinkedList<FloorNode> shortestPath = new LinkedList<>(sourcePathList);
            shortestPath.add(sourceNode);
            shortestPaths.put(evaluationNode, shortestPath);
        }
    }


    public static FloorNodeConnection map_Dijkstra_findNextConnectionToDestination(FloorNode from, FloorNode to) {
        if (from == null || to == null || from == to) {
            return null;
        }
        HashMap<FloorNode, Integer> distances = new HashMap<>();
        HashMap<FloorNode, LinkedList<FloorNode>> shortestPaths = new HashMap<>();
        distances.put(from, 0);
        Set<FloorNode> settledNodes = new HashSet<>();
        Set<FloorNode> unsettledNodes = new HashSet<>();
        unsettledNodes.add(from);
        while (unsettledNodes.size() != 0) {
            FloorNode currentNode = map_Dijkstra_findLowestDistanceNode(distances, unsettledNodes);
            unsettledNodes.remove(currentNode);
            for (FloorNodeConnection floorNodeConnection : currentNode.connections) {
                int edgeWeigh = 1;
                if (!settledNodes.contains(floorNodeConnection.destination)) {
                    map_Dijkstra_calculateMinimumDistance(distances, shortestPaths, floorNodeConnection.destination, edgeWeigh, currentNode);
                    unsettledNodes.add(floorNodeConnection.destination);
                }
            }
            settledNodes.add(currentNode);
        }

        // Get Shortest path to destination
        LinkedList<FloorNode> shortestPath = shortestPaths.get(to);
        if(shortestPath == null){
            return null; // no path exists
        }

        // Convert to connections
        ArrayList<FloorNodeConnection> connections = new ArrayList<>();
        FloorNode current = from;
        for (int i = 0; i < shortestPath.size(); i++) {
            FloorNode toFloorNode = shortestPath.get(i);
            for (FloorNodeConnection floorNodeConnection : current.connections) {
                if (floorNodeConnection.destination == toFloorNode || floorNodeConnection.destination == to) {
                    connections.add(floorNodeConnection);
                    current = floorNodeConnection.destination;
                }
            }
        }




        if (connections.size() > 0) {
            if (connections.get(0).transportation.getClass() == Elevator.class) {
                // Bug: Gives same elevator in a row, cant be shortest path
                FloorNodeConnection last = connections.get(0);
                beforeLastLoop:for(int i=0;i<connections.size();i++){
                    FloorNodeConnection lastNew = connections.get(i);
                    if(lastNew.transportation == connections.get(0).transportation){
                        last = lastNew;
                    }else{
                        break beforeLastLoop;
                    }
                }
                return last;
            }else{
                return connections.get(0);
            }
            //return connections.get(0); // Return next connection that has to be used
        } else {
            return null;
        }

    }