#include <stdio.h>
#include <stdlib.h>
#include <string.h>


/*
 * Misc cut&paste from kernel code
 */
#define BITS_X_LONG	(8 * sizeof(long))

#define likely(x)	__builtin_expect(!!(x), 1)
#define unlikely(e)	__builtin_expect(!!(e), 0)

struct list_head {
	struct list_head *next, *prev;
};

#define LIST_HEAD_INIT(name) { &(name), &(name) }

#define LIST_HEAD(name) \
	struct list_head name = LIST_HEAD_INIT(name)

#define offsetof(type, member) ((long) &((type *) 0)->member)
#define container_of(ptr, type, member) ({			\
        const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
        (type *)( (char *)__mptr - offsetof(type,member) );})

#define list_entry(ptr, type, member) \
	container_of(ptr, type, member)

static inline void INIT_LIST_HEAD(struct list_head *list) {
	list->next = list;
	list->prev = list;
}
static inline void __list_add(struct list_head *new,
			      struct list_head *prev,
			      struct list_head *next) {
	next->prev = new;
	new->next = next;
	new->prev = prev;
	prev->next = new;
}
static inline void list_add_tail(struct list_head *new, struct list_head *head) {
	__list_add(new, head->prev, head);
}
static inline void __list_del(struct list_head * prev, struct list_head * next) {
	next->prev = prev;
	prev->next = next;
}
static inline void list_del(struct list_head *entry) {
	__list_del(entry->prev, entry->next);
	entry->next = NULL;
	entry->prev = NULL;
}
static inline int list_empty(const struct list_head *head) {
	return head->next == head;
}
/*
 * Too much GCC versioning assembly there. Using dumb versions.
 */
static inline void __set_bit(unsigned int nr, volatile unsigned long *addr) {
	addr[nr / BITS_X_LONG] |= 1UL << (nr % BITS_X_LONG);
}
static inline void __clear_bit(unsigned int nr, volatile unsigned long *addr) {
	addr[nr / BITS_X_LONG] &= ~(1UL << (nr % BITS_X_LONG));
}
#if defined(__x86_64__)
static inline unsigned long __ffs(unsigned long word) {
	__asm__("bsfq %1,%0"
		:"=r" (word)
		:"rm" (word));
	return word;
}
#define rdtscll(val) do { \
	unsigned int __a, __d; \
	asm volatile("rdtsc" : "=a" (__a), "=d" (__d)); \
	(val) = ((unsigned long) __a) | (((unsigned long) __d) << 32); \
} while (0)
#elif defined(__i386__)
static inline unsigned long __ffs(unsigned long word) {
	__asm__("bsfl %1,%0"
		:"=r" (word)
		:"rm" (word));
	return word;
}
#define rdtscll(val) \
	__asm__ __volatile__("rdtsc" : "=A" (val))
#else
#error "it's 2007 and you're using which CPU?"
#endif

typedef unsigned long long nstime_t;

/******************************************************************************/
/* GAVL from uLUt libraries */

/*
  (C) Copyright 2003-2004 by Pavel Pisa - Originator

  The uLan utilities library can be used, copied and modified under
  next licenses
    - GPL - GNU General Public License
    - LGPL - GNU Lesser General Public License
    - MPL - Mozilla Public License
    - and other licenses added by project originators
*/

#if 0

/* Compule against generic GAVL support found in uLUt library */
#include "ul_gavl.h"
#include "ul_gavlcust.h"
#include "ul_gavlrepcust.h"
#include "ul_gavlflesint.h"

#else

/* We do not want external dependencies there*/

/*** Common GAVL code headers ***/

/* Add support to work correctly even with unbalamced tree
   with hdiff out of range <-1,0,+1>, else unbalanced tree
   results in continuous tree degradation and even fatal errors.
   This option does not solve errors caused by incorrectly 
   assigned hdiff values.
 */
#define GAVL_UNBALANCED_SUPPORT

/**
 * struct gavl_node - Structure Representing Node of Generic AVL Tree
 * @left:	pointer to left child or NULL
 * @right:	pointer to right child or NULL
 * @parent:	pointer to parent node, NULL for root
 * @hdiff:	difference of height between left and right child
 *
 * This structure represents one node in the tree and links @left and @right
 * to nodes with lower and higher value of order criterion.
 * Each tree is built from one type of items defined by user.
 * User can decide to include node structure inside item representation
 * or GAVL can malloc node structures for each inserted item.
 * The GAVL allocates memory space with capacity 
 * sizeof(gavl_node_t)+sizeof(void*) in the second case. The item pointer
 * is stored following node structure (void**)(node+1);
 */
typedef struct gavl_node {
  struct gavl_node *left;
  struct gavl_node *right;
  struct gavl_node *parent;
  int hdiff;
} gavl_node_t;


#define GAVL_FANY 0
#define GAVL_FFIRST 1
#define GAVL_FAFTER 2
#define GAVL_FCMP 0x80

/**
 * struct gavl_root - Structure Representing Root of Generic AVL Tree
 * @root:	pointer to root node of GAVL tree
 */
typedef struct{
  gavl_node_t *root;
  gavl_node_t *first;
  gavl_node_t *last;
  long count;
} gavl_fles_int_root_field_t;

#define GAVL_FLES_INT_DEC(cust_prefix, cust_root_t, cust_item_t, cust_key_t,\
		cust_root_field, cust_item_node, cust_item_key, cust_cmp_fnc) \
\
static inline cust_item_t * \
cust_prefix##_node2item(const cust_root_t *root, const gavl_node_t *node) \
  {return (cust_item_t*)((char*)node-(long)&((cust_item_t*)0)->cust_item_node);}\
