PriorityQueue fix

Rx.NET/Source/src/System.Reactive/Internal/PriorityQueue.cs

@@ -29,51 +29,38 @@ private bool IsHigherPriority(int left, int right)
             return _items[left].CompareTo(_items[right]) < 0;
         }
 
-        private void Percolate(int index)
+        private int Percolate(int index)
         {
             if (index >= _size || index < 0)
-            {
-                return;
-            }
-
+                return index;
             var parent = (index - 1) / 2;
             if (parent < 0 || parent == index)
-            {
-                return;
-            }
+                return index;
 
             if (IsHigherPriority(index, parent))
             {
                 var temp = _items[index];
                 _items[index] = _items[parent];
                 _items[parent] = temp;
-                Percolate(parent);
+                return Percolate(parent);
             }
-        }
 
-        private void Heapify() => Heapify(index: 0);
+            return index;
+        }
 
         private void Heapify(int index)
         {
             if (index >= _size || index < 0)
-            {
                 return;
-            }
 
             var left = 2 * index + 1;
             var right = 2 * index + 2;
             var first = index;
 
             if (left < _size && IsHigherPriority(left, first))
-            {
                 first = left;
-            }
-
             if (right < _size && IsHigherPriority(right, first))
-            {
                 first = right;
-            }
-
             if (first != index)
             {
                 var temp = _items[index];
@@ -98,7 +85,8 @@ private void RemoveAt(int index)
             _items[index] = _items[--_size];
             _items[_size] = default(IndexedItem);
 
-            Heapify();
+            if (Percolate(index) == index)
+                Heapify(index);
 
             if (_size < _items.Length / 4)
             {
@@ -152,10 +140,7 @@ public int CompareTo(IndexedItem other)
             {
                 var c = Value.CompareTo(other.Value);
                 if (c == 0)
-                {
                     c = Id.CompareTo(other.Id);
-                }
-
                 return c;
             }
         }

Rx.NET/Source/tests/Tests.System.Reactive/Tests/Internal/PriorityQueueTest.cs

@@ -0,0 +1,112 @@
+using System.Reactive;
+using Xunit;
+
+namespace ReactiveTests.Tests
+{
+    public class PriorityQueueTest
+    {
+        [Fact]
+        public void Enqueue_dequeue()
+        {
+            var q = new PriorityQueue<int>();
+
+            for (var i = 0; i < 16; i++)
+            {
+                Assert.Equal(0, q.Count);
+
+                q.Enqueue(i);
+
+                Assert.Equal(1, q.Count);
+                Assert.Equal(i, q.Peek());
+                Assert.Equal(1, q.Count);
+                Assert.Equal(i, q.Dequeue());
+                Assert.Equal(0, q.Count);
+            }
+        }
+
+        [Fact]
+        public void Enqueue_all_dequeue_all()
+        {
+            var q = new PriorityQueue<int>();
+
+            for (var i = 0; i < 33; i++)
+            {
+                q.Enqueue(i);
+                Assert.Equal(i + 1, q.Count);
+            }
+
+            Assert.Equal(33, q.Count);
+
+            for (var i = 0; i < 33; i++)
+            {
+                Assert.Equal(33 - i, q.Count);
+                Assert.Equal(i, q.Peek());
+                Assert.Equal(i, q.Dequeue());
+            }
+
+            Assert.Equal(0, q.Count);
+        }
+
+        [Fact]
+        public void Reverse_Enqueue_all_dequeue_all()
+        {
+            var q = new PriorityQueue<int>();
+
+            for (var i = 32; i >= 0; i--)
+            {
+                q.Enqueue(i);
+                Assert.Equal(33 - i, q.Count);
+            }
+
+            Assert.Equal(33, q.Count);
+
+            for (var i = 0; i < 33; i++)
+            {
+                Assert.Equal(33 - i, q.Count);
+                Assert.Equal(i, q.Peek());
+                Assert.Equal(i, q.Dequeue());
+            }
+
+            Assert.Equal(0, q.Count);
+        }
+
+        [Fact]
+        public void Remove_from_middle()
+        {
+            var q = new PriorityQueue<int>();
+
+            for (var i = 0; i < 33; i++)
+            {
+                q.Enqueue(i);
+            }
+
+            q.Remove(16);
+
+            for (var i = 0; i < 16; i++)
+            {
+                Assert.Equal(i, q.Dequeue());
+            }
+
+            for (var i = 16; i < 32; i++)
+            {
+                Assert.Equal(i + 1, q.Dequeue());
+            }
+        }
+
+        [Fact]
+        public void Repro_329()
+        {
+            var queue = new PriorityQueue<int>();
+
+            queue.Enqueue(2);
+            queue.Enqueue(1);
+            queue.Enqueue(5);
+            queue.Enqueue(2);
+
+            Assert.Equal(1, queue.Dequeue());
+            Assert.Equal(2, queue.Dequeue());
+            Assert.Equal(2, queue.Dequeue());
+            Assert.Equal(5, queue.Dequeue());
+        }
+    }
+}