@Override
    public Plan findPath(SearchDomain map, State sNode, State tNode) {
        // Date inTime = Calendar.getInstance().getTime();
        // If the destination is not traversible, there can be no path. Same applies to the start
        // node.
        if (map.isBlocked(sNode) || map.isBlocked(tNode)) {
            // Date outTime = Calendar.getInstance().getTime();
            // System.out.println("Time Taken: ASTAR: " + (outTime.getTime() - inTime.getTime()));
            return null;
        }
        
        /**
         * Initial state for A*. The closed list is empty. Only the starting
         * node is in the open list and its cost is zero, i.e. we're already
         * there
         */
        initialise();
        // Create and initialise SearchNodes for start and end node.
        SearchNode sSNode = newSearchNode(sNode);
        SearchNode tSNode = newSearchNode(tNode);
        
        // Annotate the start node and put it in the open list.
        sSNode.set(G, 0);
        sSNode.set(H, m_heuristics.h(map, sNode, tNode)); // Heuristics is h
        sSNode.set(F, sSNode.get(G) + sSNode.get(H)); // Total f-value = g + h
        m_openList.add(sSNode);
        
        tSNode.setParent(null);
        
        // Until the target node is reached
        while (m_openList.size() != 0) {
            // Pull out the top Node from the open list, which is most likely to
            // be the next step based on the heuristics.
            SearchNode currentSNode = m_openList.peek();
            State currentNode = currentSNode.getNode();
            
            // Terminate the search if the target node was found, else continue
            // searching
            if (currentSNode == null || currentSNode.equals(tSNode)) {
                break;
            }
            
            m_openList.remove(currentSNode);
            m_closedListHashMap.put(currentNode, currentSNode);
            m_expandedNodes.add(currentNode);
            
            // Now begin searching through all the successors of the current
            // node and evaluate them as candidates for next step, putting them
            // all in the Open List if they have not been visited already.
            ArrayList<State> neighbours = map.getSuccessors(currentSNode.getNode());
            for (State neighbourNode : neighbours) {
                SearchNode neighbourSNode = newSearchNode(neighbourNode);
                float nextStepG = currentSNode.get(G) + map.cost(currentNode, neighbourNode);
                
                if (nextStepG < neighbourSNode.get(G)) {
                    // There's a better way
                    neighbourSNode.setParent(null);
                    m_closedListHashMap.remove(neighbourNode);
                    m_openList.remove(neighbourSNode);
                    
                    // Process the node
                    neighbourSNode.set(G, nextStepG);
                    neighbourSNode.set(H, m_heuristics.h(map, neighbourNode, tNode));
                    neighbourSNode.set(F, nextStepG + Weight*neighbourSNode.get(H));
                    // if (neighbourSNode.get(F) != Float.POSITIVE_INFINITY) {
                    neighbourSNode.setParent(currentSNode);
                    // }
                    m_openList.add(neighbourSNode);
                }
            } // END foreach Neighbours
            
        } // END while open != EMPTY
        
        // If there was no path found to target, return null
        if (tSNode.getParent() == null) {
            // Date outTime = Calendar.getInstance().getTime();
            // System.out.println("Time Taken: ASTAR: " + (outTime.getTime() - inTime.getTime()));
            return null;
        }
        
        // At this point, a path was defintely found, which means we need to
        // create a Path by traversing through the parent pointers
        ComputedPlan path = new ComputedPlan();
        
        SearchNode target = tSNode;
        while (target != sSNode) {
            path.prependStep(target.getNode());
            target = target.getParent();
        }
        path.prependStep(sNode);
        path.setCost(tSNode.get(G));
        
        // System.gc(); // Free up lost and unused memory
        
        // Date outTime = Calendar.getInstance().getTime();
        // System.out.println("Time Taken: ASTAR: " + (outTime.getTime() - inTime.getTime())); //
        // SOP TimeTaken
        return path;
    }