using System; using System.Collections; using System.Collections.Generic; using System.Runtime.CompilerServices; //TODO Fix namespace or replace namespace Priority_Queue { /// /// Credit: https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp /// A copy of StablePriorityQueue which also has generic priority-type /// /// The values in the queue. Must extend the GenericPriorityQueue class /// The priority-type. Must extend IComparable<TPriority> public sealed class GenericPriorityQueue : IFixedSizePriorityQueue where TItem : GenericPriorityQueueNode where TPriority : IComparable { private int _numNodes; private TItem[] _nodes; private long _numNodesEverEnqueued; /// /// Instantiate a new Priority Queue /// /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) public GenericPriorityQueue(int maxNodes) { #if DEBUG if (maxNodes <= 0) { throw new InvalidOperationException("New queue size cannot be smaller than 1"); } #endif _numNodes = 0; _nodes = new TItem[maxNodes + 1]; _numNodesEverEnqueued = 0; } /// /// Returns the number of nodes in the queue. /// O(1) /// public int Count => _numNodes; /// /// Returns the maximum number of items that can be enqueued at once in this queue. Once you hit this number (ie. once Count == MaxSize), /// attempting to enqueue another item will cause undefined behavior. O(1) /// public int MaxSize => _nodes.Length - 1; /// /// Removes every node from the queue. /// O(n) (So, don't do this often!) /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Clear() { Array.Clear(_nodes, 1, _numNodes); _numNodes = 0; } /// /// Returns (in O(1)!) whether the given node is in the queue. O(1) /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Contains(TItem node) { #if DEBUG 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?"); } #endif return (_nodes[node.QueueIndex] == node); } /// /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out. /// If the queue is full, the result is undefined. /// If the node is already enqueued, the result is undefined. /// O(log n) /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Enqueue(TItem node, TPriority priority) { #if DEBUG 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); } #endif node.Priority = priority; _numNodes++; _nodes[_numNodes] = node; node.QueueIndex = _numNodes; node.InsertionIndex = _numNodesEverEnqueued++; CascadeUp(_nodes[_numNodes]); } [MethodImpl(MethodImplOptions.AggressiveInlining)] private void Swap(TItem node1, TItem node2) { //Swap the nodes _nodes[node1.QueueIndex] = node2; _nodes[node2.QueueIndex] = node1; //Swap their indicies int temp = node1.QueueIndex; node1.QueueIndex = node2.QueueIndex; node2.QueueIndex = temp; } //Performance appears to be slightly better when this is NOT inlined o_O private void CascadeUp(TItem node) { //aka Heapify-up int parent = node.QueueIndex / 2; while (parent >= 1) { var parentNode = _nodes[parent]; if (HasHigherPriority(parentNode, node)) break; //Node has lower priority value, so move it up the heap Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown() parent = node.QueueIndex / 2; } } [MethodImpl(MethodImplOptions.AggressiveInlining)] private void CascadeDown(TItem node) { //aka Heapify-down TItem newParent; int finalQueueIndex = node.QueueIndex; while (true) { newParent = node; int childLeftIndex = 2 * finalQueueIndex; //Check if the left-child is higher-priority than the current node if (childLeftIndex > _numNodes) { //This could be placed outside the loop, but then we'd have to check newParent != node twice node.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = node; break; } var childLeft = _nodes[childLeftIndex]; if (HasHigherPriority(childLeft, newParent)) { newParent = childLeft; } //Check if the right-child is higher-priority than either the current node or the left child int childRightIndex = childLeftIndex + 1; if (childRightIndex <= _numNodes) { var childRight = _nodes[childRightIndex]; if (HasHigherPriority(childRight, newParent)) { newParent = childRight; } } //If either of the children has higher (smaller) priority, swap and continue cascading if (newParent != node) { //Move new parent to its new index. node will be moved once, at the end //Doing it this way is one less assignment operation than calling Swap() _nodes[finalQueueIndex] = newParent; int temp = newParent.QueueIndex; newParent.QueueIndex = finalQueueIndex; finalQueueIndex = temp; } else { //See note above node.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = node; break; } } } /// /// 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 /// [MethodImpl(MethodImplOptions.AggressiveInlining)] private bool HasHigherPriority(TItem higher, TItem lower) { var cmp = higher.Priority.CompareTo(lower.Priority); return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex)); } /// /// 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) /// public bool TryDequeue(out TItem item) { if (_numNodes <= 0) { item = default(TItem); return false; } #if DEBUG 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?)"); } #endif var returnMe = _nodes[1]; Remove(returnMe); item = returnMe; return true; } /// /// 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) /// public void Resize(int maxNodes) { #if DEBUG if (maxNodes <= 0) { throw new InvalidOperationException("Queue size cannot be smaller than 1"); } if (maxNodes < _numNodes) { throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes"); } #endif TItem[] newArray = new TItem[maxNodes + 1]; int highestIndexToCopy = Math.Min(maxNodes, _numNodes); for (int i = 1; i <= highestIndexToCopy; i++) { newArray[i] = _nodes[i]; } _nodes = newArray; } /// /// Returns the head of the queue, without removing it (use Dequeue() for that). /// If the queue is empty, behavior is undefined. /// O(1) /// public TItem First { get { #if DEBUG if (_numNodes <= 0) { throw new InvalidOperationException("Cannot call .First on an empty queue"); } #endif return _nodes[1]; } } /// /// This method must be called on a node every time its priority changes while it is in the queue. /// Forgetting to call this method will result in a corrupted queue! /// Calling this method on a node not in the queue results in undefined behavior /// O(log n) /// [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdatePriority(TItem node, TPriority priority) { #if DEBUG 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); } #endif node.Priority = priority; OnNodeUpdated(node); } private void OnNodeUpdated(TItem node) { //Bubble the updated node up or down as appropriate int parentIndex = node.QueueIndex / 2; var parentNode = _nodes[parentIndex]; if (parentIndex > 0 && HasHigherPriority(node, parentNode)) { CascadeUp(node); } else { //Note that CascadeDown will be called if parentNode == node (that is, node is the root) CascadeDown(node); } } /// /// 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) /// public void Remove(TItem node) { #if DEBUG 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); } #endif //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 var formerLastNode = _nodes[_numNodes]; Swap(node, formerLastNode); _nodes[_numNodes] = null; _numNodes--; //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate OnNodeUpdated(formerLastNode); } public IEnumerator GetEnumerator() { for (int i = 1; i <= _numNodes; i++) yield return _nodes[i]; } IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); } /// /// Should not be called in production code. /// Checks to make sure the queue is still in a valid state. Used for testing/debugging the queue. /// public bool IsValidQueue() { for (int i = 1; i < _nodes.Length; i++) { if (_nodes[i] != null) { int childLeftIndex = 2 * i; if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i])) return false; int childRightIndex = childLeftIndex + 1; if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i])) return false; } } return true; } } }