\
static inline cust_key_t *\
cust_prefix##_node2key(const cust_root_t *root, gavl_node_t *node)\
  { return &(cust_prefix##_node2item(root, node)->cust_item_key);}\
\
void cust_prefix##_init_root_field(cust_root_t *root);\
int cust_prefix##_search_node(const cust_root_t *root, cust_key_t *key, gavl_node_t **nodep);\
cust_item_t *cust_prefix##_find(const cust_root_t *root, cust_key_t *key);\
cust_item_t *cust_prefix##_find_first(const cust_root_t *root, cust_key_t *key);\
cust_item_t *cust_prefix##_find_after(const cust_root_t *root, cust_key_t *key);\
int cust_prefix##_insert(cust_root_t *root, cust_item_t *item);\
cust_item_t *cust_prefix##_cut_first(cust_root_t *root);\
int cust_prefix##_delete_node(cust_root_t *root, gavl_node_t *node);\
int cust_prefix##_delete(cust_root_t *root, cust_item_t *item);\
\
static inline void \
cust_prefix##_init_detached(cust_item_t *item){\
  item->cust_item_node.parent=NULL;\
}\
static inline gavl_node_t *\
cust_prefix##_first_node(const cust_root_t *root)\
{\
  return root->cust_root_field.first;\
}\
static inline gavl_node_t *\
cust_prefix##_last_node(const cust_root_t *root)\
{\
  return root->cust_root_field.last;\
}\
static inline cust_item_t *\
cust_prefix##_first(const cust_root_t *root)\
{\
  gavl_node_t *n=cust_prefix##_first_node(root);\
  return n?cust_prefix##_node2item(root,n):NULL;\
}\
static inline cust_item_t *\
cust_prefix##_last(const cust_root_t *root)\
{\
  gavl_node_t *n=cust_prefix##_last_node(root);\
  return n?cust_prefix##_node2item(root,n):NULL;\
}\
static inline cust_item_t *\
cust_prefix##_next(const cust_root_t *root, cust_item_t *item)\
{\
  gavl_node_t *n=gavl_next_node(&item->cust_item_node);\
  return n?cust_prefix##_node2item(root,n):NULL;\
}\
static inline cust_item_t *\
cust_prefix##_prev(const cust_root_t *root, cust_item_t *item)\
{\
  gavl_node_t *n=gavl_prev_node(&item->cust_item_node);\
  return n?cust_prefix##_node2item(root,n):NULL;\
}\
static inline int \
cust_prefix##_is_empty(const cust_root_t *root)\
{\
  return !root->cust_root_field.root;\
}

/**
 * GAVL_FLES_INT_IMP - Implementation of new custom tree with fast first/last functions
 * @cust_prefix:	defines prefix for builded function names 
 * @cust_root_t:	user defined structure type of root of the tree
 * @cust_item_t:	user defined structure type of items stored in the tree
 * @cust_key_t:		type of the key used for sorting of the items
 * @cust_root_field:	the field of the root structure pointing to the tree root node 
 * @cust_item_node:	the field of item structure used for chaining of items
 * @cust_item_key:	the key field of item structure defining order of items
 * @cust_cmp_fnc:	the keys compare function 
 *
 * This version of custom tree implementation allows multiple items with same
 * key value to be stored in tree.
 * There are two macros designed for building custom AVL trees. The macro
 * %GAVL_CUST_NODE_INT_DEC declares functions for custom tree manipulations
 * and is intended for use in header files.
 * The macro %GAVL_CUST_NODE_INT_REP_IMP builds implementations for non-inlined
 * functions declared by %GAVL_CUST_NODE_INT_DEC. The @cust_cmp_fnc is used
 * for comparison of item keys in the search and insert functions. The types
 * of two input arguments of @cust_cmp_fnc functions must correspond 
 * with @cust_key_t type. Return value should be positive for case when
 * the first pointed key value is greater then second, negative for reverse
 * case and zero for equal pointed values.
 */
#define GAVL_FLES_INT_IMP(cust_prefix, cust_root_t, cust_item_t, cust_key_t,\
		cust_root_field, cust_item_node, cust_item_key, cust_cmp_fnc,\
		cust_ins_fl, cust_first_change, cust_last_change, cust_empty_state) \
