/* Copyright (C) 1995-2018 Free Software Foundation, Inc.

   This file is part of the GNU C Library.

   Contributed by Bernd Schmidt <crux@Pool.Informatik.RWTH-Aachen.DE>, 1997.

   The GNU C Library is free software; you can redistribute it and/or

   modify it under the terms of the GNU Lesser General Public

   License as published by the Free Software Foundation; either

   version 2.1 of the License, or (at your option) any later version.

   The GNU C Library is distributed in the hope that it will be useful,

   but WITHOUT ANY WARRANTY; without even the implied warranty of

   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public

   License along with the GNU C Library; if not, see

   <http://www.gnu.org/licenses/>.  */

/* Tree search for red/black trees.

   The algorithm for adding nodes is taken from one of the many "Algorithms"

   books by Robert Sedgewick, although the implementation differs.

   The algorithm for deleting nodes can probably be found in a book named

   "Introduction to Algorithms" by Cormen/Leiserson/Rivest.  At least that's

   the book that my professor took most algorithms from during the "Data

   Structures" course...

   Totally public domain.  */

/* Red/black trees are binary trees in which the edges are colored either red

   or black.  They have the following properties:

   1. The number of black edges on every path from the root to a leaf is

      constant.

   2. No two red edges are adjacent.

   Therefore there is an upper bound on the length of every path, it's

   O(log n) where n is the number of nodes in the tree.  No path can be longer

   than 1+2*P where P is the length of the shortest path in the tree.

   Useful for the implementation:

   3. If one of the children of a node is NULL, then the other one is red

      (if it exists).

   In the implementation, not the edges are colored, but the nodes.  The color

   interpreted as the color of the edge leading to this node.  The color is

   meaningless for the root node, but we color the root node black for

   convenience.  All added nodes are red initially.

   Adding to a red/black tree is rather easy.  The right place is searched

   with a usual binary tree search.  Additionally, whenever a node N is

   reached that has two red successors, the successors are colored black and

   the node itself colored red.  This moves red edges up the tree where they

   pose less of a problem once we get to really insert the new node.  Changing

   N's color to red may violate rule 2, however, so rotations may become

   necessary to restore the invariants.  Adding a new red leaf may violate

   the same rule, so afterwards an additional check is run and the tree

   possibly rotated.

   Deleting is hairy.  There are mainly two nodes involved: the node to be

   deleted (n1), and another node that is to be unchained from the tree (n2).

   If n1 has a successor (the node with a smallest key that is larger than

   n1), then the successor becomes n2 and its contents are copied into n1,

   otherwise n1 becomes n2.

   Unchaining a node may violate rule 1: if n2 is black, one subtree is

   missing one black edge afterwards.  The algorithm must try to move this

   error upwards towards the root, so that the subtree that does not have

   enough black edges becomes the whole tree.  Once that happens, the error

   has disappeared.  It may not be necessary to go all the way up, since it

   is possible that rotations and recoloring can fix the error before that.

   Although the deletion algorithm must walk upwards through the tree, we

   do not store parent pointers in the nodes.  Instead, delete allocates a

   small array of parent pointers and fills it while descending the tree.

   Since we know that the length of a path is O(log n), where n is the number

   of nodes, this is likely to use less memory.  */

/* Tree rotations look like this:

      A                C

     / \              / \

    B   C            A   G

   / \ / \  -->     / \

   D E F G         B   F

                  / \

                 D   E

   In this case, A has been rotated left.  This preserves the ordering of the

   binary tree.  */

#include <assert.h>

#include <stdalign.h>

#include <stddef.h>

#include <stdlib.h>

#include <string.h>

#include <search.h>

/* Assume malloc returns naturally aligned (alignof (max_align_t))

   pointers so we can use the low bits to store some extra info.  This

   works for the left/right node pointers since they are not user

   visible and always allocated by malloc.  The user provides the key

   pointer and so that can point anywhere and doesn't have to be

   aligned.  */

#define USE_MALLOC_LOW_BIT 1

#ifndef USE_MALLOC_LOW_BIT

typedef struct node_t

