[vg.git] / src / vg / vg_store.h

#ifndef VG_STORE_H
#define VG_STORE_H

#include "vg_stdint.h"
#include "vg_io.h"

/* 
 * Anderson tree implementation with extensions:
 *   parents are kept track of
 *   duplicates are allowed
 *   data is never allocated or destroyed here
 *
 * TODO: seperate offset,stride,base into 'generic array', seperate pool
 */

typedef struct aatree aatree;
typedef struct aatree_node aatree_node;
typedef struct aatree_pool_node aatree_pool_node;

typedef u32 aatree_ptr;
#define AATREE_PTR_NIL 0xffffffff

struct aatree
{
   u32 offset, stride; /* distance between elements */
   void *base;

   int (*p_cmp)( void *a, void *b );
};

#pragma pack(push,1)
struct aatree_node
{
   aatree_ptr left, right, parent;
   u32 level, count;
};

struct aatree_pool_node 
{
   aatree_ptr next_free;
};
#pragma pack(pop)

/* api 
 * ===========================================================================*/

/* return a pointer to the start of the data referenced by t */
static void *aatree_get_data( aatree *tree, aatree_ptr t );

/* link node x into the tree with root t */
static aatree_ptr aatree_insert( aatree *tree, aatree_ptr t, aatree_ptr x );

/* delete node x from tree, does not free memory */
static aatree_ptr aatree_del( aatree *tree, aatree_ptr x );

/* get pointer to element in tree with index k */
static aatree_ptr aatree_kth( aatree *tree, aatree_ptr t, u32 k );

/* get pointer to the element above x */
static aatree_ptr aatree_next( aatree *tree, aatree_ptr x );

/* get pointer by value, returns NIL if not found.
 *
 * if duplicates values are present then the result is undefined, but it will be
 * AN node with that value, just maybe not the first lexicographically
 */
static aatree_ptr aatree_find( aatree *tree, aatree_ptr t, void *value );

/* implementation 
 * ===========================================================================*/

static void *aatree_get_data( aatree *tree, aatree_ptr t )
{
   return (u8 *)tree->base + tree->stride*t;
}

static u8 *aatree_node_base( aatree *tree, aatree_ptr t )
{
   return (u8 *)tree->base + tree->stride*t + tree->offset;
}

static aatree_pool_node *aatree_get_pool_node( aatree *tree, aatree_ptr t )
{
   return (aatree_pool_node *)aatree_node_base( tree, t );
}

static aatree_node *aatree_get_node( aatree *tree, aatree_ptr t )
{
   return (aatree_node *)aatree_node_base( tree, t );
}

static void aatree_recount( aatree *tree, aatree_ptr n )
{
   aatree_node *pnode = aatree_get_node( tree, n );
   pnode->count = 1;

   if( pnode->left != AATREE_PTR_NIL )
      pnode->count += aatree_get_node( tree, pnode->left )->count;

   if( pnode->right != AATREE_PTR_NIL )
      pnode->count += aatree_get_node( tree, pnode->right )->count;
}

/*         .            .
 *         |            |
 *    L <- T            L -> T
 *   / \    \    ->    /    / \
 *  A   B    R        A    B   R
 */
static aatree_ptr aatree_skew( aatree *tree, u32 t )
{
   if( t == AATREE_PTR_NIL ) return t;

   aatree_node *ptnode = aatree_get_node( tree, t );
   if( ptnode->left == AATREE_PTR_NIL ) return t;

   aatree_node *plnode = aatree_get_node( tree, ptnode->left );
   if( plnode->level == ptnode->level )
   {
      aatree_ptr l = ptnode->left;
      ptnode->left = plnode->right;
      plnode->right = t;

      aatree_recount( tree, t );
      aatree_recount( tree, l );

      plnode->parent = ptnode->parent;
      ptnode->parent = l;
      if( ptnode->left != AATREE_PTR_NIL )
         aatree_get_node( tree, ptnode->left )->parent = t;

      return l;
   }

   return t;
}