\
void cust_prefix##_init_root_field(cust_root_t *root)\
{\
  root->cust_root_field.root=NULL;\
  root->cust_root_field.first=NULL;\
  root->cust_root_field.last=NULL;\
  root->cust_root_field.count=0;\
}\
\
int cust_prefix##_search_node4(const cust_root_t *root, cust_key_t *key, gavl_node_t **nodep, int mode)\
{\
  int cmp=1;\
  gavl_node_t *n, *p;\
  n=p=root->cust_root_field.root;\
  while(n){\
    cmp=cust_cmp_fnc(cust_prefix##_node2key(root,n),key);\
    p=n;\
    if(cmp>0){\
      n=n->left;\
    }else if((cmp<0)||(mode&GAVL_FAFTER)){\
      n=n->right;\
    }else{\
      break;\
    }\
  }\
  if(!cmp && (mode&GAVL_FFIRST)){\
    while((n=p->left)){\
      cmp=cust_cmp_fnc(cust_prefix##_node2key(root,n),key);\
      if(!cmp){\
        p=n;\
      }else{\
        while((n=n->right)){\
          cmp=cust_cmp_fnc(cust_prefix##_node2key(root,n),key);\
          if(!cmp){\
            p=n;\
            break;\
          }\
        }\
        if(cmp) break;\
      }\
    }\
    cmp=0;\
  }\
  *nodep=p;\
  return cmp;\
}\
\
int cust_prefix##_search_node(const cust_root_t *root, cust_key_t *key, gavl_node_t **nodep)\
{\
  return cust_prefix##_search_node4(root, key, nodep, 0);\
}\
\
cust_item_t *cust_prefix##_find(const cust_root_t *root, cust_key_t *key)\
{\
  gavl_node_t *node;\
  if(cust_prefix##_search_node4(root, key, &node, 0))\
    return NULL;\
  return cust_prefix##_node2item(root,node);\
}\
\
cust_item_t *cust_prefix##_find_first(const cust_root_t *root, cust_key_t *key)\
{\
  gavl_node_t *n;\
  if(cust_prefix##_search_node4(root, key, &n, GAVL_FFIRST))\
    return NULL;\
  return cust_prefix##_node2item(root,n);\
}\
\
cust_item_t *cust_prefix##_find_after(const cust_root_t *root, cust_key_t *key)\
{\
  gavl_node_t *node;\
  if(cust_prefix##_search_node4(root, key, &node, GAVL_FAFTER)<=0){\
     if(node) node=gavl_next_node(node);\
  }\
  return node?cust_prefix##_node2item(root,node):NULL;\
}\
\
int cust_prefix##_insert(cust_root_t *root, cust_item_t *item)\
{\
  int cmp;\
  gavl_node_t *where, *n2add;\
  \
  cmp=cust_prefix##_search_node4(root, &item->cust_item_key, &where, cust_ins_fl);\
  if(!cmp && !(cust_ins_fl&GAVL_FAFTER)) return -1;\
  n2add=&item->cust_item_node;\
  if(!root->cust_root_field.root){\
    root->cust_root_field.first=root->cust_root_field.last=n2add;\
    cust_first_change; cust_last_change;\
  }else if((cmp>0)&&(where==root->cust_root_field.first)){\
    root->cust_root_field.first=n2add;\
    cust_first_change;\
  }else if((cmp<=0)&&(where==root->cust_root_field.last)){\
    root->cust_root_field.last=n2add;\
    cust_last_change;\
  }\
  root->cust_root_field.count++;\
  return gavl_insert_primitive_at(&root->cust_root_field.root, n2add, where, cmp);\
}\
\
int cust_prefix##_delete_node(cust_root_t *root, gavl_node_t *node)\
{\
  gavl_node_t *p;\
  /*check if node is inserted into tree*/\
  for(p=node; p->parent; p=p->parent);\
  if(p!=root->cust_root_field.root)\
    return -1;\
  if(root->cust_root_field.first==node){\
    if(root->cust_root_field.last==node){\
      root->cust_root_field.first=root->cust_root_field.last=NULL;\
      cust_empty_state;\
    }else{\
      root->cust_root_field.first=gavl_next_node(node);\
      cust_first_change;\
    }\
  }else if(root->cust_root_field.last==node){\
    root->cust_root_field.last=gavl_prev_node(node);\
    cust_last_change;\
  }\
  root->cust_root_field.count--;\
  return gavl_delete_primitive(&root->cust_root_field.root, node);\
}\
\
int cust_prefix##_delete(cust_root_t *root, cust_item_t *item)\
{\
  gavl_node_t *n;\
  if(!item) return -1;\
  n=&item->cust_item_node;\
  return cust_prefix##_delete_node(root, n);\
}\
\
cust_item_t *cust_prefix##_cut_first(cust_root_t *root)\
{\
  gavl_node_t *n, *p;\
  gavl_node_t **np=&root->cust_root_field.root;\
  if(!(n=root->cust_root_field.first))\
    return NULL;\
  if(n->parent) np=&n->parent->left;\
  if((*np=n->right)){\
    p=n->right;\
    p->parent=n->parent;\
    while(p->left) p=p->left;\
    root->cust_root_field.first=p;\
    cust_first_change;\
  }else{\
    if(!(root->cust_root_field.first=n->parent)){\
      root->cust_root_field.last=n->parent;\
      cust_empty_state;\
    }else{\
      cust_first_change;\
    }\
  }\
  for(p=n->parent;p;p=p->parent)\
    if(p->hdiff++<0) break;\
  n->parent=n->left=n->right=NULL;\
  root->cust_root_field.count--;\
  return cust_prefix##_node2item(root,n);\
}

/*** Common GAVL code implementation ***/

#define GAVL_HDIFF_DEBUG(tree, mode)

/**
 * gavl_next_node - Returns Next Node of GAVL Tree
 * @node:	node for which accessor is looked for
 *
 * Return Value: pointer to next node of tree according to ordering
 */
gavl_node_t *
gavl_next_node(const gavl_node_t *node)
{
  gavl_node_t *n;
  if((n=node->right)){
    while(n->left) n=n->left;
    return n;
  } else {
    while((n=node->parent)){
      if(n->right!=node) break;
      node=n;
    }
    return n;
  }
}

/**
 * gavl_prev_node - Returns Previous Node of GAVL Tree
 * @node:	node for which predecessor is looked for
 *
 * Return Value: pointer to previous node of tree according to ordering
 */
gavl_node_t *
gavl_prev_node(const gavl_node_t *node)
{
  gavl_node_t *n;
  if((n=node->left)){
    while(n->right) n=n->right;
    return n;
  } else {
    while((n=node->parent)){
      if(n->left!=node) break;
      node=n;
    }
    return n;
  }
}

/**
 * gavl_balance_one - Balance One Node to Enhance Balance Factor
 * @subtree:	pointer to pointer to node for which balance is enhanced
 *
 * Return Value: returns nonzero value if height of subtree is lowered by one
 */
int
gavl_balance_one(gavl_node_t **subtree)
{
  int shdiff; int ret;
  gavl_node_t *n, *p, *parent;
  
 #ifdef GAVL_UNBALANCED_SUPPORT
  if(!*subtree) return 0;
 #endif /* GAVL_UNBALANCED_SUPPORT */
  parent=(*subtree)->parent;
  shdiff=(*subtree)->hdiff;
  
  if (shdiff>1)
  {
    n=(*subtree)->left;
   #ifdef GAVL_UNBALANCED_SUPPORT
    if(!n) return 0;
   #endif /* GAVL_UNBALANCED_SUPPORT */
    if (n->hdiff>=0) {
      /* ds1=ds-dn-1; */
      /* if(ds>dn) dn1=dn-1; else dn1=ds-2; */
      (*subtree)->hdiff=shdiff-n->hdiff-1;
      ret=n->hdiff>0;
     #ifdef GAVL_UNBALANCED_SUPPORT
      if(shdiff<=n->hdiff)
	n->hdiff=shdiff-2;
      else
     #endif /* GAVL_UNBALANCED_SUPPORT */
	n->hdiff--;
      p=n->right;
      n->right=*subtree; (*subtree)->parent=n;
      (*subtree)->left=p; if(p) p->parent=*subtree;
      *subtree=n; n->parent=parent;
      GAVL_HDIFF_DEBUG(*subtree, 0);
    }else{
      p=n->right;
     #ifdef GAVL_UNBALANCED_SUPPORT
      if(!p) return 0;
     #endif /* GAVL_UNBALANCED_SUPPORT */
      shdiff-=2;
      if(p->hdiff>=0){
        /* ds1=ds-2-dp; dn1=dn+1; dp1=min(dp;ds-2); */
        (*subtree)->hdiff=shdiff-p->hdiff;
	n->hdiff++;
       #ifndef GAVL_UNBALANCED_SUPPORT
	p->hdiff=0;
       #else /* GAVL_UNBALANCED_SUPPORT */
	if (p->hdiff>shdiff) p->hdiff=shdiff;
       #endif /* GAVL_UNBALANCED_SUPPORT */
      }else{
        /* ds1=ds-2; dn1=dn+1-dp; dp1=max(dn+1;dp); */
        (*subtree)->hdiff=shdiff;
	shdiff=n->hdiff; /* shdiff reused for nhdiff */
	n->hdiff=shdiff+1-p->hdiff;
       #ifndef GAVL_UNBALANCED_SUPPORT
	p->hdiff=0;
       #else /* GAVL_UNBALANCED_SUPPORT */
	if (p->hdiff<=shdiff) p->hdiff=shdiff+1;
       #endif /* GAVL_UNBALANCED_SUPPORT */
      }
      n->right=p->left; if(p->left) p->left->parent=n;
      (*subtree)->left=p->right; if(p->right) p->right->parent=*subtree;
      p->left=n; n->parent=p;
      p->right=*subtree; (*subtree)->parent=p;
      *subtree=p; p->parent=parent;
      GAVL_HDIFF_DEBUG(*subtree, 0);
      ret=1;
    }
  }
  else if (shdiff<-1)
  {
    n=(*subtree)->right;
   #ifdef GAVL_UNBALANCED_SUPPORT
    if(!n) return 0;
   #endif /* GAVL_UNBALANCED_SUPPORT */
    if (n->hdiff<=0) {
      /* ds1=ds-dn+1; */
      /* if(ds<dn) dn1=dn+1; else dn1=ds+2; */
      (*subtree)->hdiff=shdiff-n->hdiff+1;
      ret=n->hdiff<0;
     #ifdef GAVL_UNBALANCED_SUPPORT
      if(shdiff>=n->hdiff)
	n->hdiff=shdiff+2;
      else
     #endif /* GAVL_UNBALANCED_SUPPORT */
	n->hdiff++;
      p=n->left;
      n->left=*subtree; (*subtree)->parent=n;
      (*subtree)->right=p; if(p) p->parent=*subtree;
      *subtree=n; n->parent=parent;
      GAVL_HDIFF_DEBUG(*subtree, 0);
    }else{
      p=n->left;
     #ifdef GAVL_UNBALANCED_SUPPORT
      if(!p) return 0;
     #endif /* GAVL_UNBALANCED_SUPPORT */
      shdiff+=2;
      if(p->hdiff<=0){
        /* ds1=ds+2-dp; dn1=dn-1; dp1=max(dp;ds+2); */
        (*subtree)->hdiff=shdiff-p->hdiff;
	n->hdiff--;
       #ifndef GAVL_UNBALANCED_SUPPORT
	p->hdiff=0;
       #else /* GAVL_UNBALANCED_SUPPORT */
	if (p->hdiff<shdiff) p->hdiff=shdiff;
       #endif /* GAVL_UNBALANCED_SUPPORT */
      }else{
        /* ds1=ds+2; dn1=dn-1-dp; dp1=min(dn-1;dp); */
        (*subtree)->hdiff=shdiff;
	shdiff=n->hdiff; /* shdiff reused for nhdiff */
	n->hdiff=shdiff-1-p->hdiff;
       #ifndef GAVL_UNBALANCED_SUPPORT
	p->hdiff=0;
       #else /* GAVL_UNBALANCED_SUPPORT */
	if (p->hdiff>=shdiff) p->hdiff=shdiff-1;
       #endif /* GAVL_UNBALANCED_SUPPORT */
      }
      n->left=p->right; if(p->right) p->right->parent=n;
      (*subtree)->right=p->left; if(p->left) p->left->parent=*subtree;
      p->right=n; n->parent=p;
      p->left=*subtree; (*subtree)->parent=p;
      *subtree=p; p->parent=parent;
      GAVL_HDIFF_DEBUG(*subtree, 0);
      ret=1;
    }
  } else ret=0;

  /*printf("#%d",ret);*/
  return(ret);
}

/**
 * gavl_insert_primitive_at - Low Lewel Routine to Insert Node into Tree 
 * @root_nodep:	pointer to pointer to GAVL tree root node
 * @node:	pointer to inserted node
 * @where:	pointer to found parent node
 * @leftright:	left (>=1) or right (<=0) branch
 *
 * This function can be used for implementing AVL trees with custom
 * root definition. The value of the selected @left or @right pointer
 * of provided @node has to be NULL before insert operation,
 * i.e. node has to be end node in the selected direction.
 * Return Value: positive value informs about success
 */
int
gavl_insert_primitive_at(gavl_node_t **root_nodep, gavl_node_t *node,
		    gavl_node_t *where, int leftright)
{
  int hdiff;

  node->hdiff=0;
  node->left=node->right=0;
  if(!*root_nodep){
    node->parent=NULL;
    *root_nodep=node;
    return 1;
  }
  node->parent=where;
  if(leftright>0)
    where->left=node;  /* where->avl+=1 */
  else
    where->right=node; /* where->avl-=1 */
    
  do{
    hdiff=where->hdiff;
    if(where->left==node){
      /* break if balance enhanced */
      if(where->hdiff++ <0) break;
    }else{
      /* break if balance enhanced */
      if(where->hdiff-- >0) break;
    }
    node=where;
    where=where->parent;
    if(hdiff){
      gavl_node_t **np;
      if(!where)
        np=root_nodep;
      else if(where->left==node)
        np=&where->left;
      else
        np=&where->right;
      /* if only balanced trees are supposed, then next operation
         leads to loop break for all possible cases */
      if(gavl_balance_one(np)) break;
    }
  } while(where);
    
  return 1;
}

/**
 * gavl_delete_primitive - Low Lewel Deletes/Unlinks Node from GAVL Tree 
 * @root_nodep:	pointer to pointer to GAVL tree root node
 * @node:	pointer to deleted node
 *
 * Return Value: positive value informs about success.
 */
int
gavl_delete_primitive(gavl_node_t  **root_nodep, gavl_node_t *node)
{
  int bal=0;
  int hdiff=1;
  int left_fl;
  gavl_node_t *p, *n;
  gavl_node_t **np;

  p=node->parent;
  if(node==*root_nodep){
    np=root_nodep;
    left_fl=0;
  }else if(p->left==node){
    np=&p->left;
    left_fl=1;
  }else{
    np=&p->right;
    left_fl=0;
  }
  if(!node->left){
    if(!node->right){
      /* No child */
      *np=NULL;
    }else{
      /* Right child only */
      *np=node->right;
      node->right->parent=p;
    }
  }else{
    if(!node->right){
      /* Left child only */
      *np=node->left;
      node->left->parent=p;
    }else{
      gavl_node_t *neigh;
      if(node->hdiff>=0){
        gavl_node_t **np;
        /* Use nearest left node for top of subtree */
	np=&node->left; n=*np;
	while(n->right) {np=&n->right; n=*np;}
	neigh=n;
	if((*np=n->left)) n->left->parent=n->parent;
	while(n->parent!=node){
	  n=n->parent;
	  if(bal){
	    bal=0;
            if(!gavl_balance_one(&n->right)) {hdiff=0; break;}
	  }
	  if(n->hdiff++ ==0) {hdiff=0; break;}
	  bal=(n->hdiff>0);
	}
        if(bal)
          if(!gavl_balance_one(&node->left)) hdiff=0;

	neigh->hdiff=node->hdiff;
	if(hdiff){
	  if(!(neigh->hdiff--)) hdiff=0;
	}
      }else{
        gavl_node_t **np;
        /* Use nearest right node for top of subtree */
	np=&node->right; n=*np;
	while(n->left) {np=&n->left; n=*np;}
	neigh=n;
	if((*np=n->right)) n->right->parent=n->parent; 
	while(n->parent!=node){
	  n=n->parent;
	  if(bal){
	    bal=0;
            if(!gavl_balance_one(&n->left)) {hdiff=0; break;}
	  }
	  if(n->hdiff-- ==0) {hdiff=0; break;}
	  bal=(n->hdiff<0);
 	}
        if(bal)
          if(!gavl_balance_one(&node->right)) hdiff=0;

	neigh->hdiff=node->hdiff;
	if(hdiff){
	  if(!(neigh->hdiff++)) hdiff=0;
	}
      }
      if((neigh->left=node->left)) neigh->left->parent=neigh;
      if((neigh->right=node->right)) neigh->right->parent=neigh;
      neigh->parent=node->parent;

      bal=0;
      p=node->parent;
      *np=neigh;
    }
  }
  if(hdiff) do{
    if(!p){
      if(bal)
        gavl_balance_one(root_nodep);
      break;
    }else if(left_fl){
      if(bal)
        if(!gavl_balance_one(&p->left)) break;
      /* three cases for hdiff--
       * +1 ->  0 => recurse
       *  0 -> -1 => break
       * -1 -> -2 => balance and recurse
       */
      bal=p->hdiff<0;
      if(!p->hdiff--) break;
    }else{
      if(bal)
        if(!gavl_balance_one(&p->right)) break;
      /* three cases for hdiff++
       * -1 ->  0 => recurse
       *  0 -> +1 => break
       * +1 -> +2 => balance and recurse
       */
      bal=p->hdiff>0;
      if(!p->hdiff++) break;
    }
    n=p;
    p=p->parent;
    if(p) left_fl=(p->left==n);
  }while(1);

  node->parent=node->left=node->right=NULL;
  return 1;
}

#endif

/*** CFS test code ***/

#define GAVL_TEST_FLES

struct gavl_cfs_task {
	gavl_node_t run_node;
	nstime_t t;
	/* ... */
};

struct gavl_cfs_rq {
#ifndef GAVL_TEST_FLES
	gavl_cust_root_field_t tasks_timeline;
#else /*GAVL_TEST_FLES*/
	gavl_fles_int_root_field_t tasks_timeline;
#endif /*GAVL_TEST_FLES*/
	struct gavl_node_t *rb_leftmost;
};


/* Custom tree declarations */
/* GAVL_CUST_NODE_INT_DEC - standard custom tree with internal node */
/* GAVL_FLES_INT_DEC      - tree with enhanced first last access speed  */

#ifndef GAVL_TEST_FLES
GAVL_CUST_NODE_INT_DEC(gavl_cfs, struct gavl_cfs_rq, struct gavl_cfs_task, nstime_t,
	tasks_timeline, run_node, t, gavl_cfs_cmp_fnc)
#else /*GAVL_TEST_FLES*/
GAVL_FLES_INT_DEC(gavl_cfs, struct gavl_cfs_rq, struct gavl_cfs_task, nstime_t,
	tasks_timeline, run_node, t, gavl_cfs_cmp_fnc)
#endif /*GAVL_TEST_FLES*/

static inline int
gavl_cfs_cmp_fnc(const nstime_t *a, const nstime_t *b)
{
  if (*a>*b) return 1;
  if (*a<*b) return -1;
  return 0;
}

/* Custom tree implementation */
/* GAVL_CUST_NODE_INT_IMP - version with strict ordering */
/* GAVL_CUST_NODE_INT_REP_IMP - version without strict ordering */
/* GAVL_FLES_INT_IMP      - tree with enhanced first last access speed  */

#ifndef GAVL_TEST_FLES
GAVL_CUST_NODE_INT_REP_IMP(gavl_cfs, struct gavl_cfs_rq, struct gavl_cfs_task, nstime_t,
	tasks_timeline, run_node, t, gavl_cfs_cmp_fnc)
#else /*GAVL_TEST_FLES*/
GAVL_FLES_INT_IMP(gavl_cfs, struct gavl_cfs_rq, struct gavl_cfs_task, nstime_t,
	tasks_timeline, run_node, t, gavl_cfs_cmp_fnc, GAVL_FAFTER, 
	,
	, 
	)
#endif /*GAVL_TEST_FLES*/

/******************************************************************************/

/*
 * RB-Tree code ...
 */
struct rb_node
{
	unsigned long  rb_parent_color;
#define	RB_RED		0
#define	RB_BLACK	1
	struct rb_node *rb_right;
	struct rb_node *rb_left;
} __attribute__((aligned(sizeof(long))));
/* The alignment might seem pointless, but allegedly CRIS needs it */

struct rb_root
{
	struct rb_node *rb_node;
};


#define rb_parent(r)   ((struct rb_node *)((r)->rb_parent_color & ~3))
#define rb_color(r)   ((r)->rb_parent_color & 1)
#define rb_is_red(r)   (!rb_color(r))
#define rb_is_black(r) rb_color(r)
#define rb_set_red(r)  do { (r)->rb_parent_color &= ~1; } while (0)
#define rb_set_black(r)  do { (r)->rb_parent_color |= 1; } while (0)

static inline void rb_set_parent(struct rb_node *rb, struct rb_node *p)
{
	rb->rb_parent_color = (rb->rb_parent_color & 3) | (unsigned long)p;
}
static inline void rb_set_color(struct rb_node *rb, int color)
{
	rb->rb_parent_color = (rb->rb_parent_color & ~1) | color;
}

#define RB_ROOT	(struct rb_root) { NULL, }
#define	rb_entry(ptr, type, member) container_of(ptr, type, member)

#define RB_EMPTY_ROOT(root)	((root)->rb_node == NULL)
#define RB_EMPTY_NODE(node)	(rb_parent(node) == node)
#define RB_CLEAR_NODE(node)	(rb_set_parent(node, node))

extern void rb_insert_color(struct rb_node *, struct rb_root *);
extern void rb_erase(struct rb_node *, struct rb_root *);

/* Find logical next and previous nodes in a tree */
extern struct rb_node *rb_next(struct rb_node *);
extern struct rb_node *rb_prev(struct rb_node *);
extern struct rb_node *rb_first(struct rb_root *);
extern struct rb_node *rb_last(struct rb_root *);

/* Fast replacement of a single node without remove/rebalance/add/rebalance */
extern void rb_replace_node(struct rb_node *victim, struct rb_node *new,
			    struct rb_root *root);

static inline void rb_link_node(struct rb_node * node, struct rb_node * parent,
				struct rb_node ** rb_link)
{
	node->rb_parent_color = (unsigned long )parent;
	node->rb_left = node->rb_right = NULL;

	*rb_link = node;
}

static void __rb_rotate_left(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *right = node->rb_right;
	struct rb_node *parent = rb_parent(node);

	if ((node->rb_right = right->rb_left))
		rb_set_parent(right->rb_left, node);
	right->rb_left = node;

	rb_set_parent(right, parent);

	if (parent)
	{
		if (node == parent->rb_left)
			parent->rb_left = right;
		else
			parent->rb_right = right;
	}
	else
		root->rb_node = right;
	rb_set_parent(node, right);
}

static void __rb_rotate_right(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *left = node->rb_left;
	struct rb_node *parent = rb_parent(node);

	if ((node->rb_left = left->rb_right))
		rb_set_parent(left->rb_right, node);
	left->rb_right = node;

	rb_set_parent(left, parent);

	if (parent)
	{
		if (node == parent->rb_right)
			parent->rb_right = left;
		else
			parent->rb_left = left;
	}
	else
		root->rb_node = left;
	rb_set_parent(node, left);
}

void rb_insert_color(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *parent, *gparent;

	while ((parent = rb_parent(node)) && rb_is_red(parent))
	{
		gparent = rb_parent(parent);

		if (parent == gparent->rb_left)
		{
			{
				register struct rb_node *uncle = gparent->rb_right;
				if (uncle && rb_is_red(uncle))
				{
					rb_set_black(uncle);
					rb_set_black(parent);
					rb_set_red(gparent);
					node = gparent;
					continue;
				}
			}

			if (parent->rb_right == node)
			{
				register struct rb_node *tmp;
				__rb_rotate_left(parent, root);
				tmp = parent;
				parent = node;
				node = tmp;
			}

			rb_set_black(parent);
			rb_set_red(gparent);
			__rb_rotate_right(gparent, root);
		} else {
			{
				register struct rb_node *uncle = gparent->rb_left;
				if (uncle && rb_is_red(uncle))
				{
					rb_set_black(uncle);
					rb_set_black(parent);
					rb_set_red(gparent);
					node = gparent;
					continue;
				}
			}

			if (parent->rb_left == node)
			{
				register struct rb_node *tmp;
				__rb_rotate_right(parent, root);
				tmp = parent;
				parent = node;
				node = tmp;
			}

			rb_set_black(parent);
			rb_set_red(gparent);
			__rb_rotate_left(gparent, root);
		}
	}

	rb_set_black(root->rb_node);
}
static void __rb_erase_color(struct rb_node *node, struct rb_node *parent,
			     struct rb_root *root)
{
	struct rb_node *other;

	while ((!node || rb_is_black(node)) && node != root->rb_node)
	{
		if (parent->rb_left == node)
		{
			other = parent->rb_right;
			if (rb_is_red(other))
			{
				rb_set_black(other);
				rb_set_red(parent);
				__rb_rotate_left(parent, root);
				other = parent->rb_right;
			}
			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
			    (!other->rb_right || rb_is_black(other->rb_right)))
			{
				rb_set_red(other);
				node = parent;
				parent = rb_parent(node);
			}
			else
			{
				if (!other->rb_right || rb_is_black(other->rb_right))
				{
					struct rb_node *o_left;
					if ((o_left = other->rb_left))
						rb_set_black(o_left);
					rb_set_red(other);
					__rb_rotate_right(other, root);
					other = parent->rb_right;
				}
				rb_set_color(other, rb_color(parent));
				rb_set_black(parent);
				if (other->rb_right)
					rb_set_black(other->rb_right);
				__rb_rotate_left(parent, root);
				node = root->rb_node;
				break;
			}
		}
		else
		{
			other = parent->rb_left;
			if (rb_is_red(other))
			{
				rb_set_black(other);
				rb_set_red(parent);
				__rb_rotate_right(parent, root);
				other = parent->rb_left;
			}
			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
			    (!other->rb_right || rb_is_black(other->rb_right)))
			{
				rb_set_red(other);
				node = parent;
				parent = rb_parent(node);
			}
			else
			{
				if (!other->rb_left || rb_is_black(other->rb_left))
				{
					register struct rb_node *o_right;
					if ((o_right = other->rb_right))
						rb_set_black(o_right);
					rb_set_red(other);
					__rb_rotate_left(other, root);
					other = parent->rb_left;
				}
				rb_set_color(other, rb_color(parent));
				rb_set_black(parent);
				if (other->rb_left)
					rb_set_black(other->rb_left);
				__rb_rotate_right(parent, root);
				node = root->rb_node;
				break;
			}
		}
	}
	if (node)
		rb_set_black(node);
}

void rb_erase(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *child, *parent;
	int color;

	if (!node->rb_left)
		child = node->rb_right;
	else if (!node->rb_right)
		child = node->rb_left;
	else
	{
		struct rb_node *old = node, *left;

		node = node->rb_right;
		while ((left = node->rb_left) != NULL)
			node = left;
		child = node->rb_right;
		parent = rb_parent(node);
		color = rb_color(node);

		if (child)
			rb_set_parent(child, parent);
		if (parent == old) {
			parent->rb_right = child;
			parent = node;
		} else
			parent->rb_left = child;

		node->rb_parent_color = old->rb_parent_color;
		node->rb_right = old->rb_right;
		node->rb_left = old->rb_left;

		if (rb_parent(old))
		{
			if (rb_parent(old)->rb_left == old)
				rb_parent(old)->rb_left = node;
			else
				rb_parent(old)->rb_right = node;
		} else
			root->rb_node = node;

		rb_set_parent(old->rb_left, node);
		if (old->rb_right)
			rb_set_parent(old->rb_right, node);
		goto color;
	}

	parent = rb_parent(node);
	color = rb_color(node);

	if (child)
		rb_set_parent(child, parent);
	if (parent)
	{
		if (parent->rb_left == node)
			parent->rb_left = child;
		else
			parent->rb_right = child;
	}
	else
		root->rb_node = child;

	color:
	if (color == RB_BLACK)
		__rb_erase_color(child, parent, root);
}
/*
 * This function returns the first node (in sort order) of the tree.
 */
struct rb_node *rb_first(struct rb_root *root)
{
	struct rb_node	*n;

	n = root->rb_node;
	if (!n)
		return NULL;
	while (n->rb_left)
		n = n->rb_left;
	return n;
}
struct rb_node *rb_last(struct rb_root *root)
{
	struct rb_node	*n;

	n = root->rb_node;
	if (!n)
		return NULL;
	while (n->rb_right)
		n = n->rb_right;
	return n;
}
struct rb_node *rb_next(struct rb_node *node)
{
	struct rb_node *parent;

	if (rb_parent(node) == node)
		return NULL;

	/* If we have a right-hand child, go down and then left as far
	 as we can. */
	if (node->rb_right) {
		node = node->rb_right;
		while (node->rb_left)
			node=node->rb_left;
		return node;
	}

	/* No right-hand children.  Everything down and left is
	 smaller than us, so any 'next' node must be in the general
	 direction of our parent. Go up the tree; any time the
	 ancestor is a right-hand child of its parent, keep going
	 up. First time it's a left-hand child of its parent, said
	 parent is our 'next' node. */
	while ((parent = rb_parent(node)) && node == parent->rb_right)
		node = parent;

	return parent;
}
struct rb_node *rb_prev(struct rb_node *node)
{
	struct rb_node *parent;

	if (rb_parent(node) == node)
		return NULL;

	/* If we have a left-hand child, go down and then right as far
	 as we can. */
	if (node->rb_left) {
		node = node->rb_left;
		while (node->rb_right)
			node=node->rb_right;
		return node;
	}

	/* No left-hand children. Go up till we find an ancestor which
	 is a right-hand child of its parent */
	while ((parent = rb_parent(node)) && node == parent->rb_left)
		node = parent;

	return parent;
}
void rb_replace_node(struct rb_node *victim, struct rb_node *new,
		     struct rb_root *root)
{
	struct rb_node *parent = rb_parent(victim);

	/* Set the surrounding nodes to point to the replacement */
	if (parent) {
		if (victim == parent->rb_left)
			parent->rb_left = new;
		else
			parent->rb_right = new;
	} else {
		root->rb_node = new;
	}
	if (victim->rb_left)
		rb_set_parent(victim->rb_left, new);
	if (victim->rb_right)
		rb_set_parent(victim->rb_right, new);

	/* Copy the pointers/colour from the victim to the replacement */
	*new = *victim;
}


/*
 *  CFS code ...
 */
struct cfs_task {
	struct rb_node run_node;
	nstime_t t;
	/* ... */
};

struct cfs_rq {
	struct rb_root tasks_timeline;
	struct rb_node *rb_leftmost;
};

void cfs_rqinit(struct cfs_rq *rq) {
	rq->rb_leftmost = NULL;
	rq->tasks_timeline = RB_ROOT;
}
void cfs_queue(struct cfs_task *tsk, struct cfs_rq *rq, nstime_t t) {
	struct rb_node **link = &rq->tasks_timeline.rb_node;
	struct rb_node *parent = NULL;
	struct cfs_task *entry;
	int leftmost = 1;

	tsk->t = t;
	/*
	 * Find the right place in the rbtree:
	 */
	while (*link) {
		parent = *link;
		entry = rb_entry(parent, struct cfs_task, run_node);
		/*
		 *  * We dont care about collisions. Nodes with
		 *  * the same key stay together.
		 *  */
		if (t < entry->t) {
			link = &(*link)->rb_left;
		} else {
			link = &(*link)->rb_right;
			leftmost = 0;
		}
	}

	/*
	 * Maintain a cache of leftmost tree entries (it is frequently used)
	 */
	if (leftmost)
		rq->rb_leftmost = &tsk->run_node;
	rb_link_node(&tsk->run_node, parent, link);
	rb_insert_color(&tsk->run_node, &rq->tasks_timeline);
}
static void __dequeue_task_fair(struct cfs_rq *rq, struct cfs_task *p) {
	if (rq->rb_leftmost == &p->run_node)
		rq->rb_leftmost = NULL;
	rb_erase(&p->run_node, &rq->tasks_timeline);
}
static inline struct rb_node * first_fair(struct cfs_rq *rq) {
	if (rq->rb_leftmost)
		return rq->rb_leftmost;
	return rb_first(&rq->tasks_timeline);

}
struct cfs_task *cfs_dequeue(struct cfs_rq *rq) {
	struct rb_node *first = first_fair(rq);
	struct cfs_task *next;

	next = rb_entry(first, struct cfs_task, run_node);
	if (likely(next))
		__dequeue_task_fair(rq, next);

	return next;
}

/******************************************************************************/


/*
 * Timed ring code
 */
#define MAX_RQ		(1 << 8)
#define RQ_MASK		(MAX_RQ - 1)
#define MAP_LONGS	(MAX_RQ / BITS_X_LONG)
#define NS_SLOT		(5 * 1000000UL)

struct tr_task {
	struct list_head lnk;
	nstime_t t;
	/* ... */
};

struct tr_rq {
	unsigned int ibase;
	nstime_t tbase;
	struct list_head tsks[MAX_RQ];
	unsigned long map[MAP_LONGS];
};


void tr_rqinit(struct tr_rq *rq) {
	int i;

	rq->ibase = 0;
	rq->tbase = 0;
	for (i = 0; i < MAX_RQ; i++)
		INIT_LIST_HEAD(&rq->tsks[i]);
	memset(rq->map, 0, sizeof(rq->map));
}

static int rel_ffs(struct tr_rq *rq) {
	unsigned int i, b, n;
	unsigned long mask;

	i = rq->ibase / BITS_X_LONG;
	b = rq->ibase % BITS_X_LONG;
	mask = ~((1UL << b) - 1);
	for (n = MAP_LONGS; n; n--) {
		unsigned long v = rq->map[i] & mask;

		if (likely(v))
			return i * BITS_X_LONG + __ffs(v);
		i = (i + 1) % MAP_LONGS;
		mask = ~0UL;
	}
	return MAX_RQ;
}

struct tr_task *tr_dequeue(struct tr_rq *rq) {
	unsigned int idx, d;
	struct tr_task *tsk;

	idx = rel_ffs(rq);
	if (unlikely(idx == MAX_RQ)) {
		rq->tbase = 0;
		return NULL;
	}
	tsk = list_entry(rq->tsks[idx].next, struct tr_task, lnk);
	list_del(&tsk->lnk);
	if (list_empty(&rq->tsks[idx]))
		__clear_bit(idx, rq->map);
	d = (idx - rq->ibase) & RQ_MASK;
	rq->ibase = idx;
	rq->tbase += NS_SLOT * d;

	return tsk;
}

void tr_queue(struct tr_task *tsk, struct tr_rq *rq, nstime_t t) {
	unsigned int idx;

	if (unlikely(rq->tbase == 0))
		rq->tbase = tsk->t /* sched_clock() */;
	tsk->t = t;
	idx = (unsigned int) ((t - rq->tbase) / NS_SLOT);
	if (unlikely(idx >= MAX_RQ))
		idx = MAX_RQ - 1;
	idx = (idx + rq->ibase) & RQ_MASK;
	list_add_tail(&tsk->lnk, &rq->tsks[idx]);
	__set_bit(idx, rq->map);
}


/******************************************************************************/

void gavl_cfs_test(int ntasks, int times, int loops) {
	int i;
	unsigned long long ts, te;
	struct gavl_cfs_task *tasks, *tsk;
	struct gavl_cfs_rq rq;

	tasks = (struct gavl_cfs_task *) malloc(ntasks * sizeof(struct gavl_cfs_task));
	gavl_cfs_init_root_field(&rq);
	for (i = 0; i < ntasks; i++) {
		tasks[i].t = 0;
		tasks[i].t = (nstime_t) (rand() % times) * NS_SLOT;
		gavl_cfs_insert(&rq, &tasks[i]);
	}
	rdtscll(ts);
	for (i = 0; i < loops; i++) {
		tsk = gavl_cfs_cut_first(&rq);
		tsk->t = tsk->t + (nstime_t) (times / 7) * NS_SLOT;
		gavl_cfs_insert(&rq, tsk);
	}
	rdtscll(te);
	free(tasks);

	fprintf(stdout, "gavl_cfs = %.2lf cycles/loop\n", (double) (te - ts) / loops);
}

void cfs_test(int ntasks, int times, int loops) {
	int i;
	unsigned long long ts, te;
	struct cfs_task *tasks, *tsk;
	struct cfs_rq rq;

	tasks = (struct cfs_task *) malloc(ntasks * sizeof(struct cfs_task));
	cfs_rqinit(&rq);
	for (i = 0; i < ntasks; i++) {
		tasks[i].t = 0;
		cfs_queue(&tasks[i], &rq, (nstime_t) (rand() % times) * NS_SLOT);
	}
	rdtscll(ts);
	for (i = 0; i < loops; i++) {
		tsk = cfs_dequeue(&rq);
		cfs_queue(tsk, &rq, tsk->t + (nstime_t) (times / 7) * NS_SLOT);
	}
	rdtscll(te);
	free(tasks);

	fprintf(stdout, "CFS = %.2lf cycles/loop\n", (double) (te - ts) / loops);
}

void tr_test(int ntasks, int times, int loops) {
	int i;
	unsigned long long ts, te;
	struct tr_task *tasks, *tsk;
	struct tr_rq rq;

	tasks = (struct tr_task *) malloc(ntasks * sizeof(struct tr_task));
	tr_rqinit(&rq);
	for (i = 0; i < ntasks; i++) {
		tasks[i].t = 0;
		tr_queue(&tasks[i], &rq, (nstime_t) (rand() % times) * NS_SLOT);
	}
	rdtscll(ts);
	for (i = 0; i < loops; i++) {
		tsk = tr_dequeue(&rq);
		tr_queue(tsk, &rq, tsk->t + (nstime_t) (times / 7) * NS_SLOT);
	}
	rdtscll(te);
	free(tasks);

	fprintf(stdout, "TR  = %.2lf cycles/loop\n", (double) (te - ts) / loops);
}

/******************************************************************************/

int main(int ac, char **av) {
	int i, ntasks = 128, loops = 200000;

	for (i = 1; i < ac; i++) {
		if (!strcmp(av[i], "-n")) {
			if (++i < ac)
				ntasks = atoi(av[i]);
		} else if (!strcmp(av[i], "-l")) {
			if (++i < ac)
				loops = atoi(av[i]);
		}
	}

	gavl_cfs_test(ntasks, MAX_RQ, loops);
	cfs_test(ntasks, MAX_RQ, loops);
	tr_test(ntasks, MAX_RQ, loops);
	cfs_test(ntasks, MAX_RQ, loops);
	gavl_cfs_test(ntasks, MAX_RQ, loops);

	return 0;
}