{

  /* Callers expect this to be the first element in the structure - do not

     move!  */

  const void *key;

  struct node_t *left_node;

  struct node_t *right_node;

  unsigned int is_red:1;

} *node;

#define RED(N) (N)->is_red

#define SETRED(N) (N)->is_red = 1

#define SETBLACK(N) (N)->is_red = 0

#define SETNODEPTR(NP,P) (*NP) = (P)

#define LEFT(N) (N)->left_node

#define LEFTPTR(N) (&(N)->left_node)

#define SETLEFT(N,L) (N)->left_node = (L)

#define RIGHT(N) (N)->right_node

#define RIGHTPTR(N) (&(N)->right_node)

#define SETRIGHT(N,R) (N)->right_node = (R)

#define DEREFNODEPTR(NP) (*(NP))

#else /* USE_MALLOC_LOW_BIT */

typedef struct node_t

{

  /* Callers expect this to be the first element in the structure - do not

     move!  */

  const void *key;

  uintptr_t left_node; /* Includes whether the node is red in low-bit. */

  uintptr_t right_node;

} *node;

#define RED(N) (node)((N)->left_node & ((uintptr_t) 0x1))

#define SETRED(N) (N)->left_node |= ((uintptr_t) 0x1)

#define SETBLACK(N) (N)->left_node &= ~((uintptr_t) 0x1)

#define SETNODEPTR(NP,P) (*NP) = (node)((((uintptr_t)(*NP)) \

					 & (uintptr_t) 0x1) | (uintptr_t)(P))

#define LEFT(N) (node)((N)->left_node & ~((uintptr_t) 0x1))

#define LEFTPTR(N) (node *)(&(N)->left_node)

#define SETLEFT(N,L) (N)->left_node = (((N)->left_node & (uintptr_t) 0x1) \

				       | (uintptr_t)(L))

#define RIGHT(N) (node)((N)->right_node)

#define RIGHTPTR(N) (node *)(&(N)->right_node)

#define SETRIGHT(N,R) (N)->right_node = (uintptr_t)(R)

#define DEREFNODEPTR(NP) (node)((uintptr_t)(*(NP)) & ~((uintptr_t) 0x1))

#endif /* USE_MALLOC_LOW_BIT */

typedef const struct node_t *const_node;

#undef DEBUGGING

#ifdef DEBUGGING

/* Routines to check tree invariants.  */

#define CHECK_TREE(a) check_tree(a)

static void

check_tree_recurse (node p, int d_sofar, int d_total)

{

  if (p == NULL)

    {

      assert (d_sofar == d_total);

      return;

    }

  check_tree_recurse (LEFT(p), d_sofar + (LEFT(p) && !RED(LEFT(p))),

		      d_total);

  check_tree_recurse (RIGHT(p), d_sofar + (RIGHT(p) && !RED(RIGHT(p))),

		      d_total);

  if (LEFT(p))

    assert (!(RED(LEFT(p)) && RED(p)));

  if (RIGHT(p))

    assert (!(RED(RIGHT(p)) && RED(p)));

}

static void

check_tree (node root)

{

  int cnt = 0;

  node p;

  if (root == NULL)

    return;

  SETBLACK(root);

  for(p = LEFT(root); p; p = LEFT(p))

    cnt += !RED(p);

  check_tree_recurse (root, 0, cnt);

}

#else

#define CHECK_TREE(a)

#endif

/* Possibly "split" a node with two red successors, and/or fix up two red

   edges in a row.  ROOTP is a pointer to the lowest node we visited, PARENTP

   and GPARENTP pointers to its parent/grandparent.  P_R and GP_R contain the

   comparison values that determined which way was taken in the tree to reach

   ROOTP.  MODE is 1 if we need not do the split, but must check for two red

   edges between GPARENTP and ROOTP.  */

static void

maybe_split_for_insert (node *rootp, node *parentp, node *gparentp,

			int p_r, int gp_r, int mode)

