static int _dfs(struct _internal_node * node,
     int (*leafcb)(LEAFTYPE),
     uint32_t (*inodecb)(struct _internal_node *),
     uint32_t direction,
     int32_t pre_post) {
    uint32_t cur = direction;

    if ((pre_post == -1) && (inodecb != NULL) && (inodecb(node) == 0)) return 0;
    if (IS_LEAF(node, cur)) {
        if (leafcb(node->child[cur].leaf) == 0)
            return 0;
    } else {
        if (_dfs(node->child[cur].node, leafcb, inodecb, direction, pre_post) == 0)
            return 0;
    }

    if ((pre_post ==  0) && (inodecb != NULL) && (inodecb(node) == 0)) return 0;
    cur = 1 - cur; /* now the other child */
    if (IS_LEAF(node, cur)) {
        if (leafcb(node->child[cur].leaf) == 0)
            return 0;
    } else {
        if (_dfs(node->child[cur].node, leafcb, inodecb, direction, pre_post) == 0)
            return 0;
    }
    if ((pre_post ==  1) && (inodecb != NULL) && (inodecb(node) == 0)) return 0;

    return 1;
}

static void __traverse(node_t *n, int d, int _nrb)
{
    int i;
    if ( n == NULL ) 
    {
        if ( nrb == -1 ) nrb = _nrb;
        if ( nrb != _nrb )
            printf("Imbalance at depth %d (%d,%d)\n", d, nrb, _nrb);
        return;
    }
    if ( IS_LEAF(n) && (n->k != 0) )
    {
        assert(n->l == NULL);
        assert(n->r == NULL);
        assert(IS_BLACK(n->v));
    }
    if ( !IS_LEAF(n) && IS_RED(n->v) )
    {
        assert(IS_BLACK(n->l->v));
        assert(IS_BLACK(n->r->v));
    }
    if ( IS_BLACK(n->v) ) _nrb++;
    __traverse(n->l, d+1, _nrb);
    if ( valll > n->k ) bug=1;
#if 0
    for ( i = 0; i < d; i++ ) printf("  ");
    printf("%c%p K: %5d  V: %p  P: %p  L: %p  R: %p  depth: %d\n",
           IS_BLACK(n->v) ? 'B' : 'R', n, n->k, n->v, n->p, n->l, n->r, d);
#endif
    valll = n->k;
    __traverse(n->r, d+1, _nrb);
}

static bstnode *bst_findpath(bstnode *n, long key)
{
    if (key == n->key)
        return n;
    else if (key < n->key)
        return (IS_LEAF(n->left)) ? n : bst_findpath(n->left, key);
    else
        return (IS_LEAF(n->right)) ? n : bst_findpath(n->right, key);
}

static void bst_remove_at(bst *t, bstnode *n)
{
    bstnode *p;

    /* n has two children */
    if (!IS_LEAF(n->left) && !IS_LEAF(n->right))
    {
        static int del_from_left = 1;
        long tmpkey;
        void *tmpdata;
        if (del_from_left)
        {
            p = n->left;
            while (!IS_LEAF(p->right))
                p = p->right;
        }
        else
        {
            p = n->right;
            while (!IS_LEAF(p->left))
                p = p->left;
        }

        /* swap key and data and remove swapped node (which has 0 or 1 child) */
        tmpkey = p->key, p->key = n->key, n->key = tmpkey;
        tmpdata = p->data, p->data = n->data, n->data = tmpdata;
        bst_remove_at(t, p);
        return;
    }

    /* n has no/one child */
    p = IS_LEAF(n->left) ? n->right : n->left;

    /* replace n with p */
    p->parent = n->parent;
    if (n->parent)
    {
        if (IS_LEFT_CHILD(n))
            n->parent->left = p;
        else
            n->parent->right = p;
    }
    else
        t->root = p;

    if (IS_BLACK(n))
    {
        if (p->color == RED)
            p->color = BLACK;
        else
            bst_remove_repair(t, p);
    }
    free(n);
    return;
}

