feat: Adding additional enqueue methods to PriorityQueue

ICD.Common.Utils.Tests/Collections/PriorityQueueTest.cs

@@ -98,6 +98,51 @@ namespace ICD.Common.Utils.Tests.Collections
 			Assert.AreEqual(1, dequeue[2]);
 		}
 
+		[Test]
+		public void EnqueueFirstTest()
+		{
+			PriorityQueue<int> queue = new PriorityQueue<int>();
+
+			queue.Enqueue(1, int.MaxValue);
+			queue.Enqueue(2, int.MinValue);
+			queue.EnqueueFirst(3);
+
+			Assert.AreEqual(3, queue.Count);
+
+			Assert.AreEqual(3, queue.Dequeue());
+		}
+
+		[Test]
+		public void EnqueueRemoveTest()
+		{
+			PriorityQueue<int> queue = new PriorityQueue<int>();
+
+			queue.Enqueue(1);
+			queue.Enqueue(2);
+			queue.Enqueue(3);
+
+			queue.EnqueueRemove(4, i => i == 2);
+
+			Assert.AreEqual(3, queue.Count);
+
+			List<int> dequeue = new List<int>
+			{
+				queue.Dequeue(),
+				queue.Dequeue(),
+				queue.Dequeue()
+			};
+
+			Assert.AreEqual(1, dequeue[0]);
+			Assert.AreEqual(4, dequeue[1]);
+			Assert.AreEqual(3, dequeue[2]);
+		}
+
+		[Test]
+		public void EnqueueRemovePriorityTest()
+		{
+			Assert.Inconclusive();
+		}
+
 		[Test]
 		public void DequeueTest()
 		{

ICD.Common.Utils/Collections/PriorityQueue.cs

@@ -80,6 +80,85 @@ namespace ICD.Common.Utils.Collections
 			m_Count++;
 		}
 
+		/// <summary>
+		/// Enqueues the item at the beginning of the queue.
+		/// </summary>
+		/// <param name="item"></param>
+		[PublicAPI]
+		public void EnqueueFirst(T item)
+		{
+			const int priority = int.MinValue;
+
+			if (!m_PriorityToQueue.ContainsKey(priority))
+				m_PriorityToQueue.Add(priority, new List<T>());
+
+			m_PriorityToQueue[priority].Insert(0, item);
+			m_Count++;
+		}
+
+		/// <summary>
+		/// Removes any items in the queue matching the predicate.
+		/// Inserts the given item in the position of the first removed item, or at the end of the queue.
+		/// This is useful for reducing duplication, or replacing items with something more pertinant.
+		/// </summary>
+		/// <param name="item"></param>
+		/// <param name="remove"></param>
+		[PublicAPI]
+		public void EnqueueRemove(T item, Func<T, bool> remove)
+		{
+			if (remove == null)
+				throw new ArgumentNullException("remove");
+
+			EnqueueRemove(item, remove, int.MaxValue);
+		}
+
+		/// <summary>
+		/// Removes any items in the queue matching the predicate.
+		/// Inserts the given item in the position of the first removed item, or at the end of the queue.
+		/// This is useful for reducing duplication, or replacing items with something more pertinant.
+		/// </summary>
+		/// <param name="item"></param>
+		/// <param name="remove"></param>
+		/// <param name="priority"></param>
+		[PublicAPI]
+		public void EnqueueRemove(T item, Func<T, bool> remove, int priority)
+		{
+			if (remove == null)
+				throw new ArgumentNullException("remove");
+
+			bool inserted = false;
+
+			foreach (KeyValuePair<int, List<T>> kvp in m_PriorityToQueue)
+			{
+				int[] removeIndices =
+					kvp.Value
+					   .FindIndices(v => remove(v))
+					   .Reverse()
+					   .ToArray();
+
+				if (removeIndices.Length == 0)
+					continue;
+
+				foreach (int removeIndex in removeIndices)
+				{
+					kvp.Value.RemoveAt(removeIndex);
+					m_Count--;
+				}
+
+				if (!inserted)
+				{
+					int insertIndex = removeIndices[0];
+					kvp.Value.Insert(insertIndex, item);
+					m_Count++;
+
+					inserted = true;
+				}
+			}
+
+			if (!inserted)
+				Enqueue(item, priority);
+		}
+
 		/// <summary>
 		/// Dequeues the first item with the lowest priority value.
 		/// </summary>