--- /dev/null
+#ifndef VG_STORE_H
+#define VG_STORE_H
+
+#include "vg_stdint.h"
+#include "vg_io.h"
+#include "vg_log.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_info( " " );
+ }
+ p_show( data );
+ vg_info( " (%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_info( " " );
+ }
+ p_show( data );
+ vg_info( " (%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_info( "%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_info( "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 */