/////////////////////////////////////////////////////////////////////////////
// Original work:
// (C) Copyright Ion Gaztanaga 2007
// Distributed under the Boost Software License, Version 1.0.
//    (See THIRD_PARTY_LICENSES/BOOST_LICENSE_1_0 or
//     http://www.boost.org/LICENSE_1_0.txt)
//
// This file may contain modifications by DBZero Software sp. z o.o.
// Any modifications are Copyright (c) 2025 DBZero Software sp. z o.o.
// and licensed under LGPL-2.1.
//
// SPDX-License-Identifier: BSL-1.0 AND LGPL-2.1
/////////////////////////////////////////////////////////////////////////////
	
#pragma once

#include "config_begin.hpp"
#include "../intrusive_fwd.hpp"
#include "utilities.hpp"

#include <cstddef>
#include <list>

namespace intrusive {
namespace detail {

//!   This is an implementation of a binary search tree.
//!   A node in the search tree has references to its children and its parent. This
//!   is to allow traversal of the whole tree from a given node making the
//!   implementation of iterator a pointer to a node.
//!   At the top of the tree a node is used specially. This node's parent pointer
//!   is pointing to the root of the tree. Its left pointer points to the
//!   leftmost node in the tree and the right pointer to the rightmost one.
//!   This node is used to represent the end-iterator.
//!
//!                                            +---------+
//!       header------------------------------>|         |
//!                                            |         |
//!                   +----------(left)--------|         |--------(right)---------+
//!                   |                        +---------+                        |
//!                   |                             |                             |
//!                   |                             | (parent)                    |
//!                   |                             |                             |
//!                   |                             |                             |
//!                   |                        +---------+                        |
//!    root of tree ..|......................> |         |                        |
//!                   |                        |    D    |                        |
//!                   |                        |         |                        |
//!                   |                +-------+---------+-------+                |
//!                   |                |                         |                |
//!                   |                |                         |                |
//!                   |                |                         |                |
//!                   |                |                         |                |
//!                   |                |                         |                |
//!                   |          +---------+                 +---------+          |
//!                   |          |         |                 |         |          |
//!                   |          |    B    |                 |    F    |          |
//!                   |          |         |                 |         |          |
//!                   |       +--+---------+--+           +--+---------+--+       |
//!                   |       |               |           |               |       |
//!                   |       |               |           |               |       |
//!                   |       |               |           |               |       |
//!                   |   +---+-----+   +-----+---+   +---+-----+   +-----+---+   |
//!                   +-->|         |   |         |   |         |   |         |<--+
//!                       |    A    |   |    C    |   |    E    |   |    G    |
//!                       |         |   |         |   |         |   |         |
//!                       +---------+   +---------+   +---------+   +---------+
//!

//! tree_algorithms is configured with a NodeTraits class, which encapsulates the
//! information about the node to be manipulated. NodeTraits must support the
//! following interface:
//!
//! <b>Typedefs</b>:
//!
//! <tt>node</tt>: The type of the node that forms the circular list
//!
//! <tt>node_ptr</tt>: A pointer to a node
//!
//! <tt>const_node_ptr</tt>: A pointer to a const node
//!
//! <b>Static functions</b>:
//!
//! <tt>static node_ptr get_parent(const_node_ptr n);</tt>
//!
//! <tt>static void set_parent(node_ptr n, node_ptr parent);</tt>
//!
//! <tt>static node_ptr get_left(const_node_ptr n);</tt>
//!
//! <tt>static void set_left(node_ptr n, node_ptr left);</tt>
//!
//! <tt>static node_ptr get_right(const_node_ptr n);</tt>
//!
//! <tt>static void set_right(node_ptr n, node_ptr right);</tt>
template<class NodeTraits>
class tree_algorithms
{
   /// @cond
   private:
   typedef typename NodeTraits::node            node;
   /// @endcond

   public:
   typedef NodeTraits                           node_traits;
   typedef typename NodeTraits::node_ptr        node_ptr;
   typedef typename NodeTraits::const_node_ptr  const_node_ptr;

   struct link_data
   {
	   node_ptr y;
	   bool link_left;
   };

   //! This type is the information that will be filled by insert_unique_check
   struct insert_commit_data
   {
      insert_commit_data()
         :  link_left(false)
         ,  node(node_traits::get_null())
      {}
      bool     link_left;
      node_ptr node;
   };

   struct nop_erase_fixup
   {
      void operator()(node_ptr, node_ptr){}
   };

   /// @cond
   private:
   /// @endcond

   public:
   static node_ptr begin_node(const node_ptr &header)
   {  return node_traits::get_left(header);   }

   static node_ptr end_node(const node_ptr &header)
   {  return header;   }

   //! <b>Requires</b>: node is a node of the tree or an node initialized
   //!   by init(...) or init_node.
   //!
   //! <b>Effects</b>: Returns true if the node is initialized by init() or init_node().
   //!
   //! <b>Complexity</b>: Constant time.
   //!
   //! <b>Throws</b>: Nothing.
   static bool unique(const node_ptr &node)
   { return NodeTraits::get_parent(node) == node_traits::get_null(); }

   static node_ptr get_header(const node_ptr &node)
   {
      node_ptr h = node;
      if(NodeTraits::get_parent(node)){
         h = NodeTraits::get_parent(node);
         while(!is_header(h))
            h = NodeTraits::get_parent(h);
      }
      return h;
   }

   //! <b>Requires</b>: node1 and node2 can't be header nodes
   //!  of two trees.
   //!
   //! <b>Effects</b>: Swaps two nodes. After the function node1 will be inserted
   //!   in the position node2 before the function. node2 will be inserted in the
   //!   position node1 had before the function.
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Note</b>: This function will break container ordering invariants if
   //!   node1 and node2 are not equivalent according to the ordering rules.
   //!
   //!Experimental function
   static void swap_nodes(node_ptr node1, node_ptr node2)
   {
      if(node1 == node2)
         return;

      node_ptr header1(get_header(node1)), header2(get_header(node2));
      swap_nodes(node1, header1, node2, header2);
   }

