using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class PriorityQueue<T>
{
    private Heap<T> heap;
	
	public int Count { get { return heap.Count; } }

    public PriorityQueue(System.Comparison<T> comparison)
    {
		heap = new Heap<T>(comparison);
    }

    public void Enqueue(T item)
    {
		heap.Insert(item);
    }

    public T Dequeue()
    {
        return heap.Pop();
    }

	public bool Contains(T item)
	{
		return heap.Contains(item);
	}

	public void ValueUpdated(T item)
	{
		heap.HeapUpdated(item);
	}

	public IEnumerator<T> GetEnumerator()
	{
		return heap.GetEnumerator();
	}
}

class Heap<T>
{
	private List<T> list = new List<T>();
	private System.Comparison<T> comparison;

	public int Count { get { return list.Count; } }

	public Heap(System.Comparison<T> comparison)
	{
		this.comparison = comparison;
	}

	public bool Contains(T item)
	{
		return list.Contains(item);
	}

	public void Insert(T item)
	{
		list.Add(item);
		TryWithParent(list.Count - 1);
		/*
		int index = list.Count - 1;
		while (comparison.Invoke(list[index], list[Parent(index)]) > 0)
		{
			T tmp = list[index];
			list[index] = list[Parent(index)];
			list[Parent(index)] = tmp;
			index = Parent(index);
		}
		*/
	}

	public T Pop()
	{
		T tmp = list[0];
		list[0] = list[list.Count - 1];
		list.RemoveAt(list.Count - 1);
		Heapify();
		return tmp;
	}

	private void Heapify()
	{
		TryWithChildren(0);
		/*
		int index = 0;
		while (true)
		{
			int left = Left(index);
			int right = Right(index);

			int target = index;
			if (left < list.Count && comparison.Invoke(list[left], list[index]) > 0)
			{
				target = left;
			}
			if (right < list.Count && comparison.Invoke(list[right], list[index]) > 0)
			{
				if (comparison.Invoke(list[left], list[right]) < 0)
				{
					target = right;
				}
			}
			if (index != target)
			{
				T tmp = list[index];
				list[index] = list[target];
				list[target] = tmp;
				index = target;
			}
			else
			{
				break;
			}
		}
		*/
	}

	public bool isHeap()
	{
		for(int i = 0; i < list.Count / 2; i++)
		{
			int left = 2 * i + 1;
			int right = 2 * i + 2;
			if (comparison.Invoke(list[i], list[left]) < 0)
			{
				return false;
			}
			if (right < list.Count && comparison.Invoke(list[i], list[right]) < 0)
			{
				return false;
			}
		}
		return true;
	}

	public void HeapUpdated(T item)
	{
		TryWithParent(list.IndexOf(item));
		TryWithChildren(list.IndexOf(item));

		/*
		if (!list.Contains(item))
			Debug.LogError("Invalid item");
		int index = list.IndexOf(item);
		int left = 2 * index + 1;
		int right = 2 * index + 2;
		if (comparison.Invoke(list[index], list[(index - 1) / 2]) > 0)
		{
			while(comparison.Invoke(list[index], list[(index -1) / 2]) > 0)
			{
				T tmp = list[index];
				list[index] = list[(index - 1) / 2];
				list[(index - 1) / 2] = tmp;
				index /= 2;
			}
		}
		else if (left < list.Count && comparison.Invoke(list[index], list[left]) < 0)
		{
			T tmp = list[index];
			list[index] = list[left];
			list[left] = tmp;
		}
		else if (right < list.Count && comparison.Invoke(list[index], list[right]) < 0)
		{
			T tmp = list[index];
			list[index] = list[right];
			list[right] = tmp;
		}
		*/
	}

	private int Left(int index)
	{
		return index * 2 + 1;
	}

	private int Right(int index)
	{
		return index * 2 + 2;
	}

	private int Parent(int index)
	{
		return (index - 1) / 2;
	}

	private void TryWithChildren(int index)
	{
		
		if (Left(index) >= list.Count)
		{
			return;
		}
		int biggerChild;
		if (Right(index) >= list.Count)
		{
			biggerChild = Left(index);
		}
		else
		{
			biggerChild = comparison.Invoke(list[Left(index)], list[Right(index)]) >= 0 ? Left(index) : Right(index);
		}

		if (comparison.Invoke(list[biggerChild], list[index]) > 0)
		{
			T tmp = list[index];
			list[index] = list[biggerChild];
			list[biggerChild] = tmp;
			TryWithChildren(biggerChild);
		}

	}

	private void TryWithParent(int index)
	{
		if(comparison.Invoke(list[index], list[Parent(index)]) > 0)
		{
			T tmp = list[index];
			list[index] = list[Parent(index)];
			list[Parent(index)] = tmp;
			TryWithParent(Parent(index));
		}
	}

	public IEnumerator<T> GetEnumerator()
	{
		return list.GetEnumerator();
	}
}