PriorityQueue

namespace PriorityQueueAdt
{
    using System;
    using System.Collections;
    using System.Collections.Generic;

    public class PriorityQueue<T> : IEnumerable<T>
        where T : IComparable<T>
    {
        private const int DefaultCapacity = 16;

        private T[] elements;

        public PriorityQueue()
        {
            this.elements = new T[DefaultCapacity];
        }

        public int Capacity
        {
            get
            {
                return this.elements.Length;
            }
        }

        public int Count { get; private set; }

        public T Dequeue()
        {
            T value = this.Peek();

            this.Count--;

            if (this.Count > 0)
            {
                this.BalanceThreeOnDequeue();
            }

            return value;
        }

        public void Enqueue(T item)
        {
            if (this.Count + 1 > this.elements.Length - 1)
            {
                this.Resize();
            }

            this.elements[this.Count + 1] = item;

            this.BalanceThreeOnEnqueue(this.Count + 1);

            this.Count++;
        }

        public IEnumerator<T> GetEnumerator()
        {
            for (int i = 1; i <= this.Count; i++)
            {
                yield return this.elements[i];
            }
        }

        public T Peek()
        {
            if (this.Count == 0)
            {
                throw new InvalidOperationException("Queue empty.");
            }

            return this.elements[1];
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }

        private void BalanceThreeOnDequeue()
        {
            int index = 1;

            this.elements[index] = this.elements[this.Count + 1];

            while (true)
            {
                int left = index * 2;
                int right = left + 1;

                if (left >= this.Count)
                {
                    break;
                }

                if (right >= this.Count)
                {
                    this.Swap(index, left);
                    break;
                }

                T leftElement = this.elements[left];
                T rightElement = this.elements[right];

                if (leftElement.CompareTo(rightElement) == 1)
                {
                    index = this.Swap(index, left);
                }
                else if (leftElement.CompareTo(rightElement) == -1)
                {
                    index = this.Swap(index, right);
                }
                else
                {
                    break;
                }
            }
        }

        private void BalanceThreeOnEnqueue(int index)
        {
            while (index > 1)
            {
                T element = this.elements[index];

                int parentIndex = index / 2;
                T parentElement = this.elements[parentIndex];

                if (element.CompareTo(parentElement) == 1)
                {
                    index = this.Swap(index, parentIndex);
                }
                else
                {
                    break;
                }
            }
        }

        private void Resize()
        {
            T[] newElements = new T[this.elements.Length * 2];
            this.elements.CopyTo(newElements, 0);
            this.elements = newElements;
        }

        private int Swap(int index1, int index2)
        {
            T first = this.elements[index1];
            this.elements[index1] = this.elements[index2];
            this.elements[index2] = first;

            return index2;
        }
    }
}