/*    .                    .
 *    |                    |
 *    T -> R -> X   ->     R
 *   /    /               / \
 *  A    B               T   X
 *                      / \
 *                     A   B
 */
static aatree_ptr aatree_split( aatree *tree, aatree_ptr t )
{
   if( t == AATREE_PTR_NIL ) return t;

   aatree_node *ptnode = aatree_get_node( tree, t );
   if( ptnode->right == AATREE_PTR_NIL ) return t;

   aatree_node *prnode = aatree_get_node( tree, ptnode->right );
   if( prnode->right == AATREE_PTR_NIL ) return t;

   aatree_node *prrnode = aatree_get_node( tree, prnode->right );
   if( ptnode->level == prrnode->level )
   {
      aatree_ptr r = ptnode->right;
      ptnode->right = prnode->left;
      prnode->left = t;
      prnode->level ++;

      aatree_recount( tree, t );
      aatree_recount( tree, r );

      prnode->parent = ptnode->parent;
      ptnode->parent = r;
      if( ptnode->right != AATREE_PTR_NIL )
         aatree_get_node( tree, ptnode->right )->parent = t;

      return r;
   }

   return t;
}

static aatree_ptr aatree_insert( aatree *tree, aatree_ptr t, aatree_ptr x )
{
   aatree_node *pxnode = aatree_get_node( tree, x );

   if( t == AATREE_PTR_NIL )
   {
      pxnode->left = AATREE_PTR_NIL;
      pxnode->right = AATREE_PTR_NIL;
      pxnode->parent = AATREE_PTR_NIL;
      pxnode->level = 0;
      pxnode->count = 1;
      return x;
   }

   aatree_node *ptnode = aatree_get_node( tree, t );
   int cmp_result = tree->p_cmp( aatree_get_data( tree, t ), 
                                 aatree_get_data( tree, x ) );
   
   ptnode->count ++;

   if( cmp_result <= 0 )
   {
      ptnode->left = aatree_insert( tree, ptnode->left, x );
      aatree_node *plnode = aatree_get_node( tree, ptnode->left );
      plnode->parent = t;
   }
   else
   {
      ptnode->right = aatree_insert( tree, ptnode->right, x );
      aatree_node *prnode = aatree_get_node( tree, ptnode->right );
      prnode->parent = t;
   }

   t = aatree_skew( tree, t );
   t = aatree_split( tree, t );
   return t;
}

static void aatree_link_down( aatree *tree, aatree_ptr p, aatree_ptr *pl,
                              aatree_ptr l )
{
   *pl = l;

   if( *pl != AATREE_PTR_NIL )
      aatree_get_node( tree, *pl )->parent = p;
}

static aatree_ptr aatree_copy_links( aatree *tree, aatree_ptr root,
                                     aatree_ptr src, aatree_ptr dst )
{
   aatree_node *pdst = aatree_get_node( tree, dst ),
               *psrc = aatree_get_node( tree, src );

   pdst->count = psrc->count;
   pdst->level = psrc->level;
   pdst->parent = psrc->parent;

   aatree_link_down( tree, dst, &pdst->left, psrc->left );
   aatree_link_down( tree, dst, &pdst->right, psrc->right );
   
   if( pdst->parent != AATREE_PTR_NIL )
   {
      aatree_node *parent = aatree_get_node( tree, pdst->parent );

      if( parent->left == src )
         parent->left = dst;
      else if( parent->right == src )
         parent->right = dst;
   }
   else
      return dst;

   return root;
}

