/**
   * Add node to search queue
   *
   * @param n Destination node to add
   * @param pn Previous node
   * @param e Edge distance (previous to this)
   */
  void Push(const Node &node, const Node &parent,
            const AStarPriorityValue &edge_value) {
    // Try to find the given node n in the node_value_map
    node_value_iterator it = node_values.find(node);
    if (it == node_values.end()) {
      // first entry
      // If the node wasn't found
      // -> Insert a new node into the node_value_map
      it = node_values.insert(std::make_pair(node, edge_value)).first;

      // Remember the parent node
      SetPredecessor(node, parent);
    } else if (it->second > edge_value) {
      // If the node was found and the new value is smaller
      // -> Replace the value with the new one
      it->second = edge_value;
      // replace, it's bigger

      // Remember the new parent node
      SetPredecessor(node, parent);
    } else
      // If the node was found but the value is higher or equal
      // -> Don't use this new leg
      return;

    q.push(NodeValue(edge_value, it));
  }

  /** 
   * Clears the queues
   */
  void Clear() {
    // Clear the search queue
    q.clear();

    // Clear EdgeMap
    edges.clear();

    current_value = 0;
  }

  /** Clears the queues */
  void Clear() {
    // Clear the search queue
    q.clear();

    // Clear the node_parent_map
    node_parents.clear();
    // Clear the node_value_map
    node_values.clear();
  }

  /**
   * Add node to search queue
   *
   * @param n Destination node to add
   * @param pn Previous node
   * @param e Edge distance (previous to this)
   */
  void push(const Node &node, const Node &parent, unsigned edge_value = 0) {
    // Try to find the given node n in the edge_map
    edge_iterator it = edges.find(node);
    if (it == edges.end())
      // first entry
      // If the node wasn't found
      // -> Insert a new node
      it = edges.insert(std::make_pair(node, Edge(parent, edge_value))).first;
    else if (it->second.value > edge_value)
      // If the node was found and the new value is smaller
      // -> Replace the value with the new one
      it->second = Edge(parent, edge_value);
    else
      // If the node was found but the new value is higher or equal
      // -> Don't use this new leg
      return;

    q.push(Value(edge_value, it));
  }

 /**
  * Reserve queue size (if available)
  */
 void reserve(unsigned size) {
   q.reserve(size);
 }

 /**
  * Return size of queue
  *
  * @return Queue size in elements
  */
 gcc_pure
 unsigned queue_size() const {
   return q.size();
 }

 /**
  * Test whether queue is empty
  *
  * @return True if no more nodes to search
  */
 gcc_pure
 bool empty() const {
   return q.empty();
 }

 /**
  * Return size of queue
  *
  * @return Queue size in elements
  */
 gcc_pure
 unsigned GetQueueSize() const {
   return q.size();
 }