   //! <b>Requires</b>: node1 and node2 can't be header nodes
   //!  of two trees with header header1 and header2.
   //!
   //! <b>Effects</b>: Swaps two nodes. After the function node1 will be inserted
   //!   in the position node2 before the function. node2 will be inserted in the
   //!   position node1 had before the function.
   //!
   //! <b>Complexity</b>: Constant.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Note</b>: This function will break container ordering invariants if
   //!   node1 and node2 are not equivalent according to the ordering rules.
   //!
   //!Experimental function
   static void swap_nodes(node_ptr node1, node_ptr header1, node_ptr node2, node_ptr header2)
   {
      if(node1 == node2)
         return;

      //node1 and node2 must not be header nodes
      //BOOST_INTRUSIVE_INVARIANT_ASSERT((header1 != node1 && header2 != node2));
      if(header1 != header2){
         //Update header1 if necessary
         if(node1 == NodeTraits::get_left(header1)){
            NodeTraits::set_left(header1, node2);
         }

         if(node1 == NodeTraits::get_right(header1)){
            NodeTraits::set_right(header1, node2);
         }

         if(node1 == NodeTraits::get_parent(header1)){
            NodeTraits::set_parent(header1, node2);
         }

         //Update header2 if necessary
         if(node2 == NodeTraits::get_left(header2)){
            NodeTraits::set_left(header2, node1);
         }

         if(node2 == NodeTraits::get_right(header2)){
            NodeTraits::set_right(header2, node1);
         }

         if(node2 == NodeTraits::get_parent(header2)){
            NodeTraits::set_parent(header2, node1);
         }
      }
      else{
         //If both nodes are from the same tree
         //Update header if necessary
         if(node1 == NodeTraits::get_left(header1)){
            NodeTraits::set_left(header1, node2);
         }
         else if(node2 == NodeTraits::get_left(header2)){
            NodeTraits::set_left(header2, node1);
         }

         if(node1 == NodeTraits::get_right(header1)){
            NodeTraits::set_right(header1, node2);
         }
         else if(node2 == NodeTraits::get_right(header2)){
            NodeTraits::set_right(header2, node1);
         }

         if(node1 == NodeTraits::get_parent(header1)){
            NodeTraits::set_parent(header1, node2);
         }
         else if(node2 == NodeTraits::get_parent(header2)){
            NodeTraits::set_parent(header2, node1);
         }

         //Adjust data in nodes to be swapped
         //so that final link swap works as expected
         if(node1 == NodeTraits::get_parent(node2)){
            NodeTraits::set_parent(node2, node2);

            if(node2 == NodeTraits::get_right(node1)){
               NodeTraits::set_right(node1, node1);
            }
            else{
               NodeTraits::set_left(node1, node1);
            }
         }
         else if(node2 == NodeTraits::get_parent(node1)){
            NodeTraits::set_parent(node1, node1);

            if(node1 == NodeTraits::get_right(node2)){
               NodeTraits::set_right(node2, node2);
            }
            else{
               NodeTraits::set_left(node2, node2);
            }
         }
      }

      //Now swap all the links
      node_ptr temp;
      //swap left link
      temp = NodeTraits::get_left(node1);
      NodeTraits::set_left(node1, NodeTraits::get_left(node2));
      NodeTraits::set_left(node2, temp);
      //swap right link
      temp = NodeTraits::get_right(node1);
      NodeTraits::set_right(node1, NodeTraits::get_right(node2));
      NodeTraits::set_right(node2, temp);
      //swap parent link
      temp = NodeTraits::get_parent(node1);
      NodeTraits::set_parent(node1, NodeTraits::get_parent(node2));
      NodeTraits::set_parent(node2, temp);

      //Now adjust adjacent nodes for newly inserted node 1
      if((temp = NodeTraits::get_left(node1))){
         NodeTraits::set_parent(temp, node1);
      }
      if((temp = NodeTraits::get_right(node1))){
         NodeTraits::set_parent(temp, node1);
      }
      if((temp = NodeTraits::get_parent(node1)) &&
         //The header has been already updated so avoid it
         temp != header2){
         if(NodeTraits::get_left(temp) == node2){
            NodeTraits::set_left(temp, node1);
         }
         if(NodeTraits::get_right(temp) == node2){
            NodeTraits::set_right(temp, node1);
         }
      }
      //Now adjust adjacent nodes for newly inserted node 2
      if((temp = NodeTraits::get_left(node2))){
         NodeTraits::set_parent(temp, node2);
      }
      if((temp = NodeTraits::get_right(node2))){
         NodeTraits::set_parent(temp, node2);
      }
      if((temp = NodeTraits::get_parent(node2)) &&
         //The header has been already updated so avoid it
         temp != header1){
         if(NodeTraits::get_left(temp) == node1){
            NodeTraits::set_left(temp, node2);
         }
         if(NodeTraits::get_right(temp) == node1){
            NodeTraits::set_right(temp, node2);
         }
      }
   }

   //! <b>Requires</b>: node_to_be_replaced must be inserted in a tree
   //!   and new_node must not be inserted in a tree.
   //!
   //! <b>Effects</b>: Replaces node_to_be_replaced in its position in the
   //!   tree with new_node. The tree does not need to be rebalanced
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Note</b>: This function will break container ordering invariants if
   //!   new_node is not equivalent to node_to_be_replaced according to the
   //!   ordering rules. This function is faster than erasing and inserting
   //!   the node, since no rebalancing and comparison is needed.
   //!
   //!Experimental function
   static void replace_node(node_ptr node_to_be_replaced, node_ptr new_node)
   {
      if(node_to_be_replaced == new_node)
         return;
      replace_node(node_to_be_replaced, get_header(node_to_be_replaced), new_node);
   }

   //! <b>Requires</b>: node_to_be_replaced must be inserted in a tree
   //!   with header "header" and new_node must not be inserted in a tree.
   //!
   //! <b>Effects</b>: Replaces node_to_be_replaced in its position in the
   //!   tree with new_node. The tree does not need to be rebalanced
   //!
   //! <b>Complexity</b>: Constant.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Note</b>: This function will break container ordering invariants if
   //!   new_node is not equivalent to node_to_be_replaced according to the
   //!   ordering rules. This function is faster than erasing and inserting
   //!   the node, since no rebalancing or comparison is needed.
   //!
   //!Experimental function
   static void replace_node(node_ptr node_to_be_replaced, node_ptr header, node_ptr new_node)
   {
      if(node_to_be_replaced == new_node)
         return;

      //Update header if necessary
      if(node_to_be_replaced == NodeTraits::get_left(header)){
         NodeTraits::set_left(header, new_node);
      }

      if(node_to_be_replaced == NodeTraits::get_right(header)){
         NodeTraits::set_right(header, new_node);
      }

      if(node_to_be_replaced == NodeTraits::get_parent(header)){
         NodeTraits::set_parent(header, new_node);
      }

      //Now set data from the original node
      node_ptr temp;
      NodeTraits::set_left(new_node, NodeTraits::get_left(node_to_be_replaced));
      NodeTraits::set_right(new_node, NodeTraits::get_right(node_to_be_replaced));
      NodeTraits::set_parent(new_node, NodeTraits::get_parent(node_to_be_replaced));

      //Now adjust adjacent nodes for newly inserted node
      if((temp = NodeTraits::get_left(new_node))){
         NodeTraits::set_parent(temp, new_node);
      }
      if((temp = NodeTraits::get_right(new_node))){
         NodeTraits::set_parent(temp, new_node);
      }
      if((temp = NodeTraits::get_parent(new_node)) &&
         //The header has been already updated so avoid it
         temp != header){
         if(NodeTraits::get_left(temp) == node_to_be_replaced){
            NodeTraits::set_left(temp, new_node);
         }
         if(NodeTraits::get_right(temp) == node_to_be_replaced){
            NodeTraits::set_right(temp, new_node);
         }
      }
   }

   //! <b>Requires</b>: p is a node from the tree except the header.
   //!
   //! <b>Effects</b>: Returns the next node of the tree.
   //!
   //! <b>Complexity</b>: Average constant time.
   //!
   //! <b>Throws</b>: Nothing.
   static node_ptr next_node(const node_ptr &p)
   {
      node_ptr p_right(NodeTraits::get_right(p));
      if (!!p_right){
         return minimum(p_right);
      } else {
	      node_ptr p_temp = p;
         node_ptr x = NodeTraits::get_parent(p_temp);
         while (!!(p_temp == NodeTraits::get_right(x))) {
            p_temp = x;
            x = NodeTraits::get_parent(x);
         }
         return NodeTraits::get_right(p_temp) != x ? x : p_temp;
      }
   }

