using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;

/// <summary>
/// Completely untested heap
/// </summary>
/// <typeparam name="T"></typeparam>
public class MinHeap<T> where T : System.IComparable<T>
{
    private int m_capacity;
    private int m_size;
    private T[] m_data;

    public int Count => m_size;

    public MinHeap(int capacity = 16)
    {
        m_capacity = capacity;
        m_size = 0;
        m_data = new T[capacity];
    }

    public T Peek()
    {
        if (m_size == 0) throw new System.IndexOutOfRangeException();
        return m_data[0];
    }

    public T Poll()
    {
        if (m_size == 0) throw new System.IndexOutOfRangeException();
        T item = m_data[0];
        m_size--;
        m_data[0] = m_data[m_size];
        HeapifyDown();
        return item;
    }

    public void Add(T t)
    {
        if(m_size == m_capacity)
        {
            Resize(m_capacity * 2);
        }

        m_data[m_size] = t;
        m_size++;
        HeapifyUp();
    }

    public bool Contains(T t)
    {
        return m_data.Contains(t);
    }

    private int GetLeftChildIndex(int parent) { return 2 * parent + 1; }
    private int GetRightChildIndex(int parent) { return 2 * parent + 2; }
    private int GetParentIndex(int child) { return (child - 1) / 2; }

    private bool HasLeftChild(int index) { return GetLeftChildIndex(index) < m_size; }
    private bool HasRightChild(int index) { return GetRightChildIndex(index) < m_size; }
    private bool HasParent(int index) { return GetParentIndex(index) >= 0; }

    private T LeftChild(int index) { return m_data[GetLeftChildIndex(index)]; }
    private T RightChild(int index) { return m_data[GetRightChildIndex(index)]; }
    private T Parent(int index) { return m_data[GetParentIndex(index)]; }

    private void Swap(int first, int second)
    {
        T temp = m_data[first];
        m_data[first] = m_data[second];
        m_data[second] = temp;
    }

    private void Resize(int newSize)
    {
        if(newSize > m_capacity)
        {
            Array.Resize(ref m_data, newSize);
            m_capacity = newSize;
        }
    }

    private void HeapifyDown()
    {
        int index = 0;
        while (HasLeftChild(index))
        {
            int childIndex = GetLeftChildIndex(index);
            if (HasRightChild(index) && RightChild(index).CompareTo(LeftChild(index)) < 0)
            {
                childIndex = GetRightChildIndex(index);
            }

            if(m_data[index].CompareTo(m_data[childIndex]) < 0)
            {
                break;
            }
            else
            {
                Swap(index, childIndex);
            }
            index = childIndex;
        }
    }

    private void HeapifyUp()
    {
        int index = m_size - 1;

        while(HasParent(index) && Parent(index).CompareTo(m_data[index]) < 0)
        {
            Swap(GetParentIndex(index), index);
            index = GetParentIndex(index);
        }
    }
}