CSG_Node *arrange_link(CSG *csg, CSG_Node *state, CSG_Node *waiting, int go_sl)
{
	int lprefix      = LEN_L(state);
	int is_end       = TRUE;
	CSG_Node *source       = state;
	CSG_Node *parent_state = state;
	const char *wprefix    = LABEL(waiting);
	CSG_Node *next;
	CSG_Node *split;
	CSG_Node *node_act = state;
	
	if (go_sl && waiting != NINDEF)
		is_end  = class_link(csg,  &state, &parent_state, &lprefix);
	while (!is_end) {
		if (IS_LEAF(state)) {
			state = compres_leaf(csg,  source, parent_state, state
			, waiting, lprefix);
			source = state;
			node_act = state;
  		} else {
			split = divide(csg,  state, lprefix, NINDEF);
   			CLINK(waiting) = split;
   			waiting = split;
			if (state == node_act)
				node_act = waiting;
		}
		is_end  = class_link(csg,  &state, &parent_state, &lprefix);
	}
	/* search for another leaf in the same t-path */
	next = NEXT(csg, state, *wprefix);
	while (CLINK(waiting) == NINDEF) {
		if (state != source && IS_LEAF(next)) {
			if (!GET_SOLID(csg, state, next)) {
				SWAP_EDGE(csg, state, next, waiting);
				is_end  = class_link(csg,  &state
    				, &parent_state, &lprefix);
				next = NEXT(csg, state, *wprefix);
			} else {
				SWAP_EDGE(csg, source, waiting, next);
				waiting = next;
				csg->last_state--;
				DEC_NLEAFS;
			}
		} else  {
			CLINK(waiting) = (next == NINDEF) || (next == waiting) 
			? INITIAL : next;
		}
	}
	return node_act;
}

LEAFTYPE
radix_get(struct ROOTSTRUCT * tree, LEAFTYPE leaf, EXTRA_ARG aux) {
    LEAFTYPE result;
    struct _internal_node * node;
    uint32_t dir;

    if (tree->leafcount == 0) return NO_LEAF;
    if (tree->leafcount == 1) {
        result = tree->root.leaf;
        if (COMPARE(result, leaf) == -1) return result;
        else return NO_LEAF;
    } /* root points to a node */

    node = tree->root.node;
    while (1) {
        dir = DECIDE(leaf, node->critbit, aux);
        if (IS_LEAF(node, dir)) {
            result = node->child[dir].leaf;
            break;
        } else {
            node = node->child[dir].node;
        }
    }

    if (COMPARE(result, leaf) == -1) return result;
    else return NO_LEAF;
}

static void remove_child4(art_node4 *n, art_node **ref, art_node **l) {
    int pos = l - n->children;
    memmove(n->keys+pos, n->keys+pos+1, n->n.num_children - 1 - pos);
    memmove(n->children+pos, n->children+pos+1, (n->n.num_children - 1 - pos)*sizeof(void*));
    n->n.num_children--;

    // Remove nodes with only a single child
    if (n->n.num_children == 1) {
        art_node *child = n->children[0];
        if (!IS_LEAF(child)) {
            // Concatenate the prefixes
            int prefix = n->n.partial_len;
            if (prefix < MAX_PREFIX_LEN) {
                n->n.partial[prefix] = n->keys[0];
                prefix++;
            }
            if (prefix < MAX_PREFIX_LEN) {
                int sub_prefix = min(child->partial_len, MAX_PREFIX_LEN - prefix);
                memcpy(n->n.partial+prefix, child->partial, sub_prefix);
                prefix += sub_prefix;
            }

            // Store the prefix in the child
            memcpy(child->partial, n->n.partial, min(prefix, MAX_PREFIX_LEN));
            child->partial_len += n->n.partial_len + 1;
        }
        *ref = child;
        free(n);
    }
}

