/// <summary>
/// Removes a node from the queue. The node does not need to be the head of the queue.
/// If the node is not in the queue, the result is undefined. If unsure, check Contains() first
/// O(log n)
/// </summary>
public void Remove(PriorityQueueNode <TPriority, TValue> node)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (!Contains(node))
{
throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
}
//If the node is already the last node, we can remove it immediately
if (node.QueueIndex == _numNodes)
{
_nodes[_numNodes] = null;
_numNodes--;
return;
}
//Swap the node with the last node
PriorityQueueNode <TPriority, TValue> formerLastNode = _nodes[_numNodes];
_nodes[node.QueueIndex] = formerLastNode;
formerLastNode.QueueIndex = node.QueueIndex;
_nodes[_numNodes] = null;
_numNodes--;
//Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
OnNodeUpdated(formerLastNode);
}
/// <summary>
/// Returns (in O(1)!) whether the given node is in the queue. O(1)
/// </summary>
public bool Contains(PriorityQueueNode <TPriority, TValue> node)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length)
{
throw new InvalidOperationException(
"node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
}
return(_nodes[node.QueueIndex] == node);
}
void OnNodeUpdated(PriorityQueueNode <TPriority, TValue> node)
{
//Bubble the updated node up or down as appropriate
int parentIndex = node.QueueIndex >> 1;
if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex]))
{
CascadeUp(node);
}
else
{
//Note that CascadeDown will be called if parentNode == node (that is, node is the root)
CascadeDown(node);
}
}
/// <summary>
/// This method must be called on a node every time its priority changes while it is in the queue.
/// <b>Forgetting to call this method will result in a corrupted queue!</b>
/// Calling this method on a node not in the queue results in undefined behavior
/// O(log n)
/// </summary>
public void UpdatePriority(PriorityQueueNode <TPriority, TValue> node, TPriority priority)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (!Contains(node))
{
throw new InvalidOperationException(
"Cannot call UpdatePriority() on a node which is not enqueued: " + node);
}
node.Priority = priority;
OnNodeUpdated(node);
}
/// <summary>
/// Resize the queue so it can accept more nodes. All currently enqueued nodes are remain.
/// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
/// O(n)
/// </summary>
public void Resize(int maxNodes)
{
if (maxNodes <= 0)
{
throw new ArgumentOutOfRangeException(nameof(maxNodes), "Queue size cannot be smaller than 1");
}
if (maxNodes < _numNodes)
{
throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
}
var newArray = new PriorityQueueNode <TPriority, TValue> [maxNodes + 1];
int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
Array.Copy(_nodes, newArray, highestIndexToCopy + 1);
_nodes = newArray;
}
void CascadeUp(PriorityQueueNode <TPriority, TValue> node)
{
//aka Heapify-up
int parent;
if (node.QueueIndex > 1)
{
parent = node.QueueIndex >> 1;
PriorityQueueNode <TPriority, TValue> parentNode = _nodes[parent];
if (HasHigherPriority(parentNode, node))
{
return;
}
//Node has lower priority value, so move parent down the heap to make room
_nodes[node.QueueIndex] = parentNode;
parentNode.QueueIndex = node.QueueIndex;
node.QueueIndex = parent;
}
else
{
return;
}
while (parent > 1)
{
parent >>= 1;
PriorityQueueNode <TPriority, TValue> parentNode = _nodes[parent];
if (HasHigherPriority(parentNode, node))
{
break;
}
//Node has lower priority value, so move parent down the heap to make room
_nodes[node.QueueIndex] = parentNode;
parentNode.QueueIndex = node.QueueIndex;
node.QueueIndex = parent;
}
_nodes[node.QueueIndex] = node;
}
/// <summary>
/// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
/// If queue is empty, result is undefined
/// O(log n)
/// </summary>
public PriorityQueueNode <TPriority, TValue> Dequeue()
{
if (_numNodes <= 0)
{
throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
}
if (!IsValidQueue())
{
throw new InvalidOperationException(
"Queue has been corrupted (Did you update a node priority manually instead of calling " +
"UpdatePriority()? Or add the same node to two different queues?)");
}
PriorityQueueNode <TPriority, TValue> returnMe = _nodes[1];
//If the node is already the last node, we can remove it immediately
if (_numNodes == 1)
{
_nodes[1] = null;
_numNodes = 0;
return(returnMe);
}
//Swap the node with the last node
PriorityQueueNode <TPriority, TValue> formerLastNode = _nodes[_numNodes];
_nodes[1] = formerLastNode;
formerLastNode.QueueIndex = 1;
_nodes[_numNodes] = null;
_numNodes--;
//Now bubble formerLastNode (which is no longer the last node) down
CascadeDown(formerLastNode);
return(returnMe);
}
/// <summary>
/// <para>
/// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by
/// first-in-first-out.
/// </para>
/// If the queue is full, the result is undefined. If the node is already enqueued, the result is undefined.
/// O(log n)
/// </summary>
public void Enqueue(PriorityQueueNode <TPriority, TValue> node, TPriority priority)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (_numNodes >= _nodes.Length - 1)
{
throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
}
if (Contains(node))
{
throw new InvalidOperationException("Node is already enqueued: " + node);
}
node.Priority = priority;
_numNodes++;
_nodes[_numNodes] = node;
node.QueueIndex = _numNodes;
node.InsertionIndex = _numNodesEverEnqueued++;
CascadeUp(node);
}
/// <summary>
/// Returns true if 'higher' has higher priority than 'lower', false otherwise.
/// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
/// </summary>
bool HasHigherPriority(PriorityQueueNode <TPriority, TValue> higher, PriorityQueueNode <TPriority, TValue> lower)
{
int cmp = _comparer(higher.Priority, lower.Priority);
return(cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
}
void CascadeDown(PriorityQueueNode <TPriority, TValue> node)
{
//aka Heapify-down
int finalQueueIndex = node.QueueIndex;
int childLeftIndex = 2 * finalQueueIndex;
// If leaf node, we're done
if (childLeftIndex > _numNodes)
{
return;
}
// Check if the left-child is higher-priority than the current node
int childRightIndex = childLeftIndex + 1;
PriorityQueueNode <TPriority, TValue> childLeft = _nodes[childLeftIndex];
if (HasHigherPriority(childLeft, node))
{
// Check if there is a right child. If not, swap and finish.
if (childRightIndex > _numNodes)
{
node.QueueIndex = childLeftIndex;
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
_nodes[childLeftIndex] = node;
return;
}
// Check if the left-child is higher-priority than the right-child
PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex];
if (HasHigherPriority(childLeft, childRight))
{
// left is highest, move it up and continue
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
finalQueueIndex = childLeftIndex;
}
else
{
// right is even higher, move it up and continue
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
}
// Not swapping with left-child, does right-child exist?
else if (childRightIndex > _numNodes)
{
return;
}
else
{
// Check if the right-child is higher-priority than the current node
PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex];
if (HasHigherPriority(childRight, node))
{
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
// Neither child is higher-priority than current, so finish and stop.
else
{
return;
}
}
while (true)
{
childLeftIndex = 2 * finalQueueIndex;
// If leaf node, we're done
if (childLeftIndex > _numNodes)
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
// Check if the left-child is higher-priority than the current node
childRightIndex = childLeftIndex + 1;
childLeft = _nodes[childLeftIndex];
if (HasHigherPriority(childLeft, node))
{
// Check if there is a right child. If not, swap and finish.
if (childRightIndex > _numNodes)
{
node.QueueIndex = childLeftIndex;
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
_nodes[childLeftIndex] = node;
break;
}
// Check if the left-child is higher-priority than the right-child
PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex];
if (HasHigherPriority(childLeft, childRight))
{
// left is highest, move it up and continue
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
finalQueueIndex = childLeftIndex;
}
else
{
// right is even higher, move it up and continue
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
}
// Not swapping with left-child, does right-child exist?
else if (childRightIndex > _numNodes)
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
else
{
// Check if the right-child is higher-priority than the current node
PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex];
if (HasHigherPriority(childRight, node))
{
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
// Neither child is higher-priority than current, so finish and stop.
else
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
}
}
}
