/*
 * Heap priority queue pseudocode.
 *
 * <sitkarev@unixkomi.ru>
 */

/*
 * Coroutines:
 *
 * grow_heap:		grows queue heap to some extent.
 * left_child:		evaluates to left child index of the heap array.
 * parent_index:	evaluates to parent index of the heap array.
 *
 */

FUNCTION
prioq_put(QUEUE queue, INTEGER e)
	INTEGER i;

	IF (queue.used == queue.size) THEN
		grow_heap(queue);
	END

	i = queue.used;

	WHILE (i AND (queue[parent_index(i)] < e)) DO
		queue[i] = queue[parent_index(i)];
		i = parent_index(i);
	END

	queue.used = queue.used + 1;
	queue[i] = e;
END

PRIVATE 
FUNCTION
prioq_heapify(QUEUE queue)
	INTEGER index, largest, tmp;

	FOR (index = 0, largest = left_child(index);
	     largest < queue.used;
	     idx = largest, largest = left_child(index))
	DO
		IF (queue[largest] < queue[largest+1]) THEN
			largest = largest + 1;
		END

		IF (queue[index] < queue[largest]) THEN
			tmp = queue[largest];
			queue[largest] = queue[index];
			queue[index] = tmp;
		ELSE
			RETURN;
		END
	END

	IF ((largest == queue.used) AND (queue[index] < queue[largest])) THEN
		tmp = queue[largest];
		queue[largest] = queue[index];
		queue[index] = tmp;
	END
END

FUNCTION
prioq_get(QUEUE queue)
	INTEGER e;

	IF (queue.used == 0) THEN
		RETURN queue_is_empty;
	END

	e = queue[0];
	queue.used = queue.used - 1;
	queue[0] = queue[queue.used];

	prioq_heapify(queue);

	RETURN e;
END

FUNCTION
prioq_remove(QUEUE queue, INTEGER index)

	IF ((index < 0) OR (index >= queue.used)) THEN
		RETURN index_out_of_range;
	END

	WHILE (index > 0) DO
		queue[index] = queue[parent_index(index)];
		index = parent_index(index);
	END

	queue.used = queue.used - 1;
	queue[0] = queue[queue.used];

	prioq_heapify(queue);
END