static nodeid get_leaf_and_parent(fs_ptree *pt, fs_rid pk, nodeid *parent)
{
    nodeid pos = FS_PTREE_ROOT_NODE;
    for (int i=0; i < 64/FS_PTREE_BRANCH_BITS; i++) {
        int kbranch = PK_BRANCH(pk, i);
        nodeid newpos = node_ref(pt, pos)->branch[kbranch];
        if (newpos == FS_PTREE_NULL_NODE) {
            return 0;
        } else if (IS_LEAF(newpos)) {
            if (pk == LEAF_REF(pt, newpos)->pk) {
                /* PKs are the same, we can use the block */
                if (parent) *parent = pos;

                return newpos;
            }

            return 0;
        }
        pos = newpos;
    }

    fs_error(LOG_ERR, "fell through get_leaf(%016llx)", pk);
    char tmp[256];
    tmp[0] = '\0';
    for (int i=0; i < 64/FS_PTREE_BRANCH_BITS; i++) {
        int kbranch = PK_BRANCH(pk, i);
        char tmp2[16];
        sprintf(tmp2, "%d.", kbranch);
        strcat(tmp, tmp2);
    }
    fs_error(LOG_ERR, "path was %s", tmp);

    return 0;
}

void GPMKDTree::AddPerfectMatching(PointId* rev_mapping)
{
	Node* i;
	int k;
	PointId p, q = -1;
	i = &nodes[0];
	do
	{
		if (IS_LEAF(i))
		{
			for (k=0; k<-i->d; k++)
			{
				p = i->points[k];
				if (q < 0) q = p;
				else { GPM->AddInitialEdge(rev_mapping[p], rev_mapping[q]); q = -1; }
			}
		}
		else
		{
			i = i->first_child;
			continue;
		}

		while ( i->parent )
		{
			if (i->parent->first_child == i) { i ++; break; }
			i = i->parent;
		}
	} while (i->parent);
}

/**
 * Searches for a value in the ART tree
 * @arg t The tree
 * @arg key The key
 * @arg key_len The length of the key
 * @return NULL if the item was not found, otherwise
 * the value pointer is returned.
 */
void* art_search(const art_tree *t, const unsigned char *key, int key_len) {
    art_node **child;
    art_node *n = t->root;
    int prefix_len, depth = 0;
    while (n) {
        // Might be a leaf
        if (IS_LEAF(n)) {
            n = (art_node*)LEAF_RAW(n);
            // Check if the expanded path matches
            if (!leaf_matches((art_leaf*)n, key, key_len, depth)) {
                return ((art_leaf*)n)->value;
            }
            return NULL;
        }

        // Bail if the prefix does not match
        if (n->partial_len) {
            prefix_len = check_prefix(n, key, key_len, depth);
            if (prefix_len != min(MAX_PREFIX_LEN, n->partial_len))
                return NULL;
            depth = depth + n->partial_len;
        }

        // Recursively search
        child = find_child(n, key[depth]);
        n = (child) ? *child : NULL;
        depth++;
    }
    return NULL;
}

dtree_node_t *
best_leaf_node(dtree_node_t *node)
{
    dtree_node_t *cmp_y, *cmp_n, *ret;

    ret = NULL;

    if (IS_LEAF(node)) {
	ret = node;
    }
    else {
	cmp_y = best_leaf_node(node->y);
	cmp_n = best_leaf_node(node->n);

	if ((cmp_y == NULL || cmp_y->q == NULL) &&
	    (cmp_n == NULL || cmp_n->q == NULL)) {
	    return NULL;
	}
	else if ((cmp_y == NULL) || (cmp_y->q == NULL)) {
	    ret = cmp_n;
	}
	else if ((cmp_n == NULL) || (cmp_n->q == NULL)) {
	    ret = cmp_y;
	}
	else if (cmp_y->wt_ent_dec > cmp_n->wt_ent_dec) {
	    ret = cmp_y;
	}
	else {
	    ret = cmp_n;
	}
    }

    return ret;
}

// Convert from an internal node to a leaf.
static void _convert_to_leaf(node_t *node)
{
	assert(node);
	assert(!IS_LEAF(node));
	_destroy_subtree(node->l);
	_destroy_subtree(node->r);
	node->l = NULL;
	node->r = NULL;
}

