private boolean _a_star(Path a_path)
    {
        //Sets of evaluated and not evaluated nodes.
        TreeSet<Integer> evaluatedSet = new TreeSet<Integer>();
        TreeSet<Integer> toEvaluateSetMarker = new TreeSet<Integer>();
        PriorityQueue<Node> toEvaluateSet = new PriorityQueue<Node>(1000,new NodeComparatorH());

        //Initialize current node (origin).
        Node currentNode = m_graph.getNode(a_path.m_originID);
        toEvaluateSet.add(currentNode);
        toEvaluateSetMarker.add(currentNode.id());
        currentNode.m_g = 0;
        currentNode.m_h = heuristic(a_path.m_originID, a_path.m_destinationID);
        currentNode.m_f = currentNode.m_g + currentNode.m_h;

        //Check while there are still nodes in the array of nods to evaluate.
        while(!toEvaluateSet.isEmpty())
        {
            //Take next node to evaluate.
            currentNode = toEvaluateSet.poll();
            int currentNodeId = currentNode.id();
            evaluatedSet.add(currentNode.id());

            //If destination found, that's it.
            if(currentNode.id() == a_path.m_destinationID)
            {
                return true;
            }

            //For all edges from the current node...
            for(Long edgeId:currentNode.getEdgesFromNode())
            {
                //Take the edge.
                Edge edg = m_graph.getEdge(edgeId);
                int connectedID = edg.aId() != currentNodeId ? edg.aId() : edg.bId();
                Node connected = m_graph.getNode(connectedID);

                //If it has not been evaluated yet.
                if(!evaluatedSet.contains(connected.id()))
                {
                    // Cost from origin to 'connected' stored
                    Path D1 = getShortestPath(a_path.m_originID,connectedID);
                    // Cost from origin to current node stored
                    Path DA = getShortestPath(a_path.m_originID,currentNodeId);
                    // Cost from current to connected (edge cost)
                    double dA1 = edg.cost();

                    //Path to this node
                    PathCH pc = new PathCH();
                    pc.p = D1;
                    pc.destID = connectedID;

                    //If the new cost is smaller.
                    double newCost = DA.m_cost + dA1;
                    if(D1.m_cost > newCost)
                    {
                        //update cost
                        Path newD1 = new Path(DA);
                        newD1.m_destinationID = connectedID;
                        newD1.m_cost += dA1;
                        //update path
                        pc.p = newD1;
                        newD1.m_points.add(connectedID);
                        setShortestPath(newD1);
                    }

                    //Set cost, used by priority queue to navigate more efficiently
                    connected.m_g = pc.p.m_cost;
                    connected.m_f = pc.heuristicCost = pc.p.m_cost + heuristic(connectedID, a_path.m_destinationID);
                    if(!toEvaluateSetMarker.contains(connected.id()))
                    {
                        //Mark this node as evaluated.
                        toEvaluateSet.add(connected);
                        toEvaluateSetMarker.add(connected.id());
                    }

                }
            }
        }

        //If we didn't find the destination, the path could not be extracted.
        return false;
    }