2 files changed, 554 insertions, 45 deletions

diff --git a/OpenSim/Framework/MinHeap.cs b/OpenSim/Framework/MinHeap.cs
new file mode 100755
index 0000000..ad39bbc
--- /dev/null
+++ b/OpenSim/Framework/MinHeap.cs
@@ -0,0 +1,375 @@
+using System;
+using System.Threading;
+using System.Collections;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+
+namespace OpenSim.Framework
+{
+    public interface IHandle { }
+
+    [Serializable, ComVisible(false)]
+    public class MinHeap<T> : ICollection<T>, ICollection
+    {
+        private class Handle : IHandle
+        {
+            internal int index = -1;
+            internal MinHeap<T> heap = null;
+
+            internal void Clear()
+            {
+                this.index = -1;
+                this.heap = null;
+            }
+        }
+
+        private struct HeapItem
+        {
+            internal T value;
+            internal Handle handle;
+
+            internal HeapItem(T value, Handle handle)
+            {
+                this.value = value;
+                this.handle = handle;
+            }
+
+            internal void Clear()
+            {
+                this.value = default(T);
+                if (this.handle != null)
+                {
+                    this.handle.Clear();
+                    this.handle = null;
+                }
+            }
+        }
+
+        public const int DEFAULT_CAPACITY = 4;
+
+        private HeapItem[] items;
+        private int size;
+        private object sync_root;
+        private int version;
+
+        private Comparison<T> comparison;
+
+        public MinHeap() : this(DEFAULT_CAPACITY, Comparer<T>.Default) { }
+        public MinHeap(int capacity) : this(capacity, Comparer<T>.Default) { }
+        public MinHeap(IComparer<T> comparer) : this(DEFAULT_CAPACITY, comparer) { }
+        public MinHeap(int capacity, IComparer<T> comparer) :
+            this(capacity, new Comparison<T>(comparer.Compare)) { }
+        public MinHeap(Comparison<T> comparison) : this(DEFAULT_CAPACITY, comparison) { }
+        public MinHeap(int capacity, Comparison<T> comparison)
+        {
+            this.items = new HeapItem[capacity];
+            this.comparison = comparison;
+            this.size = this.version = 0;
+        }
+
+        public int Count { get { return this.size; } }
+
+        public bool IsReadOnly { get { return false; } }
+
+        public bool IsSynchronized { get { return false; } }
+
+        public T this[IHandle key]
+        {
+            get
+            {
+                Handle handle = ValidateThisHandle(key);
+                return this.items[handle.index].value;
+            }
+
+            set
+            {
+                Handle handle = ValidateThisHandle(key);
+                this.items[handle.index].value = value;
+                if (!BubbleUp(handle.index))
+                    BubbleDown(handle.index);
+            }
+        }
+
+        public object SyncRoot
+        {
+            get
+            {
+                if (this.sync_root == null)
+                    Interlocked.CompareExchange<object>(ref this.sync_root, new object(), null);
+                return this.sync_root;
+            }
+        }
+
+        private Handle ValidateHandle(IHandle ihandle)
+        {
+            if (ihandle == null)
+                throw new ArgumentNullException("handle");
+            Handle handle = ihandle as Handle;
+            if (handle == null)
+                throw new InvalidOperationException("handle is not valid");
+            return handle;
+        }
+
+        private Handle ValidateThisHandle(IHandle ihandle)
+        {
+            Handle handle = ValidateHandle(ihandle);
+            if (!object.ReferenceEquals(handle.heap, this))
+                throw new InvalidOperationException("handle is not valid for this heap");
+            if (handle.index < 0)
+                throw new InvalidOperationException("handle is not associated to a value");
+            return handle;
+        }
+
+        private void Set(HeapItem item, int index)
+        {
+            this.items[index] = item;
+            if (item.handle != null)
+                item.handle.index = index;
+        }
+
+        private bool BubbleUp(int index)
+        {
+            HeapItem item = this.items[index];
+            int current, parent;
+
+            for (current = index, parent = (current - 1) / 2;
+                (current > 0) && (this.comparison(this.items[parent].value, item.value)) > 0;
+                current = parent, parent = (current - 1) / 2)
+            {
+                Set(this.items[parent], current);
+            }
+
+            if (current != index)
+            {
+                Set(item, current);
+                ++this.version;
+                return true;
+            }
+            return false;
+        }
+
+        private void BubbleDown(int index)
+        {
+            HeapItem item = this.items[index];
+            int current, child;
+
+            for (current = index, child = (2 * current) + 1;
+                current < this.size / 2;
+                current = child, child = (2 * current) + 1)
+            {
+                if ((child < this.size - 1) && this.comparison(this.items[child].value, this.items[child + 1].value) > 0)
+                    ++child;
+                if (this.comparison(this.items[child].value, item.value) >= 0)
+                    break;
+                Set(this.items[child], current);
+            }
+
+            if (current != index)
+            {
+                Set(item, current);
+                ++this.version;
+            }
+        }
+
+        public bool TryGetValue(IHandle key, out T value)
+        {
+            Handle handle = ValidateHandle(key);
+            if (handle.index > -1)
+            {
+                value = this.items[handle.index].value;
+                return true;
+            }
+            value = default(T);
+            return false;
+        }
+
+        public bool ContainsHandle(IHandle ihandle)
+        {
+            Handle handle = ValidateHandle(ihandle);
+            return object.ReferenceEquals(handle.heap, this) && handle.index > -1;
+        }
+
+        public void Add(T value, ref IHandle handle)
+        {
+            if (handle == null)
+                handle = new Handle();
+            Add(value, handle);
+        }
+
+        public void Add(T value, IHandle ihandle)
+        {
+            if (this.size == this.items.Length)
+            {
+                int capacity = (int)((this.items.Length * 200L) / 100L);
+                if (capacity < (this.items.Length + DEFAULT_CAPACITY))
+                    capacity = this.items.Length + DEFAULT_CAPACITY;
+                Array.Resize<HeapItem>(ref this.items, capacity);
+            }
+
+            Handle handle = null;
+            if (ihandle != null)
+            {
+                handle = ValidateHandle(ihandle);
+                handle.heap = this;
+            }
+
+            HeapItem item = new MinHeap<T>.HeapItem(value, handle);
+
+            Set(item, this.size);
+            BubbleUp(this.size++);
+        }
+
+        public void Add(T value)
+        {
+            Add(value, null);
+        }
+
+        public T Min()
+        {
+            if (this.size == 0)
+                throw new InvalidOperationException("Heap is empty");
+
+            return this.items[0].value;
+        }
+
+        public void Clear()
+        {
+            for (int index = 0; index < this.size; ++index)
+                this.items[index].Clear();
+            this.size = 0;
+            ++this.version;
+        }
+
+        public void TrimExcess()
+        {
+            int length = (int)(this.items.Length * 0.9);
+            if (this.size < length)
+                Array.Resize<HeapItem>(ref this.items, Math.Min(this.size, DEFAULT_CAPACITY));
+        }
+
+        private void RemoveAt(int index)
+        {
+            if (this.size == 0)
+                throw new InvalidOperationException("Heap is empty");
+            if (index >= this.size)
+                throw new ArgumentOutOfRangeException("index");
+
+            this.items[index].Clear();
+            if (--this.size > 0 && index != this.size)
+            {
+                Set(this.items[this.size], index);
+                if (!BubbleUp(index))
+                    BubbleDown(index);
+            }
+        }
+
+        public T RemoveMin()
+        {
+            if (this.size == 0)
+                throw new InvalidOperationException("Heap is empty");
+
+            HeapItem item = this.items[0];
+            RemoveAt(0);
+            return item.value;
+        }
+
+        public T Remove(IHandle ihandle)
+        {
+            Handle handle = ValidateThisHandle(ihandle);
+            HeapItem item = this.items[handle.index];
+            RemoveAt(handle.index);
+            return item.value;
+        }
+
+        private int GetIndex(T value)
+        {
+            EqualityComparer<T> comparer = EqualityComparer<T>.Default;
+            int index;
+
+            for (index = 0; index < this.size; ++index)
+            {
+                if (comparer.Equals(this.items[index].value, value))
+                    return index;
+            }
+            return -1;
+        }
+
+        public bool Contains(T value)
+        {
+            return GetIndex(value) != -1;
+        }
+
+        public bool Remove(T value)
+        {
+            int index = GetIndex(value);
+            if (index != -1)
+            {
+                RemoveAt(index);
+                return true;
+            }
+            return false;
+        }
+
+        public void CopyTo(T[] array, int index)
+        {
+            if (array == null)
+                throw new ArgumentNullException("array");
+            if (array.Rank != 1)
+                throw new ArgumentException("Multidimensional array not supported");
+            if (array.GetLowerBound(0) != 0)
+                throw new ArgumentException("Non-zero lower bound array not supported");
+
+            int length = array.Length;
+            if ((index < 0) || (index > length))
+                throw new ArgumentOutOfRangeException("index");
+            if ((length - index) < this.size)
+                throw new ArgumentException("Not enough space available in array starting at index");
+
+            for (int i = 0; i < this.size; ++i)
+                array[index + i] = this.items[i].value;
+        }
+
+        public void CopyTo(Array array, int index)
+        {
+            if (array == null)
+                throw new ArgumentNullException("array");
+            if (array.Rank != 1)
+                throw new ArgumentException("Multidimensional array not supported");
+            if (array.GetLowerBound(0) != 0)
+                throw new ArgumentException("Non-zero lower bound array not supported");
+
+            int length = array.Length;
+            if ((index < 0) || (index > length))
+                throw new ArgumentOutOfRangeException("index");
+            if ((length - index) < this.size)
+                throw new ArgumentException("Not enough space available in array starting at index");
+
+            try
+            {
+                for (int i = 0; i < this.size; ++i)
+                    array.SetValue(this.items[i].value, index + i);
+            }
+            catch (ArrayTypeMismatchException)
+            {
+                throw new ArgumentException("Invalid array type");
+            }
+        }
+
+        public IEnumerator<T> GetEnumerator()
+        {
+            int version = this.version;
+
+            for (int index = 0; index < this.size; ++index)
+            {
+                if (version != this.version)
+                    throw new InvalidOperationException("Heap was modified while enumerating");
+                yield return this.items[index].value;
+            }
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return GetEnumerator();
+        }
+    }
+}
diff --git a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
index 82a2cdd..93fdeef 100644
--- a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
@@ -321,11 +321,14 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
         private int m_cachedTextureSerial;
         private Timer m_avatarTerseUpdateTimer;