static aatree_ptr aatree_del( aatree *tree, aatree_ptr x )
{
   aatree_ptr it = x,
              up[32];

   int count = 1, dir = 0;
   
   /* TODO: maybe be a better way to do this, without counting back up */
   for( aatree_node *s = aatree_get_node( tree, x ); 
         s->parent != AATREE_PTR_NIL;
         count ++ )
      s = aatree_get_node( tree, s->parent );

   int top=0;
   while(1)
   {
      int index = count - (++top);

      up[ index ] = it;
      aatree_node *itnode = aatree_get_node( tree, it );
      if( itnode->parent == AATREE_PTR_NIL )
         break;
      else
         it = itnode->parent;
   }
   
   aatree_ptr _ptrswap_src = AATREE_PTR_NIL,
              _ptrswap_dst = AATREE_PTR_NIL;

   aatree_node *pxnode = aatree_get_node( tree, x );
   aatree_ptr root = up[ count-1 ];
   if( pxnode->left == AATREE_PTR_NIL || pxnode->right == AATREE_PTR_NIL )
   {
      if( --top != 0 )
      {
         aatree_node *pnode = aatree_get_node( tree, up[top-1] ),
                     *parent = aatree_get_node( tree, pxnode->parent );
         
         aatree_ptr next = pxnode->left == AATREE_PTR_NIL?
                           pxnode->right:
                           pxnode->left;

         if( parent->left == x ) pnode->left = next;
         else pnode->right = next;
         
         if( next != AATREE_PTR_NIL )
         {
            aatree_node *pnext = aatree_get_node( tree, next );
            pnext->parent = up[top-1];
         }
      }
      else
      {
         if( pxnode->right != AATREE_PTR_NIL ) root = pxnode->right;
         else if( pxnode->left != AATREE_PTR_NIL ) root = pxnode->left;
         else return AATREE_PTR_NIL;

         aatree_node *newroot = aatree_get_node( tree, root );
         newroot->parent = AATREE_PTR_NIL;
      }
   }
   else
   {
      aatree_ptr heir = pxnode->right,
                 prev = x;

      aatree_node *pheir = aatree_get_node( tree, heir );
      
      while( pheir->left != AATREE_PTR_NIL )
      {
         up[top++] = prev = heir;
         heir = pheir->left;
         pheir = aatree_get_node( tree, heir );
      }
      
      _ptrswap_dst = heir;
      _ptrswap_src = x;

      aatree_node *pprev = aatree_get_node( tree, prev );
      
      if( prev == x )
         aatree_link_down( tree, prev, &pprev->right, pheir->right );
      else
         aatree_link_down( tree, prev, &pprev->left, pheir->right );
   }

   /* Tail */
   while( --top >= 0 )
   {
      if( top != 0 )
      {
         aatree_node *above = aatree_get_node( tree, up[top-1] );
         dir = above->right == up[top];
      }

      aatree_recount( tree, up[top] );
      aatree_node *pntop = aatree_get_node( tree, up[top] );

      if( !(pntop->left == AATREE_PTR_NIL || pntop->right == AATREE_PTR_NIL) )
      {
         aatree_node *pnl = aatree_get_node( tree, pntop->left ),
                     *pnr = aatree_get_node( tree, pntop->right );
         
         if( pnl->level < pntop->level-1 || pnr->level < pntop->level-1 )
         {
            if( pnr->level > --pntop->level )
               pnr->level = pntop->level;

            up[top] = aatree_skew( tree, up[top] );

            aatree_node *ut = aatree_get_node( tree, up[top] );
            ut->right = aatree_skew( tree, ut->right );

            aatree_node *utr = aatree_get_node( tree, ut->right );
            utr->right = aatree_skew( tree, utr->right );

            up[top] = aatree_split( tree, up[top] );
            ut = aatree_get_node( tree, up[top] );

            ut->right = aatree_split( tree, ut->right );
         }
      }

      if( top != 0 )
      {
         aatree_node *ut1 = aatree_get_node( tree, up[top-1] );
         
         if( dir == 1 )
            aatree_link_down( tree, up[top-1], &ut1->right, up[top] );
         else
            aatree_link_down( tree, up[top-1], &ut1->left, up[top] );
      }
      else
      {
         root = up[top];
         aatree_get_node( tree, root )->parent = AATREE_PTR_NIL;
      }
   }
   
   /* This is our extension to the original non-recursive delete, so no data
    * has to be moved */
   if( _ptrswap_dst != AATREE_PTR_NIL )
      root = aatree_copy_links( tree, root, _ptrswap_src, _ptrswap_dst );

   return root;
}