static nodeid get_or_create_leaf(fs_ptree *pt, fs_rid pk)
{
    nodeid pos = FS_PTREE_ROOT_NODE;
    for (int i=0; i < 64/FS_PTREE_BRANCH_BITS; i++) {
        int kbranch = PK_BRANCH(pk, i);
again:;
        node *nr = node_ref(pt, pos);
        if (!nr) {
            fs_error(LOG_CRIT, "failed to get node ref");

            break;
        }
        nodeid newpos = nr->branch[kbranch];
        if (newpos == FS_PTREE_NULL_NODE) {
            int done = 0;
            if (i > 1) {
                newpos = fs_ptree_new_leaf(pt);
                LEAF_REF(pt, newpos)->pk = pk;
                done = 1;
            } else {
                newpos = fs_ptree_new_node(pt);
            }
            node_ref(pt, pos)->branch[kbranch] = newpos;

            if (done) {
                return newpos;
            }
        } else if (IS_LEAF(newpos)) {
            const fs_rid existpk = LEAF_REF(pt, newpos)->pk;
            if (pk == existpk) {
                /* PKs are the same, we can reuse the block */
                return newpos;
            }
            /* split and insert node */
            int oldkbr = PK_BRANCH(existpk, i-1);
            nodeid split = fs_ptree_new_node(pt);
            node_ref(pt, pos)->branch[kbranch] = split;
            node_ref(pt, split)->branch[oldkbr] = newpos;
            goto again;
        }
        pos = newpos;
    }

    fs_error(LOG_ERR, "fell through get_or_create_leaf(%016llx)", pk);
    char tmp[256];
    tmp[0] = '\0';
    for (int i=0; i < 64/FS_PTREE_BRANCH_BITS; i++) {
        int kbranch = PK_BRANCH(pk, i);
        char tmp2[16];
        sprintf(tmp2, "%d.", kbranch);
        strcat(tmp, tmp2);
    }
    fs_error(LOG_ERR, "path was %s", tmp);

    return 0;
}

uint32
cnt_node(dtree_node_t *node)
{
    if (!IS_LEAF(node)) {
	return cnt_node(node->y) + cnt_node(node->n) + 1;
    }
    else {
	return 1;
    }
}

LEAFTYPE
radix_del(struct ROOTSTRUCT * tree, LEAFTYPE leaf, EXTRA_ARG aux) {
    LEAFTYPE result;
    struct _internal_node * node, * parent;
    uint32_t dir, dir2;

    if (tree->leafcount == 0) return NO_LEAF;
    if (tree->leafcount == 1) {
        result = tree->root.leaf;
        if (COMPARE(result, leaf) == -1) {
            tree->root.leaf = NO_LEAF;
            tree->leafcount --;
            return result;
        } else return NO_LEAF;
    } /* else */
    node = tree->root.node;
    while (1) {
        dir = DECIDE(leaf, node->critbit, aux);
        if (IS_LEAF(node, dir)) {
            result = node->child[dir].leaf;
            break;
        } else {
            node = node->child[dir].node;
        }
    }
    if (COMPARE(result, leaf) == -1) {
        parent = tree->root.node;
        while (1) {
            dir2 = DECIDE(leaf, parent->critbit, aux);
            if (parent->child[dir2].node == node) break;
            else parent = parent->child[dir2].node;
        }
        if (IS_LEAF(node, 1 - dir)) {
            parent->child[dir2].leaf = node->child[1 - dir].leaf;
            SET_LEAF(parent, dir2);
        } else {
            parent->child[dir2].node = node->child[1 - dir].node;
        }
        tree->leafcount --;
        DEALLOC(node);
        return result;
    } else return NO_LEAF;
}