{

  node root = DEREFNODEPTR(rootp);

  node *rp, *lp;

  node rpn, lpn;

  rp = RIGHTPTR(root);

  rpn = RIGHT(root);

  lp = LEFTPTR(root);

  lpn = LEFT(root);

  /* See if we have to split this node (both successors red).  */

  if (mode == 1

      || ((rpn) != NULL && (lpn) != NULL && RED(rpn) && RED(lpn)))

    {

      /* This node becomes red, its successors black.  */

      SETRED(root);

      if (rpn)

	SETBLACK(rpn);

      if (lpn)

	SETBLACK(lpn);

      /* If the parent of this node is also red, we have to do

	 rotations.  */

      if (parentp != NULL && RED(DEREFNODEPTR(parentp)))

	{

	  node gp = DEREFNODEPTR(gparentp);

	  node p = DEREFNODEPTR(parentp);

	  /* There are two main cases:

	     1. The edge types (left or right) of the two red edges differ.

	     2. Both red edges are of the same type.

	     There exist two symmetries of each case, so there is a total of

	     4 cases.  */

	  if ((p_r > 0) != (gp_r > 0))

	    {

	      /* Put the child at the top of the tree, with its parent

		 and grandparent as successors.  */

	      SETRED(p);

	      SETRED(gp);

	      SETBLACK(root);

	      if (p_r < 0)

		{

		  /* Child is left of parent.  */

		  SETLEFT(p,rpn);

		  SETNODEPTR(rp,p);

		  SETRIGHT(gp,lpn);

		  SETNODEPTR(lp,gp);

		}

	      else

		{

		  /* Child is right of parent.  */

		  SETRIGHT(p,lpn);

		  SETNODEPTR(lp,p);

		  SETLEFT(gp,rpn);

		  SETNODEPTR(rp,gp);

		}

	      SETNODEPTR(gparentp,root);

	    }

	  else

	    {

	      SETNODEPTR(gparentp,p);

	      /* Parent becomes the top of the tree, grandparent and

		 child are its successors.  */

	      SETBLACK(p);

	      SETRED(gp);

	      if (p_r < 0)

		{

		  /* Left edges.  */

		  SETLEFT(gp,RIGHT(p));

		  SETRIGHT(p,gp);

		}

	      else

		{

		  /* Right edges.  */

		  SETRIGHT(gp,LEFT(p));

		  SETLEFT(p,gp);

		}

	    }

	}

    }

}

/* Find or insert datum into search tree.

   KEY is the key to be located, ROOTP is the address of tree root,

   COMPAR the ordering function.  */

void *

__tsearch (const void *key, void **vrootp, __compar_fn_t compar)

{

  node q, root;

  node *parentp = NULL, *gparentp = NULL;

  node *rootp = (node *) vrootp;

  node *nextp;

  int r = 0, p_r = 0, gp_r = 0; /* No they might not, Mr Compiler.  */

#ifdef USE_MALLOC_LOW_BIT

  static_assert (alignof (max_align_t) > 1, "malloc must return aligned ptrs");

#endif

  if (rootp == NULL)

    return NULL;

  /* This saves some additional tests below.  */

  root = DEREFNODEPTR(rootp);

  if (root != NULL)

    SETBLACK(root);

  CHECK_TREE (root);

  nextp = rootp;

  while (DEREFNODEPTR(nextp) != NULL)

    {

      root = DEREFNODEPTR(rootp);

      r = (*compar) (key, root->key);

      if (r == 0)

	return root;

      maybe_split_for_insert (rootp, parentp, gparentp, p_r, gp_r, 0);

      /* If that did any rotations, parentp and gparentp are now garbage.

	 That doesn't matter, because the values they contain are never

	 used again in that case.  */

      nextp = r < 0 ? LEFTPTR(root) : RIGHTPTR(root);

      if (DEREFNODEPTR(nextp) == NULL)

	break;

      gparentp = parentp;

      parentp = rootp;

      rootp = nextp;

      gp_r = p_r;

      p_r = r;

    }

  q = (struct node_t *) malloc (sizeof (struct node_t));

  if (q != NULL)

    {

      /* Make sure the malloc implementation returns naturally aligned

	 memory blocks when expected.  Or at least even pointers, so we

	 can use the low bit as red/black flag.  Even though we have a

	 static_assert to make sure alignof (max_align_t) > 1 there could

	 be an interposed malloc implementation that might cause havoc by

	 not obeying the malloc contract.  */

#ifdef USE_MALLOC_LOW_BIT

      assert (((uintptr_t) q & (uintptr_t) 0x1) == 0);

#endif

      SETNODEPTR(nextp,q);		/* link new node to old */

      q->key = key;			/* initialize new node */

      SETRED(q);

      SETLEFT(q,NULL);

      SETRIGHT(q,NULL);

      if (nextp != rootp)

	/* There may be two red edges in a row now, which we must avoid by

	   rotating the tree.  */

	maybe_split_for_insert (nextp, rootp, parentp, r, p_r, 1);

    }

  return q;

}

