using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;
namespace DigitalOpus.MB.Core
{
/// From https://www.codeproject.com/Articles/126751/Priority-queue-in-Csharp-with-help-of-heap-data-st.aspx
///
/// Priority queue based on binary heap,
/// Elements with minimum priority dequeued first
///
/// Type of priorities
/// Type of values
public class PriorityQueue : ICollection>
{
public List> _baseHeap;
private IComparer _comparer;
#region Constructors
///
/// Initializes a new instance of priority queue with default initial capacity and default priority comparer
///
public PriorityQueue()
: this(Comparer.Default)
{
}
///
/// Initializes a new instance of priority queue with specified initial capacity and default priority comparer
///
/// initial capacity
public PriorityQueue(int capacity)
: this(capacity, Comparer.Default)
{
}
///
/// Initializes a new instance of priority queue with specified initial capacity and specified priority comparer
///
/// initial capacity
/// priority comparer
public PriorityQueue(int capacity, IComparer comparer)
{
if (comparer == null)
throw new ArgumentNullException();
_baseHeap = new List>(capacity);
_comparer = comparer;
}
///
/// Initializes a new instance of priority queue with default initial capacity and specified priority comparer
///
/// priority comparer
public PriorityQueue(IComparer comparer)
{
if (comparer == null)
throw new ArgumentNullException();
_baseHeap = new List>();
_comparer = comparer;
}
///
/// Initializes a new instance of priority queue with specified data and default priority comparer
///
/// data to be inserted into priority queue
public PriorityQueue(IEnumerable> data)
: this(data, Comparer.Default)
{
}
///
/// Initializes a new instance of priority queue with specified data and specified priority comparer
///
/// data to be inserted into priority queue
/// priority comparer
public PriorityQueue(IEnumerable> data, IComparer comparer)
{
if (data == null || comparer == null)
throw new ArgumentNullException();
_comparer = comparer;
_baseHeap = new List>(data);
// heapify data
for (int pos = _baseHeap.Count / 2 - 1; pos >= 0; pos--)
HeapifyFromBeginningToEnd(pos);
}
#endregion
#region Merging
///
/// Merges two priority queues
///
/// first priority queue
/// second priority queue
/// resultant priority queue
///
/// source priority queues must have equal comparers,
/// otherwise will be thrown
///
public static PriorityQueue MergeQueues(PriorityQueue pq1, PriorityQueue pq2)
{
if (pq1 == null || pq2 == null)
throw new ArgumentNullException();
if (pq1._comparer != pq2._comparer)
throw new InvalidOperationException("Priority queues to be merged must have equal comparers");
return MergeQueues(pq1, pq2, pq1._comparer);
}
///
/// Merges two priority queues and sets specified comparer for resultant priority queue
///
/// first priority queue
/// second priority queue
/// comparer for resultant priority queue
/// resultant priority queue
public static PriorityQueue MergeQueues(PriorityQueue pq1, PriorityQueue pq2, IComparer comparer)
{
if (pq1 == null || pq2 == null || comparer == null)
throw new ArgumentNullException();
// merge data
PriorityQueue result = new PriorityQueue(pq1.Count + pq2.Count, pq1._comparer);
result._baseHeap.AddRange(pq1._baseHeap);
result._baseHeap.AddRange(pq2._baseHeap);
// heapify data
for (int pos = result._baseHeap.Count / 2 - 1; pos >= 0; pos--)
result.HeapifyFromBeginningToEnd(pos);
return result;
}
#endregion
#region Priority queue operations
///
/// Enqueues element into priority queue
///
/// element priority
/// element value
public void Enqueue(TPriority priority, TValue value)
{
Insert(priority, value);
}
///
/// Dequeues element with minimum priority and return its priority and value as
///
/// priority and value of the dequeued element
///
/// Method throws if priority queue is empty
///
public KeyValuePair Dequeue()
{
if (!IsEmpty)
{
KeyValuePair result = _baseHeap[0];
DeleteRoot();
return result;
}
else
throw new InvalidOperationException("Priority queue is empty");
}
///
/// Dequeues element with minimum priority and return its value
///
/// value of the dequeued element
///
/// Method throws if priority queue is empty
///
public TValue DequeueValue()
{
return Dequeue().Value;
}
///
/// Returns priority and value of the element with minimun priority, without removing it from the queue
///
/// priority and value of the element with minimum priority
///
/// Method throws if priority queue is empty
///
public KeyValuePair Peek()
{
if (!IsEmpty)
return _baseHeap[0];
else
throw new InvalidOperationException("Priority queue is empty");
}
///
/// Returns value of the element with minimun priority, without removing it from the queue
///
/// value of the element with minimum priority
///
/// Method throws if priority queue is empty
///
public TValue PeekValue()
{
return Peek().Value;
}
///
/// Gets whether priority queue is empty
///
public bool IsEmpty
{
get { return _baseHeap.Count == 0; }
}
#endregion
#region Heap operations
private void ExchangeElements(int pos1, int pos2)
{
KeyValuePair val = _baseHeap[pos1];
_baseHeap[pos1] = _baseHeap[pos2];
_baseHeap[pos2] = val;
}
private void Insert(TPriority priority, TValue value)
{
KeyValuePair val = new KeyValuePair(priority, value);
_baseHeap.Add(val);
// heap[i] have children heap[2*i + 1] and heap[2*i + 2] and parent heap[(i-1)/ 2];
// heapify after insert, from end to beginning
HeapifyFromEndToBeginning(_baseHeap.Count - 1);
}
private int HeapifyFromEndToBeginning(int pos)
{
if (pos >= _baseHeap.Count) return -1;
while (pos > 0)
{
int parentPos = (pos - 1) / 2;
if (_comparer.Compare(_baseHeap[parentPos].Key, _baseHeap[pos].Key) > 0)
{
ExchangeElements(parentPos, pos);
pos = parentPos;
}
else break;
}
return pos;
}
private void DeleteRoot()
{
if (_baseHeap.Count <= 1)
{
_baseHeap.Clear();
return;
}
_baseHeap[0] = _baseHeap[_baseHeap.Count - 1];
_baseHeap.RemoveAt(_baseHeap.Count - 1);
// heapify
HeapifyFromBeginningToEnd(0);
}
private void HeapifyFromBeginningToEnd(int pos)
{
if (pos >= _baseHeap.Count) return;
// heap[i] have children heap[2*i + 1] and heap[2*i + 2] and parent heap[(i-1)/ 2];
while (true)
{
// on each iteration exchange element with its smallest child
int smallest = pos;
int left = 2 * pos + 1;
int right = 2 * pos + 2;
if (left < _baseHeap.Count && _comparer.Compare(_baseHeap[smallest].Key, _baseHeap[left].Key) > 0)
smallest = left;
if (right < _baseHeap.Count && _comparer.Compare(_baseHeap[smallest].Key, _baseHeap[right].Key) > 0)
smallest = right;
if (smallest != pos)
{
ExchangeElements(smallest, pos);
pos = smallest;
}
else break;
}
}
#endregion
#region ICollection> implementation
///
/// Enqueus element into priority queue
///
/// element to add
public void Add(KeyValuePair item)
{
Enqueue(item.Key, item.Value);
}
///
/// Clears the collection
///
public void Clear()
{
_baseHeap.Clear();
}
///
/// Determines whether the priority queue contains a specific element
///
/// The object to locate in the priority queue
/// true if item is found in the priority queue; otherwise, false.
public bool Contains(KeyValuePair item)
{
return _baseHeap.Contains(item);
}
public bool TryFindValue(TPriority item,out TValue foundVersion){
for (int i = 0; i < _baseHeap.Count; i++) {
if (_comparer.Compare(item,_baseHeap[i].Key) == 0){
foundVersion = _baseHeap[i].Value;
return true;
}
}
foundVersion = default(TValue);
return false;
}
///
/// Gets number of elements in the priority queue
///
public int Count
{
get { return _baseHeap.Count; }
}
///
/// Copies the elements of the priority queue to an Array, starting at a particular Array index.
///
/// The one-dimensional Array that is the destination of the elements copied from the priority queue. The Array must have zero-based indexing.
/// The zero-based index in array at which copying begins.
///
/// It is not guaranteed that items will be copied in the sorted order.
///
public void CopyTo(KeyValuePair[] array, int arrayIndex)
{
_baseHeap.CopyTo(array, arrayIndex);
}
///
/// Gets a value indicating whether the collection is read-only.
///
///
/// For priority queue this property returns false.
///
public bool IsReadOnly
{
get { return false; }
}
///
/// Removes the first occurrence of a specific object from the priority queue.
///
/// The object to remove from the ICollection <(Of <(T >)>).
/// true if item was successfully removed from the priority queue.
/// This method returns false if item is not found in the collection.
public bool Remove(KeyValuePair item)
{
// find element in the collection and remove it
int elementIdx = _baseHeap.IndexOf(item);
if (elementIdx < 0) return false;
//remove element
_baseHeap[elementIdx] = _baseHeap[_baseHeap.Count - 1];
_baseHeap.RemoveAt(_baseHeap.Count - 1);
// heapify
int newPos = HeapifyFromEndToBeginning(elementIdx);
if (newPos == elementIdx)
HeapifyFromBeginningToEnd(elementIdx);
return true;
}
///
/// Returns an enumerator that iterates through the collection.
///
/// Enumerator
///
/// Returned enumerator does not iterate elements in sorted order.
public IEnumerator> GetEnumerator()
{
return _baseHeap.GetEnumerator();
}
///
/// Returns an enumerator that iterates through the collection.
///
/// Enumerator
///
/// Returned enumerator does not iterate elements in sorted order.
IEnumerator IEnumerable.GetEnumerator()
{
return this.GetEnumerator();
}
#endregion
}
}