void percolate_up(PQ* pq, int index) {
int parent_index = index/2;
if(pq->is_min_heap) {
while(parent_index >= 1 && pq->heap[parent_index].priority > pq->heap[index].priority) {
pq_swap(pq, index, parent_index);
index = parent_index;
parent_index = index/2;
}
}
else {
while(parent_index >= 1 && pq->heap[parent_index].priority < pq->heap[index].priority) {
pq_swap(pq, index, parent_index);
index = parent_index;
parent_index = index/2;
}
}
}
// Find parents and swap if necessary
void pq_bubble_up(prio_q* pq, int val)
{
// Start with given node and its parent
int p = pq_parent(val);
if(p == -1) return;
// Swap if needed
if(pq->q[p] < pq->q[val])
{
pq_swap(pq, p, val);
// Move to next pair
pq_bubble_up(pq, p);
}
}
// Float an element down its subtree
int pq_bubble_down(prio_q* pq, int val)
{
// Start with given node and its parent
int l = pq_left_child(val);
int r = pq_right_child(val);
int m = val;
if(l <= pq->n && pq->q[l] > pq->q[m]) m = l;
if(r <= pq->n && pq->q[r] > pq->q[m]) m = r;
if(m != val)
{
pq_swap(pq, m, val);
pq_bubble_down(pq, m);
}
return 0;
}
void
pq_delete_min(PQ pq, void *retval)
{
int floater; /* previous loser floating down */
int small_child; /* smaller child of floater */
assert(!pq_is_empty(pq));
/* first copy out the winner */
memcpy(retval, REF(pq, 0), pq->element_length);
--(pq->n);
if(pq_is_empty(pq)) {
/* pq empty, nothing to do */
return;
}
/* else */
memcpy(REF(pq, 0), REF(pq, pq->n), pq->element_length);
floater = 0;
for(;;) {
/* find smaller child of floater */
if(Child(floater, 0) >= pq->n) {
return; /* no children, bail out */
} else if(Child(floater, 1) >= pq->n) {
small_child = Child(floater, 0);
} else if(pq->compare(REF(pq, Child(floater, 0)), REF(pq, Child(floater, 1))) < 0) {
small_child = Child(floater, 0);
} else {
small_child = Child(floater, 1);
}
/* is floater <= small_child? */
if(pq->compare(REF(pq, floater), REF(pq, small_child)) <= 0) {
/* yes, we are done */
return;
} else {
/* no, swap and continue floating down */
pq_swap(pq, floater, small_child);
floater = small_child;
}
}
}
/*
Insert a value with attached min.prority in the priority queue.
priority should be non-negative (>= 0)
returns: 1 if successful, 0 if failed (pq is full)
*/
int pq_enqueue(struct priority_queue *pq, int val, int priority)
{
if (pq == NULL) return 0;
if (pq->size == MAX_HEAP_SIZE) return 0;
if (priority < 0) return 0;
int i;
for (i = ++pq->size; priority < pq->heap[i / 2].priority; i /= 2)
pq_swap(pq, i, i / 2);
pq->heap[i].priority = priority;
pq->heap[i].val = val;
return 1;
}
static void
pq_insert(struct pq *pq, struct pq_node *nd)
{
unsigned i, j;
assert(pq->cnt < pq->size);
i = pq->cnt;
(pq->arr[i] = nd)->idx = i;
nd->pq = pq;
for ( ; i != 0; i = j) {
j = (i - 1) >> 1;
if ((*pq->cmp)(pq->arr[i], pq->arr[j]) >= 0) break;
pq_swap(pq, i, j);
}
++pq->cnt;
}
void percolate_down_max(PQ* pq, int index) {
int max, done = 0;
int left_index = 2*index;
int right_index = left_index+1;
while(left_index <= pq->size && !done) {
max = left_index;
if(right_index <= pq->size)
if(pq->heap[left_index].priority < pq->heap[right_index].priority)
max = right_index;
if(pq->heap[max].priority > pq->heap[index].priority) {
pq_swap(pq, max, index);
index = max;
}
else
done = 1;
left_index = 2*index;
right_index = left_index+1;
}
}
/*
Removes the lowest priority object from the priority queue
returns: 1 if successful, 0 if failed (pq is empty)
note: may remove this from the spec (do cleanup)
*/
int pq_delete(struct priority_queue *pq)
{
if (pq == NULL) return 0;
int i, c;
node last = pq->heap[pq->size--];
for (i = 1; i * 2 <= pq->size; i = c) {
c = i * 2;
if (c < pq->size && pq->heap[c].priority > pq->heap[c+1].priority) c++;
if (last.priority < pq->heap[c].priority) break;
else pq_swap(pq, i, c);
}
pq->heap[i].priority = last.priority;
pq->heap[i].val = last.val;
return 1;
}
void
pq_insert(PQ pq, const void *elt)
{
int floater; /* new element */
while(pq->n + 1 > pq->size) {
pq->size *= 2;
pq->data = realloc(pq->data, pq->element_length * pq->size);
assert(pq->data);
}
/* copy the new element in */
floater = pq->n++;
memcpy(REF(pq, floater), elt, pq->element_length);
/* float it up until it is at the top */
/* or it is no smaller than its parent */
while(floater > 0 && pq->compare(REF(pq, floater), REF(pq, Parent(floater))) <= 0) {
/* it's smaller than its parent */
pq_swap(pq, floater, Parent(floater));
floater = Parent(floater);
}
}
static void
pq_erase(struct pq_node *nd)
{
struct pq *pq;
unsigned i, j, k;
struct pq_node *parent, *left, *right;
int cl, cr;
pq = nd->pq;
if (pq == 0) return;
assert(pq->cnt > 0);
if (--pq->cnt == 0) return;
pq_swap(pq, i = nd->idx, pq->cnt);
for (;;) {
parent = pq->arr[i];
if ((j = (i << 1) + 1) >= pq->cnt) {
/* No children */
break;
}
left = pq->arr[j];
cl = (*pq->cmp)(parent, left);
if ((k = j + 1) >= pq->cnt) {
/* Left child only */
if (cl < 0) {
/* Left child OK */
break;
}
/* Left child not OK */
goto swap_left;
}
/* 2 children */
right = pq->arr[k];
cr = (*pq->cmp)(parent, right);
if (cl < 0) {
/* Left child OK */
if (cr < 0) {
/* Right child OK */
break;
}
/* Left child OK and right child not OK */
goto swap_right;
}
/* Left child not OK */
if (cr < 0) {
/* Right child OK */
goto swap_left;
}
/* Left child not OK and right child not OK */
if ((*pq->cmp)(left, right)) goto swap_left;
swap_right:
pq_swap(pq, i, k);
i = k;
continue;
swap_left:
pq_swap(pq, i, j);
i = j;
}
}
