/*
we depend on the discipline List<T> have (or not) arround allocating arrays
if you are dealing with known length then modify to have a cap parameter and pre allocate your List<T>
*/
using System;
using System.Threading;
using System.Collections.Generic;

using System.Runtime.CompilerServices;

namespace Utils
{
    public delegate bool Comparer<T>(T lh, T rh);

    public class PriorityQueue<T> where T : class
    {
        private Comparer<T> comparer;
        private ReaderWriterLockSlim rwLock = new ReaderWriterLockSlim();
        private List<T> list = new List<T>();

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private int ParentIdxOf(int idx) { return (idx - 1) / 2; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private int LeftChildIdxOf(int idx) { return (2 * idx + 1); }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private int RightChildIdxOf(int idx) { return (2 * idx + 2); }

        // must be called with lock held
        private void ensureHeap(int atIdx)
        {
            var leftChildIdx = LeftChildIdxOf(atIdx);
            var rightChildIdx = RightChildIdxOf(atIdx);
            var selectedIdx = atIdx;
            var listSize = list.Count;

            if (leftChildIdx < listSize && comparer(list[atIdx], list[leftChildIdx]))
                selectedIdx = leftChildIdx;

            if (rightChildIdx < listSize && comparer(list[atIdx], list[rightChildIdx]))
                selectedIdx = rightChildIdx;


            if (selectedIdx != atIdx)
            {
                var temp = list[selectedIdx];
                list[selectedIdx] = list[atIdx];
                list[atIdx] = temp;
                ensureHeap(selectedIdx);
            }
        }

        private PriorityQueue() { }

        public int Count
        {
            get
            {
                rwLock.EnterReadLock();
                var cnt = list.Count;
                rwLock.ExitReadLock();
                return cnt;
            }
        }



        public PriorityQueue(Comparer<T> comparer)
        {
            if (comparer == null)
                throw new ArgumentNullException();

            this.comparer = comparer;
        }

        public T Peek()
        {
            T item = null;
            rwLock.EnterReadLock();

            if (list.Count > 0)
                item = list[0];

            this.rwLock.ExitReadLock();

            return item;
        }

        public void Enqueue(T item)
        {
            if (item == null)
                throw new ArgumentNullException();

            rwLock.EnterWriteLock();
            list.Add(item);
            var at = list.Count - 1;
            while (at != 0 && comparer(list[ParentIdxOf(at)], list[at]))
            {
                var temp = list[ParentIdxOf(at)];
                list[ParentIdxOf(at)] = list[at];
                list[at] = temp;
                at = ParentIdxOf(at);
            }

            rwLock.ExitWriteLock();
        }

        public T Dequeue()
        {
            T item = null;
            rwLock.EnterWriteLock();

            if (list.Count > 0)
            {
                item = list[0];
                list[0] = list[this.list.Count - 1];
                list.RemoveAt(list.Count - 1);
                ensureHeap(0);
            }

            rwLock.ExitWriteLock();
            return item;
        }
    }
}