   //! <b>Requires</b>: p is a node from the tree except the leftmost node.
   //!
   //! <b>Effects</b>: Returns the previous node of the tree.
   //!
   //! <b>Complexity</b>: Average constant time.
   //!
   //! <b>Throws</b>: Nothing.
   static node_ptr prev_node(const node_ptr &p)
   {
      if (is_header(p)) {
		   if (!!NodeTraits::get_parent(p)) {
			   return maximum(NodeTraits::get_parent(p));
		   } else {
			   return p;
		   }
      } else {
		   if (!!NodeTraits::get_left(p)) {
			   return maximum(NodeTraits::get_left(p));
		   } else {
			   node_ptr p_temp = p;
			   node_ptr x = NodeTraits::get_parent(p_temp);
			   while (!!(p_temp == NodeTraits::get_left(x))) {
               p_temp = x;
				   x = NodeTraits::get_parent(x);
			   }
			   return x;
		   }
      }
   }
   
   //! <b>Requires</b>: p is a node of a tree but not the header.
   //!
   //! <b>Effects</b>: Returns the minimum node of the subtree starting at p.
   //!
   //! <b>Complexity</b>: Logarithmic to the size of the subtree.
   //!
   //! <b>Throws</b>: Nothing.
   static node_ptr minimum(node_ptr p) 
   {
      for (node_ptr p_left = NodeTraits::get_left(p)
         ;!!p_left
         ;p_left = NodeTraits::get_left(p)){
         p = p_left;
      }
      return p;
   }

   //! <b>Requires</b>: p is a node of a tree but not the header.
   //!
   //! <b>Effects</b>: Returns the maximum node of the subtree starting at p.
   //!
   //! <b>Complexity</b>: Logarithmic to the size of the subtree.
   //!
   //! <b>Throws</b>: Nothing.
   static node_ptr maximum (node_ptr p) 
   {
      node_ptr p_right = NodeTraits::get_right(p);
	   while (!!p_right) {
		   p = p_right;
		   p_right = NodeTraits::get_right(p);
      }
      return p;
   }

   //! <b>Requires</b>: node must not be part of any tree.
   //!
   //! <b>Effects</b>: After the function unique(node) == true.
   //!
   //! <b>Complexity</b>: Constant.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Nodes</b>: If node is inserted in a tree, this function corrupts the tree.
   static void init(node_ptr &node)
   {
	   NodeTraits::set_parent(node, 0);
	   NodeTraits::set_left(node, 0);
	   NodeTraits::set_right(node, 0);
   };

   //! <b>Requires</b>: node must not be part of any tree.
   //!
   //! <b>Effects</b>: Initializes the header to represent an empty tree.
   //!   unique(header) == true.
   //!
   //! <b>Complexity</b>: Constant.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Nodes</b>: If node is inserted in a tree, this function corrupts the tree.
   static void init_header(node_ptr &header) 
   {
      NodeTraits::set_parent(header, 0);
      NodeTraits::set_left(header, header);
      NodeTraits::set_right(header, header);
   }

   //! <b>Requires</b>: "disposer" must be an object function
   //!   taking a node_ptr parameter and shouldn't throw.
   //!
   //! <b>Effects</b>: Empties the target tree calling
   //!   <tt>void disposer::operator()(node_ptr)</tt> for every node of the tree
   //!    except the header.
   //!
   //! <b>Complexity</b>: Linear to the number of element of the source tree plus the.
   //!   number of elements of tree target tree when calling this function.
   //!
   //! <b>Throws</b>: If cloner functor throws. If this happens target nodes are disposed.
   template<class Disposer>
   static void clear_and_dispose(const node_ptr &header, Disposer disposer) 
   {
      node_ptr source_root = NodeTraits::get_parent(header);
      if (!source_root)
         return;
      dispose_subtree(source_root, disposer);
      init_header(header);
   }

   //! <b>Requires</b>: header is the header of a tree.
   //!
   //! <b>Effects</b>: Unlinks the leftmost node from the tree, and
   //!   updates the header link to the new leftmost node.
   //!
   //! <b>Complexity</b>: Average complexity is constant time.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Notes</b>: This function breaks the tree and the tree can
   //!   only be used for more unlink_leftmost_without_rebalance calls.
   //!   This function is normally used to achieve a step by step
   //!   controlled destruction of the tree.
   static node_ptr unlink_leftmost_without_rebalance(const node_ptr &header)
   {
      node_ptr leftmost = NodeTraits::get_left(header);
      if (leftmost == header)
         return 0;
      node_ptr leftmost_parent(NodeTraits::get_parent(leftmost));
      node_ptr leftmost_right (NodeTraits::get_right(leftmost));
      bool is_root = leftmost_parent == header;

      if (leftmost_right){
         NodeTraits::set_parent(leftmost_right, leftmost_parent);
         NodeTraits::set_left(header, tree_algorithms::minimum(leftmost_right));

         if (is_root)
            NodeTraits::set_parent(header, leftmost_right);
         else
            NodeTraits::set_left(NodeTraits::get_parent(header), leftmost_right);
      }
      else if (is_root){
         NodeTraits::set_parent(header, 0);
         NodeTraits::set_left(header,  header);
         NodeTraits::set_right(header, header);
      }
      else{
         NodeTraits::set_left(leftmost_parent, 0);
         NodeTraits::set_left(header, leftmost_parent);
      }
      return leftmost;
   }

   //! <b>Requires</b>: node is a node of the tree but it's not the header.
   //!
   //! <b>Effects</b>: Returns the number of nodes of the subtree.
   //!
   //! <b>Complexity</b>: Linear time.
   //!
   //! <b>Throws</b>: Nothing.
   static std::size_t count(const node_ptr &subtree)
   {
      if (!subtree) 
         return 0;

      std::size_t count = 0;
      node_ptr p = minimum(subtree);
      bool continue_looping = true;
      while (continue_looping) {
         ++count;
         node_ptr p_right(NodeTraits::get_right(p));
         if (!!p_right) {
            p = minimum(p_right);
         } else {
            for (;;) {
               node_ptr q;
               if (p == subtree) {
                  continue_looping = false;
                  break;
               }
               q = p;
               p = NodeTraits::get_parent(p);
               if (NodeTraits::get_left(p) == q)
                  break;
            }
         }
      }
      return count;
   }

   //! <b>Requires</b>: node is a node of the tree but it's not the header.
   //!
   //! <b>Effects</b>: Returns the number of nodes of the subtree.
   //!
   //! <b>Complexity</b>: Linear time.
   //!
   //! <b>Throws</b>: Nothing.
   static std::size_t size(const node_ptr &header)
   {
      node_ptr beg(begin_node(header));
      node_ptr end(end_node(header));
      std::size_t i = 0;
      for(;beg != end; beg = next_node(beg)) ++i;
      return i;
   }

   //! <b>Requires</b>: header1 and header2 must be the header nodes
   //!  of two trees.
   //!
   //! <b>Effects</b>: Swaps two trees. After the function header1 will contain
   //!   links to the second tree and header2 will have links to the first tree.
   //!
   //! <b>Complexity</b>: Constant.
   //!
   //! <b>Throws</b>: Nothing.
   static void swap_tree(node_ptr header1, node_ptr header2)
   {
      if (header1 == header2)
         return;

      node_ptr tmp;

      //Parent swap
      tmp = NodeTraits::get_parent(header1);
      NodeTraits::set_parent(header1, NodeTraits::get_parent(header2));
      NodeTraits::set_parent(header2, tmp);
      //Left swap
      tmp = NodeTraits::get_left(header1);
      NodeTraits::set_left(header1, NodeTraits::get_left(header2));
      NodeTraits::set_left(header2, tmp);
      //Right swap
      tmp = NodeTraits::get_right(header1);
      NodeTraits::set_right(header1, NodeTraits::get_right(header2));
      NodeTraits::set_right(header2, tmp);

      //Now test parent
      node_ptr h1_parent(NodeTraits::get_parent(header1));
      if(h1_parent){
         NodeTraits::set_parent(h1_parent, header1);
      }
      else{
         NodeTraits::set_left(header1, header1);
         NodeTraits::set_right(header1, header1);
      }

      node_ptr h2_parent(NodeTraits::get_parent(header2));
      if(h2_parent){
         NodeTraits::set_parent(h2_parent, header2);
      }
      else{
         NodeTraits::set_left(header2, header2);
         NodeTraits::set_right(header2, header2);
      }
   }