static aatree_ptr aatree_kth( aatree *tree, aatree_ptr t, u32 k )
{
   u32 i = 0;

   while( t != AATREE_PTR_NIL )
   {
      aatree_node *ptnode = aatree_get_node( tree, t );

      u32 j = i;
      if( ptnode->left != AATREE_PTR_NIL )
         j += aatree_get_node( tree, ptnode->left )->count;

      if( j < k )
      {
         i = j+1;
         t = ptnode->right;
      }
      else
      {
         if( j > k )
         {
            t = ptnode->left;
         }
         else
         {
            return t;
         }
      }
   }

   return AATREE_PTR_NIL;
}

static aatree_ptr aatree_next( aatree *tree, aatree_ptr x )
{
   /* if can go left, go left then all the way right,
    * else go up, if it was right link accept
    */

   aatree_node *pnode = aatree_get_node( tree, x );
   if( pnode->right != AATREE_PTR_NIL )
   {
      aatree_ptr next = pnode->right;
      
      while(1)
      {
         aatree_node *pnext = aatree_get_node( tree, next );
         
         if( pnext->left != AATREE_PTR_NIL )
            next = pnext->left;
         else
            return next;
      }
   }
   else
   {
      aatree_ptr next = x;
      
      while(1)
      {
         aatree_node *pnode = aatree_get_node( tree, next );

         if( pnode->parent == AATREE_PTR_NIL )
            return AATREE_PTR_NIL;

         aatree_node *pabove = aatree_get_node( tree, pnode->parent );
         if( pabove->left == next )
            return pnode->parent;
         else
            next = pnode->parent;
      }
   }
}

static aatree_ptr aatree_find( aatree *tree, aatree_ptr t, void *value )
{
   while( t != AATREE_PTR_NIL )
   {
      int cmp_result = tree->p_cmp( aatree_get_data( tree, t ), value );

      if( cmp_result == 0 )
         return t;
      else 
      {
         aatree_node *ptnode = aatree_get_node( tree, t );

         if( cmp_result < 0 )
            t = ptnode->left;
         else
            t = ptnode->right;
      }
   }
   return t;
}

/*
 * Debugging stuff, everything below is scaffholding and will be removed
 * =============================================================================
 */

static int aatree_verify_split( aatree *tree, aatree_ptr t )
{
   if( t == AATREE_PTR_NIL ) return 1;

   aatree_node *ptnode = aatree_get_node( tree, t );
   if( ptnode->right == AATREE_PTR_NIL ) return 1;

   aatree_node *prnode = aatree_get_node( tree, ptnode->right );
   if( prnode->right == AATREE_PTR_NIL ) return 1;

   aatree_node *prrnode = aatree_get_node( tree, prnode->right );
   if( ptnode->level == prrnode->level )
      return 0;

   return 1;
}

static int aatree_verify_skew( aatree *tree, aatree_ptr t )
{
   if( t == AATREE_PTR_NIL ) return 1;

   aatree_node *ptnode = aatree_get_node( tree, t );
   if( ptnode->left == AATREE_PTR_NIL ) return 1;

   aatree_node *plnode = aatree_get_node( tree, ptnode->left );
   if( plnode->level == ptnode->level )
      return 0;

   return 1;
}

static int aatree_verify( aatree *tree, aatree_ptr t )
{
   aatree_node *ptnode = aatree_get_node( tree, t );
   if( ptnode->parent != AATREE_PTR_NIL )
   {
      aatree_node *parent = aatree_get_node( tree, ptnode->parent );
      if( !(parent->left == t || parent->right == t) )
         return 0;
   }

   if( ptnode->left != AATREE_PTR_NIL )
      if( aatree_get_node( tree, ptnode->left )->parent != t )
         return 0;
   if( ptnode->right != AATREE_PTR_NIL )
      if( aatree_get_node( tree, ptnode->right )->parent != t )
         return 0;

   return aatree_verify_skew( tree, t ) &&
          aatree_verify_split( tree, t );
}