libc_hidden_def (__tsearch)

weak_alias (__tsearch, tsearch)

/* Find datum in search tree.

   KEY is the key to be located, ROOTP is the address of tree root,

   COMPAR the ordering function.  */

void *

__tfind (const void *key, void *const *vrootp, __compar_fn_t compar)

{

  node root;

  node *rootp = (node *) vrootp;

  if (rootp == NULL)

    return NULL;

  root = DEREFNODEPTR(rootp);

  CHECK_TREE (root);

  while (DEREFNODEPTR(rootp) != NULL)

    {

      root = DEREFNODEPTR(rootp);

      int r;

      r = (*compar) (key, root->key);

      if (r == 0)

	return root;

      rootp = r < 0 ? LEFTPTR(root) : RIGHTPTR(root);

    }

  return NULL;

}

libc_hidden_def (__tfind)

weak_alias (__tfind, tfind)

/* Delete node with given key.

   KEY is the key to be deleted, ROOTP is the address of the root of tree,

   COMPAR the comparison function.  */

void *

__tdelete (const void *key, void **vrootp, __compar_fn_t compar)

{

  node p, q, r, retval;

  int cmp;

  node *rootp = (node *) vrootp;

  node root, unchained;

  /* Stack of nodes so we remember the parents without recursion.  It's

     _very_ unlikely that there are paths longer than 40 nodes.  The tree

     would need to have around 250.000 nodes.  */

  int stacksize = 40;

  int sp = 0;

  node **nodestack = alloca (sizeof (node *) * stacksize);

  if (rootp == NULL)

    return NULL;

  p = DEREFNODEPTR(rootp);

  if (p == NULL)

    return NULL;

  CHECK_TREE (p);

  root = DEREFNODEPTR(rootp);

  while ((cmp = (*compar) (key, root->key)) != 0)

    {

      if (sp == stacksize)

	{

	  node **newstack;

	  stacksize += 20;

	  newstack = alloca (sizeof (node *) * stacksize);

	  nodestack = memcpy (newstack, nodestack, sp * sizeof (node *));

	}

      nodestack[sp++] = rootp;

      p = DEREFNODEPTR(rootp);

      if (cmp < 0)

	{

	  rootp = LEFTPTR(p);

	  root = LEFT(p);

	}

      else

	{

	  rootp = RIGHTPTR(p);

	  root = RIGHT(p);

	}

      if (root == NULL)

	return NULL;

    }

  /* This is bogus if the node to be deleted is the root... this routine

     really should return an integer with 0 for success, -1 for failure

     and errno = ESRCH or something.  */

  retval = p;

  /* We don't unchain the node we want to delete. Instead, we overwrite

     it with its successor and unchain the successor.  If there is no

     successor, we really unchain the node to be deleted.  */

  root = DEREFNODEPTR(rootp);

  r = RIGHT(root);

  q = LEFT(root);

  if (q == NULL || r == NULL)

    unchained = root;

  else

    {

      node *parentp = rootp, *up = RIGHTPTR(root);

      node upn;

      for (;;)

	{

	  if (sp == stacksize)

	    {

	      node **newstack;

	      stacksize += 20;

	      newstack = alloca (sizeof (node *) * stacksize);

	      nodestack = memcpy (newstack, nodestack, sp * sizeof (node *));

	    }

	  nodestack[sp++] = parentp;

	  parentp = up;

	  upn = DEREFNODEPTR(up);

	  if (LEFT(upn) == NULL)

	    break;

	  up = LEFTPTR(upn);

	}

      unchained = DEREFNODEPTR(up);

    }

  /* We know that either the left or right successor of UNCHAINED is NULL.

     R becomes the other one, it is chained into the parent of UNCHAINED.  */

  r = LEFT(unchained);

  if (r == NULL)

    r = RIGHT(unchained);

  if (sp == 0)

    SETNODEPTR(rootp,r);

  else

    {

      q = DEREFNODEPTR(nodestack[sp-1]);

      if (unchained == RIGHT(q))

	SETRIGHT(q,r);

      else

	SETLEFT(q,r);

    }

  if (unchained != root)

    root->key = unchained->key;

  if (!RED(unchained))

    {

      /* Now we lost a black edge, which means that the number of black

	 edges on every path is no longer constant.  We must balance the

	 tree.  */

      /* NODESTACK now contains all parents of R.  R is likely to be NULL

	 in the first iteration.  */

      /* NULL nodes are considered black throughout - this is necessary for

	 correctness.  */

      while (sp > 0 && (r == NULL || !RED(r)))

	{

	  node *pp = nodestack[sp - 1];

	  p = DEREFNODEPTR(pp);

	  /* Two symmetric cases.  */

	  if (r == LEFT(p))

	    {

	      /* Q is R's brother, P is R's parent.  The subtree with root

		 R has one black edge less than the subtree with root Q.  */

	      q = RIGHT(p);

	      if (RED(q))

		{

		  /* If Q is red, we know that P is black. We rotate P left

		     so that Q becomes the top node in the tree, with P below

		     it.  P is colored red, Q is colored black.

		     This action does not change the black edge count for any

		     leaf in the tree, but we will be able to recognize one

		     of the following situations, which all require that Q

		     is black.  */

		  SETBLACK(q);

		  SETRED(p);

		  /* Left rotate p.  */

		  SETRIGHT(p,LEFT(q));

		  SETLEFT(q,p);

		  SETNODEPTR(pp,q);

		  /* Make sure pp is right if the case below tries to use

		     it.  */

		  nodestack[sp++] = pp = LEFTPTR(q);

		  q = RIGHT(p);

		}

	      /* We know that Q can't be NULL here.  We also know that Q is

		 black.  */

	      if ((LEFT(q) == NULL || !RED(LEFT(q)))

		  && (RIGHT(q) == NULL || !RED(RIGHT(q))))

		{

		  /* Q has two black successors.  We can simply color Q red.

		     The whole subtree with root P is now missing one black

		     edge.  Note that this action can temporarily make the

		     tree invalid (if P is red).  But we will exit the loop

		     in that case and set P black, which both makes the tree

		     valid and also makes the black edge count come out

		     right.  If P is black, we are at least one step closer

		     to the root and we'll try again the next iteration.  */

		  SETRED(q);

		  r = p;

		}

	      else

		{

		  /* Q is black, one of Q's successors is red.  We can

		     repair the tree with one operation and will exit the

		     loop afterwards.  */

		  if (RIGHT(q) == NULL || !RED(RIGHT(q)))

		    {

		      /* The left one is red.  We perform the same action as

			 in maybe_split_for_insert where two red edges are

			 adjacent but point in different directions:

			 Q's left successor (let's call it Q2) becomes the

			 top of the subtree we are looking at, its parent (Q)

			 and grandparent (P) become its successors. The former

			 successors of Q2 are placed below P and Q.

			 P becomes black, and Q2 gets the color that P had.

			 This changes the black edge count only for node R and

			 its successors.  */

		      node q2 = LEFT(q);

		      if (RED(p))

			SETRED(q2);

		      else

			SETBLACK(q2);

		      SETRIGHT(p,LEFT(q2));

		      SETLEFT(q,RIGHT(q2));

		      SETRIGHT(q2,q);

		      SETLEFT(q2,p);

		      SETNODEPTR(pp,q2);

		      SETBLACK(p);

		    }

		  else

		    {

		      /* It's the right one.  Rotate P left. P becomes black,

			 and Q gets the color that P had.  Q's right successor

			 also becomes black.  This changes the black edge

			 count only for node R and its successors.  */

		      if (RED(p))

			SETRED(q);

		      else

			SETBLACK(q);

		      SETBLACK(p);

		      SETBLACK(RIGHT(q));

		      /* left rotate p */

		      SETRIGHT(p,LEFT(q));

		      SETLEFT(q,p);

		      SETNODEPTR(pp,q);

		    }

		  /* We're done.  */

		  sp = 1;

		  r = NULL;

		}

	    }

	  else

	    {

	      /* Comments: see above.  */

	      q = LEFT(p);

	      if (RED(q))

		{

		  SETBLACK(q);

		  SETRED(p);

		  SETLEFT(p,RIGHT(q));

		  SETRIGHT(q,p);

		  SETNODEPTR(pp,q);

		  nodestack[sp++] = pp = RIGHTPTR(q);

		  q = LEFT(p);

		}

	      if ((RIGHT(q) == NULL || !RED(RIGHT(q)))

		  && (LEFT(q) == NULL || !RED(LEFT(q))))

		{

		  SETRED(q);

		  r = p;

		}

	      else

		{

		  if (LEFT(q) == NULL || !RED(LEFT(q)))

		    {

		      node q2 = RIGHT(q);

		      if (RED(p))

			SETRED(q2);

		      else

			SETBLACK(q2);

		      SETLEFT(p,RIGHT(q2));

		      SETRIGHT(q,LEFT(q2));

		      SETLEFT(q2,q);

		      SETRIGHT(q2,p);

		      SETNODEPTR(pp,q2);

		      SETBLACK(p);

		    }

		  else

		    {

		      if (RED(p))

			SETRED(q);

		      else

			SETBLACK(q);

		      SETBLACK(p);

		      SETBLACK(LEFT(q));

		      SETLEFT(p,RIGHT(q));

		      SETRIGHT(q,p);

		      SETNODEPTR(pp,q);

		    }

		  sp = 1;

		  r = NULL;

		}

	    }

	  --sp;

	}

      if (r != NULL)

	SETBLACK(r);

    }

  free (unchained);

  return retval;

}