-        private List<ImprovedTerseObjectUpdatePacket.ObjectDataBlock> m_avatarTerseUpdates = new List<ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
+        private PriorityQueue<double, ImprovedTerseObjectUpdatePacket.ObjectDataBlock> m_avatarTerseUpdates_ =
+            new PriorityQueue<double, ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
         private Timer m_primTerseUpdateTimer;
-        private List<ImprovedTerseObjectUpdatePacket.ObjectDataBlock> m_primTerseUpdates = new List<ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
+        private PriorityQueue<double, ImprovedTerseObjectUpdatePacket.ObjectDataBlock> m_primTerseUpdates_ =
+            new PriorityQueue<double, ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
         private Timer m_primFullUpdateTimer;
-        private List<ObjectUpdatePacket.ObjectDataBlock> m_primFullUpdates = new List<ObjectUpdatePacket.ObjectDataBlock>();
+        private PriorityQueue<double, ObjectUpdatePacket.ObjectDataBlock> m_primFullUpdates_ =
+            new PriorityQueue<double, ObjectUpdatePacket.ObjectDataBlock>();
         private int m_moneyBalance;
         private int m_animationSequenceNumber = 1;
         private bool m_SendLogoutPacketWhenClosing = true;
@@ -3435,16 +3438,16 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             ImprovedTerseObjectUpdatePacket.ObjectDataBlock terseBlock =
                 CreateAvatarImprovedBlock(localID, position, velocity,rotation);
                 
