void rbtree_remove(struct rbtree_node *node, struct rbtree *tree)
{
struct rbtree_node *parent = get_parent(node);
struct rbtree_node *left = node->left;
struct rbtree_node *right = node->right;
struct rbtree_node *next;
enum rb_color color;
if (node == tree->first)
tree->first = rbtree_next(node);
if (node == tree->last)
tree->last = rbtree_prev(node);
if (!left)
next = right;
else if (!right)
next = left;
else
next = get_first(right);
if (parent)
set_child(next, parent, parent->left == node);
else
tree->root = next;
if (left && right) {
color = get_color(next);
set_color(get_color(node), next);
next->left = left;
set_parent(next, left);
if (next != right) {
parent = get_parent(next);
set_parent(get_parent(node), next);
node = next->right;
parent->left = node;
next->right = right;
set_parent(next, right);
} else {
set_parent(parent, next);
parent = next;
node = next->right;
}
} else {
color = get_color(node);
node = next;
}
/*
* 'node' is now the sole successor's child and 'parent' its
* new parent (since the successor can have been moved).
*/
if (node)
set_parent(parent, node);
/*
* The 'easy' cases.
*/
if (color == RB_RED)
return;
if (node && is_red(node)) {
set_color(RB_BLACK, node);
return;
}
do {
if (node == tree->root)
break;
if (node == parent->left) {
struct rbtree_node *sibling = parent->right;
if (is_red(sibling)) {
set_color(RB_BLACK, sibling);
set_color(RB_RED, parent);
rotate_left(parent, tree);
sibling = parent->right;
}
if ((!sibling->left || is_black(sibling->left))
&& (!sibling->right || is_black(sibling->right))) {
set_color(RB_RED, sibling);
node = parent;
parent = get_parent(parent);
continue;
}
if (!sibling->right || is_black(sibling->right)) {
set_color(RB_BLACK, sibling->left);
set_color(RB_RED, sibling);
rotate_right(sibling, tree);
sibling = parent->right;
}
set_color(get_color(parent), sibling);
set_color(RB_BLACK, parent);
set_color(RB_BLACK, sibling->right);
rotate_left(parent, tree);
node = tree->root;
break;
} else {
struct rbtree_node *sibling = parent->left;
if (is_red(sibling)) {
set_color(RB_BLACK, sibling);
set_color(RB_RED, parent);
rotate_right(parent, tree);
sibling = parent->left;
}
if ((!sibling->left || is_black(sibling->left))
&& (!sibling->right || is_black(sibling->right))) {
set_color(RB_RED, sibling);
node = parent;
parent = get_parent(parent);
continue;
}
if (!sibling->left || is_black(sibling->left)) {
set_color(RB_BLACK, sibling->right);
set_color(RB_RED, sibling);
rotate_left(sibling, tree);
sibling = parent->left;
}
set_color(get_color(parent), sibling);
set_color(RB_BLACK, parent);
set_color(RB_BLACK, sibling->left);
rotate_right(parent, tree);
node = tree->root;
break;
}
} while (is_black(node));
if (node)
set_color(RB_BLACK, node);
}
int router_nearest(struct router_t* router, const uint8_t id[N_NODEID], struct node_t* nodes[], size_t count)
{
int i, min, diff;
uint8_t xor[N_NODEID];
heap_t* heap;
struct rbitem_t* item;
struct rbtree_node_t* node;
const struct rbtree_node_t* prev;
const struct rbtree_node_t* next;
heap = heap_create(node_compare_less, (void*)id);
heap_reserve(heap, count + 1);
min = N_BITS;
locker_lock(&router->locker);
rbtree_find(&router->rbtree, id, &node);
if (NULL == node)
{
locker_unlock(&router->locker);
return 0;
}
item = rbtree_entry(node, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
min = min < diff ? min : diff;
heap_push(heap, item->node);
prev = rbtree_prev(node);
next = rbtree_next(node);
do
{
while (prev)
{
item = rbtree_entry(prev, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
heap_push(heap, item->node);
if (heap_size(heap) > (int)count)
heap_pop(heap);
prev = rbtree_prev(prev);
if (diff < min)
{
min = diff;
break; // try right
}
}
while (next)
{
item = rbtree_entry(next, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
heap_push(heap, item->node);
if (heap_size(heap) > (int)count)
heap_pop(heap);
next = rbtree_next(next);
if (diff < min)
{
min = diff;
break; // try left
}
}
} while (heap_size(heap) < (int)count && (prev || next));
for (i = 0; i < (int)count && !heap_empty(heap); i++)
{
nodes[i] = heap_top(heap);
node_addref(nodes[i]);
heap_pop(heap);
}
locker_unlock(&router->locker);
heap_destroy(heap);
return i;
}