// Modified from @taylorza on GHI Electronics Forum
// https://www.ghielectronics.com/community/forum/topic?id=8083&page=2#msg79627


using System;
using System.Text;
using Microsoft.SPOT;

namespace CPF
{
    public class ByteQueue
    {
        private static byte[][] buffer;
        private static byte[] returnArray;
        private static int head;
        private static int tail;
        private static int count;
        private static int capacity;
        private static int i;

        private const int defaultCapacity = 512;
        private const int defaultMessageLength = 256;

        public ByteQueue()
        {
            buffer = new byte[defaultCapacity][];
            for (i = 0; i < defaultCapacity; i++)
            {
                buffer[i] = new byte[defaultMessageLength];
            }

            returnArray = new byte[defaultMessageLength];

            capacity = defaultCapacity;
            head = 0;
            tail = 0;
            count = 0;
        }

        public int Count
        {
            get { return count; }
        }

        public void Clear()
        {
            count = 0;
            tail = head;
        }

        public void Enqueue(byte[] byteArray)
        {
            //GM I've taken out the ability to grow since if the queue does have to grow, something is wrong
            //if (count == capacity)
            //{
            //  Grow();
            //}
            //buffer[head] = value;
            if (count >= capacity)
            {
                throw new InvalidOperationException("Queue is full");
            }
            else
            {
                Array.Copy(byteArray, buffer[head], byteArray.Length);
                head = (head + 1) % capacity;
                count++;
            }
        }

        public byte[] Dequeue()
        {
            Array.Clear(returnArray, 0, returnArray.Length);
            if (count == 0)
            {
                throw new InvalidOperationException("Queue is empty");
            }
            else
            {
                //      byte value = buffer[tail];
                Array.Copy(buffer[tail], returnArray, buffer[tail].Length);

                tail = (tail + 1) % capacity;
                count--;
                return returnArray;
            }
        }

        //public int Dequeue(byte[] bytes)
        //{
        //  int maxBytes = System.Math.Min(count, bytes.Length);
        //  for (i = 0; i < maxBytes; i++)
        //  {
        //    bytes[i] = Dequeue();
        //  }
        //  return maxBytes;
        //}

        //private void Grow()
        //{
        //  int newCapacity = capacity << 1;
        //  byte[] newBuffer = new byte[newCapacity];

        //  if (tail < head)
        //  {
        //    Array.Copy(buffer, tail, newBuffer, 0, count);
        //  }
        //  else
        //  {
        //    Array.Copy(buffer, tail, newBuffer, 0, capacity - tail);
        //    Array.Copy(buffer, 0, newBuffer, capacity - tail, head);
        //  }

        //  buffer = newBuffer;
        //  head = count;
        //  tail = 0;
        //  capacity = newCapacity;      
        //}
    }
}