static void aatree_show_r( aatree *tree, aatree_ptr t, int lvl, 
                           void(*p_show)(void *data) )
{
   if( t != AATREE_PTR_NIL )
   {
      aatree_node *ptnode = aatree_get_node( tree, t );
      aatree_show_r( tree, ptnode->left, lvl+1, p_show );

      void *data = aatree_get_data( tree, t );

      for( int i=0; i<lvl; i++ )
      {
         vg_log( "  " );
      }
      p_show( data );
      vg_log( " (%d) \n", t );

      aatree_show_r( tree, ptnode->right, lvl+1, p_show );
   }
}

static void aatree_show( aatree *tree, aatree_ptr t, void(*p_show)(void *data))
{
   if( t != AATREE_PTR_NIL )
   {
      aatree_node *ptnode = aatree_get_node( tree, t );
      aatree_show( tree, ptnode->left, p_show );
      void *data = aatree_get_data( tree, t );

      for( int i=0; i<ptnode->level; i++ )
      {
         vg_log( "  " );
      }
      p_show( data );
      vg_log( " (%d) \n", t );

      aatree_show( tree, ptnode->right, p_show );
   }
}

static void aatree_show_counts( aatree *tree, aatree_ptr t, int lvl, int *ln,
                                 int *err,
                                void(*p_show)(void *data), int show )
{
   if( lvl > 20 )
      return;
   if( t == AATREE_PTR_NIL ) return;

   aatree_node *ptnode = aatree_get_node( tree, t );
   void *data = aatree_get_data( tree, t );

   aatree_show_counts( tree, ptnode->left, lvl+1, ln, err, p_show, show );

   if( show ) vg_log( "%03d| ", *ln );
   *ln = *ln +1;

   if( show ) 
      for( int i=0; i<lvl; i++ )
         printf( "   " );

   if( show )
   {
      p_show( data );

      if( ptnode->left != AATREE_PTR_NIL && ptnode->right != AATREE_PTR_NIL )
         printf( "|" );
      if( ptnode->left != AATREE_PTR_NIL && ptnode->right == AATREE_PTR_NIL )
         printf( "/" );
      if( ptnode->left == AATREE_PTR_NIL && ptnode->right != AATREE_PTR_NIL )
         printf( "\\" );

      printf( " (%d, %d, parent: %d. V: %d, level: %d) \n", t, 
            ptnode->count, ptnode->parent,
            aatree_verify( tree, t ), ptnode->level);
   }

   if( !aatree_verify( tree, t ) )
   {
      if( show )
         vg_log( "error\n" );
      *err = 1;
   }

   aatree_show_counts( tree, ptnode->right, lvl+1, ln, err, p_show, show );
}

/*
 * Pool allocator utility which can be placed in a union with regular aa nodes.
 */

static aatree_ptr aatree_init_pool( aatree *info, u32 item_count )
{
   for( aatree_ptr i=0; i<item_count; i++ )
   {
      aatree_pool_node *pn = aatree_get_pool_node( info, i );

      if( i==item_count-1 )
         pn->next_free = AATREE_PTR_NIL;
      else
         pn->next_free = i+1;
   }

   return 0;
}

static aatree_ptr aatree_pool_alloc( aatree *info, aatree_ptr *head )
{
   if( *head == AATREE_PTR_NIL )
   {
      vg_error( "No nodes free in pool allocator!\n" );
      return AATREE_PTR_NIL;
   }
   else
   {
      aatree_ptr gap = *head;
      *head = aatree_get_pool_node( info, *head )->next_free;
      return gap;
   }
}

static void aatree_pool_free( aatree *info, aatree_ptr node, aatree_ptr *head )
{
   aatree_pool_node *pn = aatree_get_pool_node( info, node );
   pn->next_free = *head;
   *head = node;
}

#endif /* VG_STORE_H */