using System;

namespace SuperTest
{
    internal class ConcurrentQueue<T>
    {
        private const long Size = 4096;
        private const long MaxSpinOnBusy = 40000000;
        private static readonly long MRingModMask = ClosestExponentOf2() - 1;
        private static readonly long MSize = ClosestExponentOf2();

        private static readonly T MEmpty = default(T);

        private readonly object _mLock = new object();

        private readonly T[] _mMem = new T[MSize];
        private long _mReadPtr;
        private long _mWritePtr;

        private static long Log2(int n, int p = 0)
        {
            return n <= 1 ? p : Log2(n / 2, p + 1);
        }

        private static int ClosestExponentOf2()
        {
            return (int) 1UL << (int) (Log2((int) (Size - 1)) + 1);
        }

        public T Pop()
        {
            if (!Peek())
                return MEmpty;

            lock (_mLock)
            {
                if (!Peek())
                    return MEmpty;

                var ret = _mMem[_mReadPtr & MRingModMask];
                _mReadPtr++;
                return ret;
            }
        }

        public bool Peek()
        {
            return _mWritePtr != _mReadPtr;
        }

        private long GetCount()
        {
            return _mWritePtr > _mReadPtr ? _mWritePtr - _mReadPtr : _mReadPtr - _mWritePtr;
        }

        private bool BusyWaitForPush()
        {
            long start = 0;
            while (GetCount() == MSize)
                if (start++ > MaxSpinOnBusy)
                    return false;
            return true;
        }

        public void Push(T pItem)
        {
            if (!BusyWaitForPush())
                throw new InvalidOperationException("Concurrent queue full cannot write to it!");

            lock (_mLock)
            {
                _mMem[_mWritePtr & MRingModMask] = pItem;
                _mWritePtr++;
            }
        }
    }
}