   static bool is_header(const node_ptr &p) 
   {
      bool result = false;
	   node_ptr p_parent = NodeTraits::get_parent(p);
	   if (!p_parent || p_parent==p) {
         result = true;
      }
      else if (NodeTraits::get_parent(p_parent) == p) {
         if (!!NodeTraits::get_left(p)) {
            if (NodeTraits::get_parent(NodeTraits::get_left(p)) != p) {
               result = true;
            }
            if (p_parent == NodeTraits::get_left(p)) {
               result = true;
            }
         }
      }
      return result;
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   KeyNodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. KeyNodePtrCompare can compare KeyType with tree's node_ptrs.
   //!
   //! <b>Effects</b>: Returns an node_ptr to the element that is equivalent to
   //!   "key" according to "comp" or "header" if that element does not exist.
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: If "comp" throws.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr find
      (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {
      node_ptr end = header;
      node_ptr y = lower_bound(header, key, comp);
      return (y == end || comp(key, y)) ? end : y;
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   KeyNodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. KeyNodePtrCompare can compare KeyType with tree's node_ptrs.
   //!
   //! <b>Effects</b>: Returns an a pair of node_ptr delimiting a range containing
   //!   all elements that are equivalent to "key" according to "comp" or an
   //!   empty range that indicates the position where those elements would be
   //!   if they there are no equivalent elements.
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: If "comp" throws.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> equal_range
      (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {
      node_ptr y = header;
      node_ptr x = NodeTraits::get_parent(header);

      while(x){
         if(comp(x, key)){
            x = NodeTraits::get_right(x);
         }
         else if(comp(key, x)){
            y = x;
            x = NodeTraits::get_left(x);
         }
         else{
            node_ptr xu(x), yu(y);
            y = x, x = NodeTraits::get_left(x);
            xu = NodeTraits::get_right(xu);

            while(x){
               if(comp(x, key)){
                  x = NodeTraits::get_right(x);
               }
               else {
                  y = x;
                  x = NodeTraits::get_left(x);
               }
            }

            while(xu){
               if(comp(key, xu)){
                  yu = xu;
                  xu = NodeTraits::get_left(xu);
               }
               else {
                  xu = NodeTraits::get_right(xu);
               }
            }
            return std::pair<node_ptr,node_ptr> (y, yu);
         }
      }
      return std::pair<node_ptr,node_ptr> (y, y);
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   KeyNodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. KeyNodePtrCompare can compare KeyType with tree's node_ptrs.
   //!
   //! <b>Effects</b>: Returns an node_ptr to the first element that is greater
   //!   than "key" according to "comp" or "header" if that element does not exist.
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: If "comp" throws.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_bound
      (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {
      node_ptr y = header;
      node_ptr x = NodeTraits::get_parent(header);
      while (x) {
         if(comp(key, x)){
            y = x;
            x = NodeTraits::get_left(x);
         }
         else {
            x = NodeTraits::get_right(x);
         }
      }
      return y;
   }

   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_equal_bound
      (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {
      node_ptr y = header;
      // start with root node
      node_ptr x = NodeTraits::get_parent(header);
      while (!!x) {
         if (comp(key, x)) {
        	// potential result
            y = x;
            x = NodeTraits::get_left(x);
         }
         else {
            if (comp(x, key)) {
               x = NodeTraits::get_right(x);
            } else {
               y = NodeTraits::get_right(x);
               // continue with equal values
               if (!y || comp(y, key) || comp(key, y)) {
                  return x;
               }
               x = y;
            }
         }
      }
      return y;
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   KeyNodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. KeyNodePtrCompare can compare KeyType with tree's node_ptrs.
   //!
   //! <b>Effects</b>: Returns an node_ptr to the first element that is
   //!   not less than "key" according to "comp" or "header" if that element does
   //!   not exist.
   //!
   //! <b>Complexity</b>: Logarithmic.
   //!
   //! <b>Throws</b>: If "comp" throws.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr lower_bound
      (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {      
      node_ptr y = header;
      node_ptr x = NodeTraits::get_parent(header);
      while (!!x) {
         if (comp(x, key)) {
            x = NodeTraits::get_right(x);
         } else {
            y = x;
            x = NodeTraits::get_left(x);
         }
      }
      return y;
   }

   /**
    * Measure tree height
    * @return tree height (maximum distance from leaf to root)
    */
   static uint64_t get_height(const node_ptr &header) {
	   return get_height(NodeTraits::get_parent(header), 0);
   }

   /**
    * Divide this data structure into approximately N chunks of approximately equal size
    * @return chunk iterators
    */
   static std::vector<node_ptr> divide(const node_ptr &header, int N) {
	   std::vector<node_ptr> result;
	   divide(NodeTraits::get_parent(header), N, result);
	   return result;
   }
   
   // @return last lower or equal node
   template<class KeyType, class KeyNodePtrCompare>
	   static node_ptr lower_equal_bound
	   (const node_ptr &header, const KeyType &key, KeyNodePtrCompare comp)
   {
      assert(!!header);
      node_ptr y = header;
      node_ptr x = NodeTraits::get_parent(header);
      while (!!x) {
         if (comp(x, key)) {
            y = x;
            x = NodeTraits::get_right(x);
         } else {
            if (comp (key, x)) {
               x = NodeTraits::get_left(x);
			   } else {
               y = NodeTraits::get_left(x);
               // continue with equal values
               if (!y || comp(y, key) || comp(key, y)) {
                  return x;
               }
               x = y;
			   }
         }
      }
      return y;
   }
   
   struct join_stack : public ::std::list<node_ptr> {
	   const node_ptr &operator*() const {
		   return this->back();
	   }
   };

   // initialize join forward iterator
   template <class KeyType, class KeyNodePtrCompare>
	   static void beginJoinForward (const node_ptr &header,join_stack &it, const KeyType &key, KeyNodePtrCompare comp)
   {
	   assert (it.empty());
	   node_ptr x = NodeTraits::get_parent(header);
	   if (!!x) {
		   it.push_back(x);
		   x = NodeTraits::get_left(header);
		   while (comp(key, x)) {
			   it.push_back(x);
			   x = NodeTraits::get_left(x);			
		   }
	   }
   }
	
   // update "it" iterator so that it points to the first
   // equal or greater node ( join forward )   
   // @return false if end of the collection reached ( join not possible )
   template <class KeyType, class KeyNodePtrCompare>
	   static bool joinForward (join_stack &it, const KeyType &key, KeyNodePtrCompare comp)
   {
	   assert (!it.empty());
	   node_ptr x = *it;
	   while (!it.empty() && comp(x,key)) {
			it.pop_back();
			if (!it.empty()) {
				x = *it;
			}
	   }
	   while (!!x) {
		   if (comp(x, key)) {
			   x = NodeTraits::get_right(x);
		   }
		   else {
			   it.push_back(x);
			   if (comp(key, x)) {
				  x = NodeTraits::get_left(x);
			   }
			   else {
				   // x equals key, join success
				   return true;
			   }
		   }
	   }
	   return !it.empty();
   }

   template <class KeyType, class KeyNodePtrCompare>
	   static void beginJoinBackward (const node_ptr &header,join_stack &it, const KeyType &key, KeyNodePtrCompare comp)
   {
	   assert (it.empty());
	   node_ptr x = NodeTraits::get_parent(header);
	   if (!!x) {
		   it.push_back(x);
		   x = NodeTraits::get_right(header);
		   if (!!x && comp(x,key)) {
			   it.push_back(x);			   
		   }
	   }
   }

   static void beginJoinBackward (const node_ptr &header,join_stack &it) 
   {
	   assert (it.empty());
	   node_ptr x = NodeTraits::get_parent(header);
	   while (!!x) {
		   it.push_back(x);
		   x = NodeTraits::get_right(x);
	   }
   }

   // update "it" iterator so that it points to the first ( iterating backwards )   
   // equal or smaller node ( join backward )
   // @return false if end of the collection reached ( join not possible )
   template <class KeyType, class KeyNodePtrCompare>
	   static bool joinBackward (join_stack &it, const KeyType &key, KeyNodePtrCompare comp)
   {
	   assert (!it.empty());
	   node_ptr x = *it;
	   while (!it.empty() && comp(key,x)) {
			it.pop_back();
			if (!it.empty()) {
				x = *it;
			}
	   }
	   while (!!x) {
		   if (comp(key,x)) {
			   x = NodeTraits::get_left(x);
		   }
		   else {
			   it.push_back(x);
			   if (comp(x,key)) {
				  x = NodeTraits::get_right(x);
			   }
			   else {
				   // x equals key, join success
				   return true;
			   }
		   }
	   }
	   return !it.empty();
   }

   template <class KeyType, class KeyNodePtrCompare>
	   static void joinBound (join_stack &it, const KeyType &key, KeyNodePtrCompare comp)
   {
	   assert (!it.empty());
	   node_ptr x = *it;
	   while (!it.empty() && comp(key,x)) {
			it.pop_back();
			if (!it.empty()) {
				x = *it;
			}
	   }
	   while (!!x) {
		   if (comp(key,x)) {
			   it.push_back(x);
			   x = NodeTraits::get_left(x);
		   }
		   else {
			   if (comp(x,key)) {
				   x = NodeTraits::get_right(x);
			   }
			   else {
				   it.push_back(x);
				   // x equals key, join success
				   break;
			   }
		   }
	   }
	   assert (!it.empty());
   }
   
   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   "commit_data" must have been obtained from a previous call to
   //!   "insert_unique_check". No objects should have been inserted or erased
   //!   from the set between the "insert_unique_check" that filled "commit_data"
   //!   and the call to "insert_commit". 
   //! 
   //! 
   //! <b>Effects</b>: Inserts new_node in the set using the information obtained
   //!   from the "commit_data" that a previous "insert_check" filled.
   //!
   //! <b>Complexity</b>: Constant time.
   //!
   //! <b>Throws</b>: Nothing.
   //! 
   //! <b>Notes</b>: This function has only sense if a "insert_unique_check" has been
   //!   previously executed to fill "commit_data". No value should be inserted or
   //!   erased between the "insert_check" and "insert_commit" calls.
   static void insert_unique_commit
      (node_ptr &header, node_ptr &new_value, insert_commit_data &commit_data)
   {
      //Check if commit_data has not been initialized by a insert_unique_check call.
      assert(!!commit_data.node);
      link(header, new_value, commit_data.node, commit_data.link_left);
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   KeyNodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. NodePtrCompare compares KeyType with a node_ptr.
   //! 
   //! <b>Effects</b>: Checks if there is an equivalent node to "key" in the
   //!   tree according to "comp" and obtains the needed information to realize
   //!   a constant-time node insertion if there is no equivalent node.
   //!
   //! <b>Returns</b>: If there is an equivalent value
   //!   returns a pair containing a node_ptr to the already present node
   //!   and false. If there is not equivalent key can be inserted returns true
   //!   in the returned pair's boolean and fills "commit_data" that is meant to
   //!   be used with the "insert_commit" function to achieve a constant-time
   //!   insertion function.
   //! 
   //! <b>Complexity</b>: Average complexity is at most logarithmic.
   //!
   //! <b>Throws</b>: If "comp" throws.
   //! 
   //! <b>Notes</b>: This function is used to improve performance when constructing
   //!   a node is expensive and the user does not want to have two equivalent nodes
   //!   in the tree: if there is an equivalent value
   //!   the constructed object must be discarded. Many times, the part of the
   //!   node that is used to impose the order is much cheaper to construct
   //!   than the node and this function offers the possibility to use that part
   //!   to check if the insertion will be successful.
   //!
   //!   If the check is successful, the user can construct the node and use
   //!   "insert_commit" to insert the node in constant-time. This gives a total
   //!   logarithmic complexity to the insertion: check(O(log(N)) + commit(O(1)).
   //!
   //!   "commit_data" remains valid for a subsequent "insert_unique_commit" only
   //!   if no more objects are inserted or erased from the set.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr &header,  const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data, std::size_t *pdepth = 0)
   {
      std::size_t depth = 0;
      node_ptr h(header);
      node_ptr y(h);
      node_ptr x(NodeTraits::get_parent(y));
      node_ptr prev(node_traits::get_null());

      //Find the upper bound, cache the previous value and if we should
      //store it in the left or right node
      bool left_child = true;
      while (!!x) {
         ++depth;
         y = x;
         x = (left_child = comp(key, x)) ? 
               NodeTraits::get_left(x) : (prev = y, NodeTraits::get_right(x));
      }

      if (pdepth) *pdepth = depth;

      //Since we've found the upper bound there is no other value with the same key if:
      //    - There is no previous node
      //    - The previous node is less than the key
      if (!prev || comp(prev, key)) {
         commit_data.link_left = left_child;
         commit_data.node      = y;
         return std::pair<node_ptr, bool>(node_ptr(), true);
      }
      //If the previous value was not less than key, it means that it's equal
      //(because we've checked the upper bound)
      else {
         return std::pair<node_ptr, bool>(prev, false);
      }
   }

   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr &header,  const node_ptr &hint, const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data, std::size_t *pdepth = 0)
   {
      // hint must be bigger than the key
      if(hint == header || comp(key, hint)) {
         node_ptr prev = hint;
         //The previous value should be less than the key
         if (prev == NodeTraits::get_left(header) || comp((prev = prev_node(hint)), key)) {
            commit_data.link_left = unique(header) || !NodeTraits::get_left(hint);
            commit_data.node      = commit_data.link_left ? hint : prev;
            if (pdepth) {
               *pdepth = commit_data.node == header ? 0 : depth(commit_data.node) + 1;
            }
            return std::pair<node_ptr, bool>(node_ptr(), true);
         }
         else {
            return insert_unique_check(header, key, comp, commit_data, pdepth);
         }
      }
      //The hint was wrong, use hintless insert
      else {
         return insert_unique_check(header, key, comp, commit_data, pdepth);
      }
   }

   //! <b>Requires</b>: "header" must be the header node of a tree.
   //!   NodePtrCompare is a function object that induces a strict weak
   //!   ordering compatible with the strict weak ordering used to create the
   //!   the tree. NodePtrCompare compares two node_ptrs. "hint" is node from
   //!   the "header"'s tree.
   //!   
   //! <b>Effects</b>: Inserts new_node into the tree, using "hint" as a hint to
   //!   where it will be inserted. If "hint" is the upper_bound
   //!   the insertion takes constant time (two comparisons in the worst case).
   //!
   //! <b>Complexity</b>: Logarithmic in general, but it is amortized
   //!   constant time if new_node is inserted immediately before "hint".
   //! 
   //! <b>Throws</b>: If "comp" throws.
   template<class NodePtrCompare>
   static node_ptr insert_equal
      (node_ptr &header, node_ptr &hint, node_ptr &new_node, NodePtrCompare comp, std::size_t *pdepth = 0)
   {
      if (hint == header || !comp(hint, new_node)) {
         node_ptr prev(hint);
         if (hint == NodeTraits::get_left(header) || 
            !comp(new_node, (prev = prev_node(hint)))) {
            bool link_left = unique(header) || !NodeTraits::get_left(hint);
            link(header, new_node, link_left ? hint : prev, link_left);
            if (pdepth) *pdepth = depth(new_node) + 1;
            return new_node;
         }
         else {
            return insert_equal_upper_bound(header, new_node, comp, pdepth);
         }
      }
      else { 
         return insert_equal_lower_bound(header, new_node, comp, pdepth);
      }
   }
   
   //! <b>Requires</b>: p can't be a header node.
   //! 
   //! <b>Effects</b>: Calculates the depth of a node: the depth of a
   //! node is the length (number of edges) of the path from the root
   //! to that node. (The root node is at depth 0.)
   //! 
   //! <b>Complexity</b>: Logarithmic to the number of nodes in the tree. 
   //! 
   //! <b>Throws</b>: Nothing.
   static std::size_t depth (node_ptr p)
   {
      std::size_t depth = 0;
      node_ptr p_parent;
      while(p != NodeTraits::get_parent(p_parent = NodeTraits::get_parent(p))){
         ++depth;
         p = p_parent;
      }
      return depth;
   }

   // obtain node to link to / no linkage performed
   template<class KeyType, class KeyNodePtrCompare>    
   static void link_equal_upper_bound (node_ptr &h, const KeyType &key, KeyNodePtrCompare comp, link_data &ld, 
	   std::size_t *pdepth = 0)
   {
	   std::size_t depth = 0;
	   ld.y = h;
      node_ptr x(NodeTraits::get_parent(ld.y));

      while(!!x) {
         ++depth;
         ld.y = x;
         x = comp(key, x) ? 
               NodeTraits::get_left(x) : NodeTraits::get_right(x);
      }
      
      ld.link_left = (ld.y == h) || comp(key, ld.y);      
      if( pdepth) *pdepth = depth;
	}
   
   template<class NodePtrCompare>
   static node_ptr insert_equal_upper_bound
      (node_ptr &h, node_ptr &new_node, NodePtrCompare comp, std::size_t *pdepth = 0)
	{
      std::size_t depth = 0;
      node_ptr y(h);
      node_ptr x(NodeTraits::get_parent(y));

      while (!!x) {
         ++depth;
         y = x;
         x = comp(new_node, x) ? 
               NodeTraits::get_left(x) : NodeTraits::get_right(x);
      }

      bool link_left = (y == h) || comp(new_node, y);
      link(h, new_node, y, link_left);
      if( pdepth) *pdepth = depth;
      return new_node;
   }

   template<class NodePtrCompare>
   static node_ptr insert_equal_lower_bound
      (node_ptr &h, node_ptr &new_node, NodePtrCompare comp, std::size_t *pdepth = 0)
   {
      std::size_t depth = 0;
      node_ptr y(h);
      node_ptr x(NodeTraits::get_parent(y));

      while(!!x) {
         ++depth;
         y = x;
         x = !comp(x, new_node) ? 
               NodeTraits::get_left(x) : NodeTraits::get_right(x);
      }

      bool link_left = (y == h) || !comp(y, new_node);
      link(h, new_node, y, link_left);
      if(pdepth)  *pdepth = depth;
      return new_node;
   }
	
   template<class KeyType, class KeyNodePtrCompare> 
   static void link_equal_lower_bound
      (node_ptr &h, const KeyType &key, KeyNodePtrCompare comp, link_data &ld, std::size_t *pdepth = 0)
   {
      std::size_t depth = 0;
      ld.y = h;
      node_ptr x(NodeTraits::get_parent(ld.y));

      while (!!x) {
         ++depth;
         ld.y = x;
         x = !comp(x, key) ? 
               NodeTraits::get_left(x) : NodeTraits::get_right(x);
      }
	
      ld.link_left = (ld.y == h) || !comp(ld.y, key);
      if( pdepth) *pdepth = depth;
   }
	
   //! <b>Requires</b>: "cloner" must be a function
   //!   object taking a node_ptr and returning a new cloned node of it. "disposer" must
   //!   take a node_ptr and shouldn't throw.
   //!
   //! <b>Effects</b>: First empties target tree calling 
   //!   <tt>void disposer::operator()(node_ptr)</tt> for every node of the tree
   //!    except the header.
   //!    
   //!   Then, duplicates the entire tree pointed by "source_header" cloning each
   //!   source node with <tt>node_ptr Cloner::operator()(node_ptr)</tt> to obtain 
   //!   the nodes of the target tree. If "cloner" throws, the cloned target nodes
   //!   are disposed using <tt>void disposer(node_ptr)</tt>.
   //! 
   //! <b>Complexity</b>: Linear to the number of element of the source tree plus the.
   //!   number of elements of tree target tree when calling this function.
   //! 
   //! <b>Throws</b>: If cloner functor throws. If this happens target nodes are disposed.
   template <class Cloner, class Disposer>
   static void clone
      (const node_ptr &source_header, node_ptr &target_header, Cloner cloner, Disposer disposer)
   {
      if (!unique(target_header)) {
         clear_and_dispose(target_header, disposer);
      }

      node_ptr leftmost, rightmost;
      node_ptr new_root = clone_subtree
         (source_header, target_header, cloner, disposer, leftmost, rightmost);

      // Now update header node
      NodeTraits::set_parent(target_header, new_root);
      NodeTraits::set_left  (target_header, leftmost);
      NodeTraits::set_right (target_header, rightmost);
   }

   template <class Cloner, class Disposer>
   static node_ptr clone_subtree
      ( const_node_ptr source_parent,  node_ptr target_parent
      , Cloner cloner,                 Disposer disposer
      , node_ptr &leftmost_out,        node_ptr &rightmost_out
      )
   {
      node_ptr target_sub_root = target_parent;
      node_ptr source_root = NodeTraits::get_parent(source_parent);
      if (!source_root) {
         leftmost_out = rightmost_out = source_root;
      }
      else {
         //We'll calculate leftmost and rightmost nodes while iterating
         node_ptr current = source_root;
         node_ptr insertion_point = target_sub_root = cloner(current);

         //We'll calculate leftmost and rightmost nodes while iterating
         node_ptr leftmost  = target_sub_root;
         node_ptr rightmost = target_sub_root;

         //First set the subroot
         NodeTraits::set_left(target_sub_root, 0);
         NodeTraits::set_right(target_sub_root, 0);
         NodeTraits::set_parent(target_sub_root, target_parent);

         try {
            while(true) {
               //First clone left nodes
               if( NodeTraits::get_left(current) &&
                  !NodeTraits::get_left(insertion_point)) {
                  current = NodeTraits::get_left(current);
                  node_ptr temp = insertion_point;
                  //Clone and mark as leaf
                  insertion_point = cloner(current);
                  NodeTraits::set_left  (insertion_point, 0);
                  NodeTraits::set_right (insertion_point, 0);
                  //Insert left
                  NodeTraits::set_parent(insertion_point, temp);
                  NodeTraits::set_left  (temp, insertion_point);
                  //Update leftmost
                  if(rightmost == target_sub_root)
                     leftmost = insertion_point;
               }
               //Then clone right nodes
               else if( NodeTraits::get_right(current) && 
                       !NodeTraits::get_right(insertion_point)){
                  current = NodeTraits::get_right(current);
                  node_ptr temp = insertion_point;
                  //Clone and mark as leaf
                  insertion_point = cloner(current);
                  NodeTraits::set_left  (insertion_point, 0);
                  NodeTraits::set_right (insertion_point, 0);
                  //Insert right
                  NodeTraits::set_parent(insertion_point, temp);
                  NodeTraits::set_right (temp, insertion_point);
                  //Update rightmost
                  rightmost = insertion_point;
               }
               //If not, go up
               else if(current == source_root){
                  break;
               }
               else{
                  //Branch completed, go up searching more nodes to clone
                  current = NodeTraits::get_parent(current);
                  insertion_point = NodeTraits::get_parent(insertion_point);
               }
            }
         }
         catch(...) {
            dispose_subtree(target_sub_root, disposer);
            throw;
         }
         leftmost_out   = leftmost;
         rightmost_out  = rightmost;
      }
      return target_sub_root;
   }

   template<class Disposer>
   static void dispose_subtree(node_ptr &x, Disposer disposer)
   {
      node_ptr save;
      while (x){
         save = NodeTraits::get_left(x);
         if (save) {
            // Right rotation
            NodeTraits::set_left(x, NodeTraits::get_right(save));
            NodeTraits::set_right(save, x);
         }
         else {
            save = NodeTraits::get_right(x);
            init(x);
            disposer(x);
         }
         x = save;
      }
   }

   //! <b>Requires</b>: p is a node of a tree.
   //! 
   //! <b>Effects</b>: Returns true if p is a left child.
   //! 
   //! <b>Complexity</b>: Constant.
   //! 
   //! <b>Throws</b>: Nothing.
   static bool is_left_child(const node_ptr p)
   {  return NodeTraits::get_left(NodeTraits::get_parent(p)) == p;  }

   //! <b>Requires</b>: p is a node of a tree.
   //! 
   //! <b>Effects</b>: Returns true if p is a right child.
   //! 
   //! <b>Complexity</b>: Constant.
   //! 
   //! <b>Throws</b>: Nothing.
   static bool is_right_child (const node_ptr &p)
   {  return NodeTraits::get_right(NodeTraits::get_parent(p)) == p;  }

   static void replace_own (node_ptr &own, node_ptr &x, node_ptr &header)
   {
      if(NodeTraits::get_parent(header) == own)
         NodeTraits::set_parent(header, x);
      else if(is_left_child(own)) {
    	 node_ptr ownParent = NodeTraits::get_parent(own);
         NodeTraits::set_left(ownParent, x);
      } else {
    	 node_ptr ownParent = NodeTraits::get_parent(own);
         NodeTraits::set_right(ownParent, x);
      }
   }

   static void rotate_left(node_ptr &p, const node_ptr &header)
   {
      node_ptr x = NodeTraits::get_right(p);
      NodeTraits::set_right(p, NodeTraits::get_left(x));
      if(NodeTraits::get_left(x) != 0)
         NodeTraits::set_parent(NodeTraits::get_left(x), p);
      NodeTraits::set_parent(x, NodeTraits::get_parent(p));
      replace_own (p, x, header);
      NodeTraits::set_left(x, p);
      NodeTraits::set_parent(p, x);
   }

   static void rotate_right(node_ptr &p, const node_ptr &header)
   {
      node_ptr x(NodeTraits::get_left(p));
      node_ptr x_right(NodeTraits::get_right(x));
      NodeTraits::set_left(p, x_right);
      if(x_right)
         NodeTraits::set_parent(x_right, p);
      NodeTraits::set_parent(x, NodeTraits::get_parent(p));
      replace_own (p, x, header);
      NodeTraits::set_right(x, p);
      NodeTraits::set_parent(p, x);
   }

   // rotate node t with left child            | complexity : constant        | exception : nothrow
   static node_ptr rotate_left(const node_ptr &t)
   {
      node_ptr x = NodeTraits::get_right(t);
      NodeTraits::set_right(t, NodeTraits::get_left(x));

      if( NodeTraits::get_right(t) != 0 ){
         NodeTraits::set_parent(NodeTraits::get_right(t), t );
      }
      NodeTraits::set_left(x, t);
      NodeTraits::set_parent(t, x);
      return x;
   }

   // rotate node t with right child            | complexity : constant        | exception : nothrow
   static node_ptr rotate_right(const node_ptr &t)
   {
      node_ptr x = NodeTraits::get_left(t);
      NodeTraits::set_left(t, NodeTraits::get_right(x));
      if( NodeTraits::get_left(t) != 0 ){
         NodeTraits::set_parent(NodeTraits::get_left(t), t);
      }
      NodeTraits::set_right(x, t);
      NodeTraits::set_parent(t, x);
      return x;
   }

   static void link(node_ptr &header, node_ptr &z, link_data &ld)
   {
	   link(header,z,ld.y,ld.link_left);
   }

   static void link(node_ptr &header, node_ptr &z, node_ptr &par, bool left)
   {
      if(par == header){
         NodeTraits::set_parent(par, z);
         NodeTraits::set_right(par, z);
         NodeTraits::set_left(par, z);
      }
      else if(left){
         NodeTraits::set_left(par, z);
         if(par == NodeTraits::get_left(header))
             NodeTraits::set_left(header, z);
      }
      else{
         NodeTraits::set_right(par, z);
         if(par == NodeTraits::get_right(header))
             NodeTraits::set_right(header, z);
      }
      NodeTraits::set_parent(z, par);
      NodeTraits::set_right(z, node_traits::get_null());
      NodeTraits::set_left(z, node_traits::get_null());
   }

   static void erase(node_ptr &header, node_ptr &z)
   {
      data_for_rebalance ignored;
      erase(header, z, nop_erase_fixup(), ignored);
   }
	
   struct data_for_rebalance
   {
      node_ptr x;
      node_ptr x_parent;
      node_ptr y;
   };

   template<class F>
   static void erase(node_ptr &header, node_ptr &z, F z_and_successor_fixup, data_for_rebalance &info)
   {
      erase_impl(header, z, info);
      if(info.y != z){
         z_and_successor_fixup(z, info.y);
      }
   }

   static void unlink(node_ptr &node)
   {
      node_ptr x = NodeTraits::get_parent(node);
      if(x){
         while(!is_header(x))
            x = NodeTraits::get_parent(x);
         erase(x, node);
      }
   }

   static void tree_to_vine(const node_ptr &header)
   {  subtree_to_vine(NodeTraits::get_parent(header)); }

   static void vine_to_tree(const node_ptr &header, std::size_t count)
   {  vine_to_subtree(NodeTraits::get_parent(header), count);  }

   static void rebalance(const node_ptr &header)
   {
      //Taken from:
      //"Tree rebalancing in optimal time and space"
      //Quentin F. Stout and Bette L. Warren
      std::size_t len;
      subtree_to_vine(NodeTraits::get_parent(header), &len);
      vine_to_subtree(NodeTraits::get_parent(header), len);
   }

   static node_ptr rebalance_subtree(const node_ptr &old_root)
   {
      std::size_t len;
      node_ptr new_root = subtree_to_vine(old_root, &len);
      return vine_to_subtree(new_root, len);
   }

   static node_ptr subtree_to_vine(const node_ptr &old_root, std::size_t *plen = 0)
   {
      std::size_t len;
      len = 0;
      if(!old_root)   return node_traits::get_null();

      //To avoid irregularities in the algorithm (old_root can be a
      //left or right child or even the root of the tree) just put the 
      //root as the right child of its parent. Before doing this backup
      //information to restore the original relationship after
      //the algorithm is applied.
      node_ptr super_root = NodeTraits::get_parent(old_root);
      assert(!!super_root);
      
      //Get info
      node_ptr super_root_right_backup = NodeTraits::get_right(super_root);
      bool super_root_is_header   = is_header(super_root);
      bool old_root_is_right  = is_right_child(old_root);

      node_ptr x(old_root);
      node_ptr new_root(x);
      node_ptr save;
      bool moved_to_right = false;
      for ( ; !!x; x = save) {
         save = NodeTraits::get_left(x);
         if (!!save) {
            // Right rotation
            node_ptr save_right = NodeTraits::get_right(save);
            node_ptr x_parent   = NodeTraits::get_parent(x);
            NodeTraits::set_parent(save, x_parent);
            NodeTraits::set_right (x_parent, save);
            NodeTraits::set_parent(x, save);
            NodeTraits::set_right (save, x);
            NodeTraits::set_left(x, save_right);
            if (!!save_right)
               NodeTraits::set_parent(save_right, x);
            if (!moved_to_right)
               new_root = save;
         } else {
            moved_to_right = true;
            save = NodeTraits::get_right(x);
            ++len;
         }
      }

      if (super_root_is_header) {
         NodeTraits::set_right(super_root, super_root_right_backup);
         NodeTraits::set_parent(super_root, new_root);
      }
      else if(old_root_is_right){
         NodeTraits::set_right(super_root, new_root);
      }
      else{
         NodeTraits::set_right(super_root, super_root_right_backup);
         NodeTraits::set_left(super_root, new_root);
      }
      if (plen) *plen = len;
      return new_root;
   }
   
   static node_ptr vine_to_subtree(const node_ptr &old_root, std::size_t count)
   {
      std::size_t leaf_nodes = count + 1 - ((size_t) 1 << floor_log2 (count + 1));
      std::size_t vine_nodes = count - leaf_nodes;

      node_ptr new_root = compress_subtree(old_root, leaf_nodes);
      while(vine_nodes > 1){
         vine_nodes /= 2;
         new_root = compress_subtree(new_root, vine_nodes);
      }
      return new_root;
   }

   static node_ptr compress_subtree(const node_ptr &old_root, std::size_t count)
   {
      if (!old_root) return old_root;

      //To avoid irregularities in the algorithm (old_root can be
      //left or right child or even the root of the tree) just put the 
      //root as the right child of its parent. First obtain
      //information to restore the original relationship after
      //the algorithm is applied.
      node_ptr super_root = NodeTraits::get_parent(old_root);
      assert(!!super_root);

      //Get info
      node_ptr super_root_right_backup = NodeTraits::get_right(super_root);
      bool super_root_is_header   = is_header(super_root);
      bool old_root_is_right  = is_right_child(old_root);

      //Put old_root as right child
      NodeTraits::set_right(super_root, old_root);

      //Start the compression algorithm            
      node_ptr even_parent = super_root;
      node_ptr new_root = old_root;

      while (count--) {
         node_ptr even = NodeTraits::get_right(even_parent);
         node_ptr odd = NodeTraits::get_right(even);

         if (new_root == old_root)
            new_root = odd;

         node_ptr even_right = NodeTraits::get_left(odd);
         NodeTraits::set_right(even, even_right);
         if (!!even_right)
            NodeTraits::set_parent(even_right, even);

         NodeTraits::set_right(even_parent, odd);
         NodeTraits::set_parent(odd, even_parent);
         NodeTraits::set_left(odd, even);
         NodeTraits::set_parent(even, odd);
         even_parent = odd;
      }

      if(super_root_is_header){
         NodeTraits::set_parent(super_root, new_root);
         NodeTraits::set_right(super_root, super_root_right_backup);
      }
      else if(old_root_is_right){
         NodeTraits::set_right(super_root, new_root);
      }
      else{
         NodeTraits::set_left(super_root, new_root);
         NodeTraits::set_right(super_root, super_root_right_backup);
      }
      return new_root;
   }

private:

   static uint64_t get_height(const node_ptr &root, uint64_t current)
   {
	   if (!!root) {
		   return std::max(
				   get_height(NodeTraits::get_left(root), current + 1),
				   get_height(NodeTraits::get_right(root), current +1));
	   }
	   return current;
   }

   /**
    * @return number of chunks created
    */
   static int divide(const node_ptr &root, int N, std::vector<node_ptr> &result) 
   {
	   int count = 0;
	   if (!!root) {
		   std::vector<node_ptr> right_result;
		   if (N > 1) {
			   int left_count = divide(NodeTraits::get_left(root), N / 2, result);
			   count += left_count;
			   count += divide(NodeTraits::get_right(root), N - left_count, right_result);
		   }
		   // if less than requested also register root element
		   if (count < N) {
			   result.emplace_back(root);
			   ++count;
		   }
		   // this push is postponed to guarantee proper ascending order
		   for (const node_ptr &ptr: right_result) {
			   result.emplace_back(ptr);
		   }
	   }
	   return count;
   }

   static void erase_impl(node_ptr &header, node_ptr &z, data_for_rebalance &info)
   {
      node_ptr y(z);
      node_ptr x;
      node_ptr x_parent(NodeTraits::get_null());
      node_ptr z_left(NodeTraits::get_left(z));
      node_ptr z_right(NodeTraits::get_right(z));
      if (!z_left) {
         x = z_right;    // x might be null.
      }
      else if (!z_right) { // z has exactly one non-null child. y == z.
         x = z_left;       // x is not null.
      } else {
         // find z's successor
         y = tree_algorithms::minimum (z_right);
         x = NodeTraits::get_right(y);     // x might be null.
      }

      if (y != z) {
         // relink y in place of z.  y is z's successor
    	   node_ptr zLeft = NodeTraits::get_left(z);
         NodeTraits::set_parent(zLeft, y);
         NodeTraits::set_left(y, NodeTraits::get_left(z));
         if(y != NodeTraits::get_right(z)){
            x_parent = NodeTraits::get_parent(y);
            if(!!x)
               NodeTraits::set_parent(x, x_parent);
            NodeTraits::set_left(x_parent, x);   // y must be a child of left_
            NodeTraits::set_right(y, NodeTraits::get_right(z));
            node_ptr zRight = NodeTraits::get_right(z);
            NodeTraits::set_parent(zRight, y);
         }
         else
            x_parent = y;
         tree_algorithms::replace_own (z, y, header);
         NodeTraits::set_parent(y, NodeTraits::get_parent(z));
      }
      else {   // y == z --> z has only one child, or none
         x_parent = NodeTraits::get_parent(z);
         if(!!x)
            NodeTraits::set_parent(x, x_parent);
         tree_algorithms::replace_own (z, x, header);
         if(NodeTraits::get_left(header) == z){
            NodeTraits::set_left(header, !NodeTraits::get_right(z) ?        // z->get_left() must be null also
               NodeTraits::get_parent(z) :  // makes leftmost == header if z == root
               tree_algorithms::minimum (x));
         }
         if(NodeTraits::get_right(header) == z){
            NodeTraits::set_right(header, !NodeTraits::get_left(z) ?        // z->get_right() must be null also
                              NodeTraits::get_parent(z) :  // makes rightmost == header if z == root
                              tree_algorithms::maximum(x));
         }
      }

      info.x = x;
      info.x_parent = x_parent;
      info.y = y;
   }

};

}  //namespace detail {
}  //namespace intrusive {

#include "config_end.hpp"