// Recursively destroys the tree
static void destroy_node(art_node *n) {
    // Break if null
    if (!n) return;

    // Special case leafs
    if (IS_LEAF(n)) {
        free(LEAF_RAW(n));
        return;
    }

    // Handle each node type
    int i;
    union {
        art_node4 *p1;
        art_node16 *p2;
        art_node48 *p3;
        art_node256 *p4;
    } p;
    switch (n->type) {
        case NODE4:
            p.p1 = (art_node4*)n;
            for (i=0;i<n->num_children;i++) {
                destroy_node(p.p1->children[i]);
            }
            break;

        case NODE16:
            p.p2 = (art_node16*)n;
            for (i=0;i<n->num_children;i++) {
                destroy_node(p.p2->children[i]);
            }
            break;

        case NODE48:
            p.p3 = (art_node48*)n;
            for (i=0;i<n->num_children;i++) {
                destroy_node(p.p3->children[i]);
            }
            break;

        case NODE256:
            p.p4 = (art_node256*)n;
            for (i=0;i<256;i++) {
                if (p.p4->children[i])
                    destroy_node(p.p4->children[i]);
            }
            break;

        default:
            abort();
    }

    // Free ourself on the way up
    free(n);
}

// Recursive function to set value for a given network prefix within
// the tree.  (Note: prefix must be in host byte order.)
static void _set_recurse(node_t *node, uint32_t prefix, int len, value_t value)
{
	assert(node);
	assert(0 <= len && len <= 32);

	if (len == 0) {
		// We're at the end of the prefix; make this a leaf and set the value.
		if (!IS_LEAF(node)) {
			_convert_to_leaf(node);
		}
		node->value = value;
		return;
	}

	if (IS_LEAF(node)) {
		// We're not at the end of the prefix, but we hit a leaf.
		if (node->value == value) {
			// A larger prefix has the same value, so we're done.
			return;
		}
		// The larger prefix has a different value, so we need to convert it
		// into an internal node and continue processing on one of the leaves.
		node->l = _create_leaf(node->value);
		node->r = _create_leaf(node->value);
	}

	// We're not at the end of the prefix, and we're at an internal
	// node.  Recurse on the left or right subtree.
	if (prefix & 0x80000000) {
		_set_recurse(node->r, prefix << 1, len - 1, value);
	} else {
		_set_recurse(node->l, prefix << 1, len - 1, value);
	}

	// At this point, we're an internal node, and the value is set
	// by one of our children or its descendent.  If both children are
	// leaves with the same value, we can discard them and become a left.
	if (IS_LEAF(node->r) && IS_LEAF(node->l) && node->r->value == node->l->value) {
		node->value = node->l->value;
		_convert_to_leaf(node);
	}
}

int bst_contains(bst *t, long key)
{
    bstnode *n;

    if (!t || !t->root || IS_LEAF(t->root))
        return 0;

    n = bst_findpath(t->root, key);

    return (key == n->key);
}

// Return the value pertaining to an address.
// (Note: address must be in host byte order.)
int constraint_lookup_ip(constraint_t *con, uint32_t address)
{
	assert(con);
	if (con->optimized) {
		// Use radix optimization
		node_t *node = con->radix[address >> (32 - RADIX_LENGTH)];
		if (IS_LEAF(node)) {
			return node->value;
		}
		return _lookup_ip(node, address << RADIX_LENGTH);
	} else {

static art_leaf* recursive_delete(art_node *n, art_node **ref, const unsigned char *key, int key_len, int depth) {
    // Search terminated
    if (!n) return NULL;

    // Handle hitting a leaf node
    if (IS_LEAF(n)) {
        art_leaf *l = LEAF_RAW(n);
        if (!leaf_matches(l, key, key_len, depth)) {
            *ref = NULL;
            return l;
        }
        return NULL;
    }

    // Bail if the prefix does not match
    if (n->partial_len) {
        int prefix_len = check_prefix(n, key, key_len, depth);
        if (prefix_len != min(MAX_PREFIX_LEN, n->partial_len)) {
            return NULL;
        }
        depth = depth + n->partial_len;
    }

    // Find child node
    art_node **child = find_child(n, key[depth]);
    if (!child) return NULL;

    // If the child is leaf, delete from this node
    if (IS_LEAF(*child)) {
        art_leaf *l = LEAF_RAW(*child);
        if (!leaf_matches(l, key, key_len, depth)) {
            remove_child(n, ref, key[depth], child);
            return l;
        }
        return NULL;

    // Recurse
    } else {
        return recursive_delete(*child, child, key, key_len, depth+1);
    }
}