std::vector<Node*> m_openList;
m_openList.push_back(startingNode);


int iter = 0;
while (iter < maxIter && !m_openList->empty())
{
    iter++;

    // Remove node from open list and put it in closed list.
    Node* bestNode = m_openList->pop();
    bestNode->flags &= ~NODE_OPEN;
    bestNode->flags |= NODE_CLOSED;

    // Reached the goal, stop searching.
    if (bestNode->id == endNodeId) {
        lastBestNode = bestNode;
        return true;
    }

    // iterate over all neighbor nodes
    for (unsigned int i = 0; i <= bestNode.neighbors.size(); ++i) {
        Node* neighbourNode = bestNode.neighbors[i];
        if (!neighbourNode) {
            continue;
        }

        // Calculate cost and heuristic.
        float cost = 0;
        float heuristic = 0;

        // Special case for last node.
        if (neighbour == endNode) {
            // Cost
            const float curCost = GetCurrentNodeCost();
            const float endCost = GetEndNodeCost();

            cost = bestNode->cost + curCost + endCost;
            heuristic = 0;
        }
        else {
            // Cost
            const float curCost = GetCurrentNodeCost();

            cost = bestNode->cost + curCost;
            heuristic = Vdist(neighbourNode->pos, endNodePosition)*H_SCALE;
        }

        const float total = cost + heuristic;

        // The node is already in open list and the new result is worse, skip.
        if ((neighbourNode->flags & NODE_OPEN) && total >= neighbourNode->total)
            continue;
        // The node is already visited and process, and the new result is worse, skip.
        if ((neighbourNode->flags & NODE_CLOSED) && total >= neighbourNode->total)
            continue;

        // Add or update the node.
        neighbourNode->id = neighbourRef;
        neighbourNode->flags &= ~NODE_CLOSED;
        neighbourNode->cost = cost;
        neighbourNode->total = total;

        if (neighbourNode->flags & NODE_OPEN) {
            // Already in open, update node location.
            m_openList->modify(neighbourNode);
        }
        else {
            // Put the node in open list.
            neighbourNode->flags |= NODE_OPEN;
            m_openList->push(neighbourNode);
        }

        // Update nearest node to target so far.
        if (heuristic < lastBestNodeCost) {
            lastBestNodeCost = heuristic;
            lastBestNode = neighbourNode;
        }
    }
}