-            lock (m_avatarTerseUpdates)
+            lock (m_avatarTerseUpdates_.SyncRoot)
             {
-                m_avatarTerseUpdates.Add(terseBlock);
+                m_avatarTerseUpdates_.Enqueue(DateTime.Now.ToOADate(), terseBlock, localID);
 
                 // If packet is full or own movement packet, send it.
-                if (m_avatarTerseUpdates.Count >= m_avatarTerseUpdatesPerPacket)
+                if (m_avatarTerseUpdates_.Count >= m_avatarTerseUpdatesPerPacket)
                 {
                     ProcessAvatarTerseUpdates(this, null);
                 }
-                else if (m_avatarTerseUpdates.Count == 1)
+                else if (m_avatarTerseUpdates_.Count == 1)
                 {
                     lock (m_avatarTerseUpdateTimer)
                         m_avatarTerseUpdateTimer.Start();
@@ -3454,7 +3457,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
         private void ProcessAvatarTerseUpdates(object sender, ElapsedEventArgs e)
         {
-            lock (m_avatarTerseUpdates)
+            lock (m_avatarTerseUpdates_.SyncRoot)
             {
                 ImprovedTerseObjectUpdatePacket terse = (ImprovedTerseObjectUpdatePacket)PacketPool.Instance.GetPacket(PacketType.ImprovedTerseObjectUpdate);
 
@@ -3465,8 +3468,8 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                         (ushort)(Scene.TimeDilation * ushort.MaxValue);
 
                 int max = m_avatarTerseUpdatesPerPacket;
-                if (max > m_avatarTerseUpdates.Count)
-                    max = m_avatarTerseUpdates.Count;
+                if (max > m_avatarTerseUpdates_.Count)
+                    max = m_avatarTerseUpdates_.Count;
 
                 int count = 0;
                 int size = 0;
@@ -3474,30 +3477,30 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 byte[] zerobuffer = new byte[1024];
                 byte[] blockbuffer = new byte[1024];
 
+                Queue<ImprovedTerseObjectUpdatePacket.ObjectDataBlock> updates = new Queue<ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
+
                 for (count = 0 ; count < max ; count++)
                 {
                     int length = 0;
-                    m_avatarTerseUpdates[count].ToBytes(blockbuffer, ref length);
+                    m_avatarTerseUpdates_.Peek().ToBytes(blockbuffer, ref length);
                     length = Helpers.ZeroEncode(blockbuffer, length, zerobuffer);
                     if (size + length > Packet.MTU)
                         break;
                     size += length;
+                    updates.Enqueue(m_avatarTerseUpdates_.Dequeue());
                 }
 
                 terse.ObjectData = new ImprovedTerseObjectUpdatePacket.ObjectDataBlock[count];
 
                 for (int i = 0 ; i < count ; i++)
-                {
-                    terse.ObjectData[i] = m_avatarTerseUpdates[0];
-                    m_avatarTerseUpdates.RemoveAt(0);
-                }
+                    terse.ObjectData[i] = updates.Dequeue();
 
                 terse.Header.Reliable = false;
                 terse.Header.Zerocoded = true;
                 // FIXME: Move this to ThrottleOutPacketType.State when the real prioritization code is committed
                 OutPacket(terse, ThrottleOutPacketType.Task);
 
-                if (m_avatarTerseUpdates.Count == 0)
+                if (m_avatarTerseUpdates_.Count == 0)
                 {
                     lock (m_avatarTerseUpdateTimer)
                         m_avatarTerseUpdateTimer.Stop();
@@ -3660,14 +3663,14 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 objectData.TextureAnim = textureanim;
             }
 
-            lock (m_primFullUpdates)
+            lock (m_primFullUpdates_.SyncRoot)
             {
-                if (m_primFullUpdates.Count == 0)
+                if (m_primFullUpdates_.Count == 0)
                     m_primFullUpdateTimer.Start();
 
-                m_primFullUpdates.Add(objectData);
+                m_primFullUpdates_.Enqueue(DateTime.Now.ToOADate(), objectData, localID);
 
-                if (m_primFullUpdates.Count >= m_primFullUpdatesPerPacket)
+                if (m_primFullUpdates_.Count >= m_primFullUpdatesPerPacket)
                     ProcessPrimFullUpdates(this, null);
             }
         }
@@ -3690,9 +3693,9 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
         void ProcessPrimFullUpdates(object sender, ElapsedEventArgs e)
         {
-            lock (m_primFullUpdates)
+            lock (m_primFullUpdates_.SyncRoot)
             {
-                if (m_primFullUpdates.Count == 0 && m_primFullUpdateTimer.Enabled)
+                if (m_primFullUpdates_.Count == 0 && m_primFullUpdateTimer.Enabled)
                 {
                     lock (m_primFullUpdateTimer)
                         m_primFullUpdateTimer.Stop();
@@ -3709,7 +3712,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 outPacket.RegionData.TimeDilation =
                         (ushort)(Scene.TimeDilation * ushort.MaxValue);
 
-                int max = m_primFullUpdates.Count;
+                int max = m_primFullUpdates_.Count;
                 if (max > m_primFullUpdatesPerPacket)
                     max = m_primFullUpdatesPerPacket;
 
@@ -3719,29 +3722,29 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 byte[] zerobuffer = new byte[1024];
                 byte[] blockbuffer = new byte[1024];
 
+                Queue<ObjectUpdatePacket.ObjectDataBlock> updates = new Queue<ObjectUpdatePacket.ObjectDataBlock>();
+
                 for (count = 0 ; count < max ; count++)
                 {
                     int length = 0;
-                    m_primFullUpdates[count].ToBytes(blockbuffer, ref length);
+                    m_primFullUpdates_.Peek().ToBytes(blockbuffer, ref length);
                     length = Helpers.ZeroEncode(blockbuffer, length, zerobuffer);
                     if (size + length > Packet.MTU)
                         break;
                     size += length;
+                    updates.Enqueue(m_primFullUpdates_.Dequeue());
                 }
 
                 outPacket.ObjectData =
                         new ObjectUpdatePacket.ObjectDataBlock[count];
 
                 for (int index = 0 ; index < count ; index++)
-                {
-                    outPacket.ObjectData[index] = m_primFullUpdates[0];
-                    m_primFullUpdates.RemoveAt(0);
-                }
+                    outPacket.ObjectData[index] = updates.Dequeue();
 
                 outPacket.Header.Zerocoded = true;
                 OutPacket(outPacket, ThrottleOutPacketType.State);
 
-                if (m_primFullUpdates.Count == 0 && m_primFullUpdateTimer.Enabled)
+                if (m_primFullUpdates_.Count == 0 && m_primFullUpdateTimer.Enabled)
                     lock (m_primFullUpdateTimer)
                         m_primFullUpdateTimer.Stop();
             }
@@ -3763,23 +3766,23 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                     CreatePrimImprovedBlock(localID, position, rotation,
                     velocity, rotationalvelocity, state);
 
-            lock (m_primTerseUpdates)
+            lock (m_primTerseUpdates_.SyncRoot)
             {
-                if (m_primTerseUpdates.Count == 0)
+                if (m_primTerseUpdates_.Count == 0)
                     m_primTerseUpdateTimer.Start();
 
-                m_primTerseUpdates.Add(objectData);
+                m_primTerseUpdates_.Enqueue(DateTime.Now.ToOADate(), objectData, localID);
 
-                if (m_primTerseUpdates.Count >= m_primTerseUpdatesPerPacket)
+                if (m_primTerseUpdates_.Count >= m_primTerseUpdatesPerPacket)
                     ProcessPrimTerseUpdates(this, null);
             }
         }
 
         void ProcessPrimTerseUpdates(object sender, ElapsedEventArgs e)
         {
-            lock (m_primTerseUpdates)
+            lock (m_primTerseUpdates_.SyncRoot)
             {
-                if (m_primTerseUpdates.Count == 0)
+                if (m_primTerseUpdates_.Count == 0)
                 {
                     lock (m_primTerseUpdateTimer)
                         m_primTerseUpdateTimer.Stop();
@@ -3797,7 +3800,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 outPacket.RegionData.TimeDilation =
                         (ushort)(Scene.TimeDilation * ushort.MaxValue);
 
-                int max = m_primTerseUpdates.Count;
+                int max = m_primTerseUpdates_.Count;
                 if (max > m_primTerseUpdatesPerPacket)
                     max = m_primTerseUpdatesPerPacket;
 
@@ -3807,14 +3810,17 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 byte[] zerobuffer = new byte[1024];
                 byte[] blockbuffer = new byte[1024];
 
+                Queue<ImprovedTerseObjectUpdatePacket.ObjectDataBlock> updates = new Queue<ImprovedTerseObjectUpdatePacket.ObjectDataBlock>();
+
                 for (count = 0 ; count < max ; count++)
                 {
                     int length = 0;
-                    m_primTerseUpdates[count].ToBytes(blockbuffer, ref length);
+                    m_primTerseUpdates_.Peek().ToBytes(blockbuffer, ref length);
                     length = Helpers.ZeroEncode(blockbuffer, length, zerobuffer);
                     if (size + length > Packet.MTU)
                         break;
                     size += length;
+                    updates.Enqueue(m_primTerseUpdates_.Dequeue());
                 }
 
                 outPacket.ObjectData =
@@ -3822,16 +3828,13 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                         ObjectDataBlock[count];
 
                 for (int index = 0 ; index < count ; index++)
-                {
-                    outPacket.ObjectData[index] = m_primTerseUpdates[0];
-                    m_primTerseUpdates.RemoveAt(0);
-                }
+                    outPacket.ObjectData[index] = updates.Dequeue();
 
                 outPacket.Header.Reliable = false;
                 outPacket.Header.Zerocoded = true;
                 OutPacket(outPacket, ThrottleOutPacketType.State);
 
-                if (m_primTerseUpdates.Count == 0)
+                if (m_primTerseUpdates_.Count == 0)
                     lock (m_primTerseUpdateTimer)
                         m_primTerseUpdateTimer.Stop();
             }
@@ -3839,15 +3842,15 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
         public void FlushPrimUpdates()
         {
-            while (m_primFullUpdates.Count > 0)
+            while (m_primFullUpdates_.Count > 0)
             {
                 ProcessPrimFullUpdates(this, null);
             }
-            while (m_primTerseUpdates.Count > 0)
+            while (m_primTerseUpdates_.Count > 0)
             {
                 ProcessPrimTerseUpdates(this, null);
             }
-            while (m_avatarTerseUpdates.Count > 0)
+            while (m_avatarTerseUpdates_.Count > 0)
             {
                 ProcessAvatarTerseUpdates(this, null);
             }
@@ -10578,5 +10581,136 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             pack.TextureData.TextureID = textureID;
             OutPacket(pack, ThrottleOutPacketType.Task);
         }
+
+        #region PriorityQueue
+        private class PriorityQueue<TPriority, TValue>
+        {
+            private MinHeap<MinHeapItem>[] heaps = new MinHeap<MinHeapItem>[1];
+            private Dictionary<uint, LookupItem> lookup_table = new Dictionary<uint, LookupItem>();
+            private Comparison<TPriority> comparison;
+            private object sync_root = new object();
+
+            internal PriorityQueue() :
+                this(MinHeap<MinHeapItem>.DEFAULT_CAPACITY, Comparer<TPriority>.Default) { }
+            internal PriorityQueue(int capacity) :
+                this(capacity, Comparer<TPriority>.Default) { }
+            internal PriorityQueue(IComparer<TPriority> comparer) :
+                this(new Comparison<TPriority>(comparer.Compare)) { }
+            internal PriorityQueue(Comparison<TPriority> comparison) :
+                this(MinHeap<MinHeapItem>.DEFAULT_CAPACITY, comparison) { }
+            internal PriorityQueue(int capacity, IComparer<TPriority> comparer) :
+                this(capacity, new Comparison<TPriority>(comparer.Compare)) { }
+            internal PriorityQueue(int capacity, Comparison<TPriority> comparison)
+            {
+                for (int i = 0; i < heaps.Length; ++i)
+                    heaps[i] = new MinHeap<MinHeapItem>(capacity);
+                this.comparison = comparison;
+            }
+
+            internal object SyncRoot { get { return this.sync_root; } }
+            internal int Count
+            {
+                get
+                {
+                    int count = 0;
+                    for (int i = 0; i < heaps.Length; ++i)
+                        count = heaps[i].Count;
+                    return count;
+                }
+            }
+
+            internal bool Enqueue(TPriority priority, TValue value, uint local_id)
+            {
+                LookupItem item;
+
+                if (lookup_table.TryGetValue(local_id, out item))
+                {
+                    item.Heap[item.Handle] = new MinHeapItem(priority, value, local_id, this.comparison);
+                    return false;
+                }
+                else
+                {
+                    item.Heap = heaps[0];
+                    item.Heap.Add(new MinHeapItem(priority, value, local_id, this.comparison), ref item.Handle);
+                    lookup_table.Add(local_id, item);
+                    return true;
+                }
+            }
+
+            internal TValue Peek()
+            {
+                for (int i = 0; i < heaps.Length; ++i)
+                    if (heaps[i].Count > 0)
+                        return heaps[i].Min().Value;
+                throw new InvalidOperationException(string.Format("The {0} is empty", this.GetType().ToString()));
+            }
+
+            internal TValue Dequeue()
+            {
+                for (int i = 0; i < heaps.Length; ++i)
+                {
+                    if (heaps[i].Count > 0)
+                    {
+                        MinHeapItem item = heaps[i].RemoveMin();
+                        lookup_table.Remove(item.LocalID);
+                        return item.Value;
+                    }
+                }
+                throw new InvalidOperationException(string.Format("The {0} is empty", this.GetType().ToString()));
+            }
+
+            #region MinHeapItem
+            private struct MinHeapItem : IComparable<MinHeapItem>
+            {
+                private TPriority priority;
+                private TValue value;
+                private uint local_id;
+                private Comparison<TPriority> comparison;
+
+                internal MinHeapItem(TPriority priority, TValue value, uint local_id) :
+                    this(priority, value, local_id, Comparer<TPriority>.Default) { }
+                internal MinHeapItem(TPriority priority, TValue value, uint local_id, IComparer<TPriority> comparer) :
+                    this(priority, value, local_id, new Comparison<TPriority>(comparer.Compare)) { }
+                internal MinHeapItem(TPriority priority, TValue value, uint local_id, Comparison<TPriority> comparison)
+                {
+                    this.priority = priority;
+                    this.value = value;
+                    this.local_id = local_id;
+                    this.comparison = comparison;
+                }
+
+                internal TPriority Priority { get { return this.priority; } }
+                internal TValue Value { get { return this.value; } }
+                internal uint LocalID { get { return this.local_id; } }
+
+                public override string ToString()
+                {
+                    StringBuilder sb = new StringBuilder();
+                    sb.Append("[");
+                    if (this.priority != null)
+                        sb.Append(this.priority.ToString());
+                    sb.Append(",");
+                    if (this.value != null)
+                        sb.Append(this.value.ToString());
+                    sb.Append("]");
+                    return sb.ToString();
+                }
+
+                public int CompareTo(MinHeapItem other)
+                {
+                    return this.comparison(this.priority, other.priority);
+                }
+            }
+            #endregion
+
+            #region LookupItem
+            private struct LookupItem {
+                internal MinHeap<MinHeapItem> Heap;
+                internal IHandle Handle;
+            }
+            #endregion
+        }
+        #endregion
+
     }
 }