libc_hidden_def (__tdelete)

weak_alias (__tdelete, tdelete)

/* Walk the nodes of a tree.

   ROOT is the root of the tree to be walked, ACTION the function to be

   called at each node.  LEVEL is the level of ROOT in the whole tree.  */

static void

trecurse (const void *vroot, __action_fn_t action, int level)

{

  const_node root = (const_node) vroot;

  if (LEFT(root) == NULL && RIGHT(root) == NULL)

    (*action) (root, leaf, level);

  else

    {

      (*action) (root, preorder, level);

      if (LEFT(root) != NULL)

	trecurse (LEFT(root), action, level + 1);

      (*action) (root, postorder, level);

      if (RIGHT(root) != NULL)

	trecurse (RIGHT(root), action, level + 1);

      (*action) (root, endorder, level);

    }

}

/* Walk the nodes of a tree.

   ROOT is the root of the tree to be walked, ACTION the function to be

   called at each node.  */

void

__twalk (const void *vroot, __action_fn_t action)

{

  const_node root = (const_node) vroot;

  CHECK_TREE ((node) root);

  if (root != NULL && action != NULL)

    trecurse (root, action, 0);

}

libc_hidden_def (__twalk)

weak_alias (__twalk, twalk)

/* The standardized functions miss an important functionality: the

   tree cannot be removed easily.  We provide a function to do this.  */

static void

tdestroy_recurse (node root, __free_fn_t freefct)

{

  if (LEFT(root) != NULL)

    tdestroy_recurse (LEFT(root), freefct);

  if (RIGHT(root) != NULL)

    tdestroy_recurse (RIGHT(root), freefct);

  (*freefct) ((void *) root->key);

  /* Free the node itself.  */

  free (root);

}

void

__tdestroy (void *vroot, __free_fn_t freefct)

{

  node root = (node) vroot;

  CHECK_TREE (root);

  if (root != NULL)

    tdestroy_recurse (root, freefct);

}

libc_hidden_def (__tdestroy)

weak_alias (__tdestroy, tdestroy)