/** Класс представляющий очередь с приоритетом. */
template <class T, class K>
class PriorityQueue {
public:
    /** Инициализируем отношение порядка на ключах. */
    PriorityQueue(function<int(const T, const T)> cmp) {
        compare = cmp;
    }

    /** Проверка очереди на пустоту **/
    bool isEmpty() {
        return heap.empty();
    }

    /** Операция добавления элемента в очередь в соотв. с приорететом **/
    void insert(T element) {
        // Элемент ставится в конец кучи
        // и поднимается обменами к нужной позиции.
        heap.push_back(element);
        int i = (int)heap.size() - 1;
        while (i > 0 && compare(heap[(i - 1) / 2], heap[i]) < 0) {
            swap(heap[(i - 1) / 2], heap[i]);
            heap[i]->setIndex(i);
            i = (i - 1) / 2;
        }

        heap[i]->setIndex(i);
    }

    /** Операция изменения ключа у элемента очереди ptr. */
    void changeKey(T ptr, K key) {
        ptr->setKey(key);
        int i = ptr->getIndex();

        while(i > 0 && compare(heap[(i - 1) / 2], heap[i]) < 0) {
            swap(heap[(i - 1) / 2], heap[i]);
            heap[i]->setIndex(i);
            i = (i - 1) / 2;
        }

        ptr->setIndex(i);
    }

    /** Снимает с дерева элемент с наибольшим приоритетом,
      * восстанавливает порядок элементов в очереди **/
    T extractRoot() {
        if (heap.empty()) {
            __throw_underflow_error("Error: PriorityQueue is empty!");
        }

        T result = heap[0];
        swap(heap[0], heap[heap.size() - 1]);
        heap.pop_back();

        if (!heap.empty()) {
            heap[0]->setIndex(0);
            returnHeapOrder();
        }

        return result;
    }

private:
    function<int(const T, const T)> compare;  // Функция реализующая отношение строго порядка
    vector<T>heap;                            // Куча для хранения данных

    /** Служебная функция: восстанавливает кучу после удаления корня.
      *  Элемент опускается обменами на нужную позицию в соотв. с приоритетом **/
    void returnHeapOrder()
    {
        size_t index = 0, child_1, child_2, swap_child;
        while(true)
        {
            // Находим дочерние вершины
            child_1 = 2 * index + 1;
            child_2 = 2 * index + 2;

            if (child_1 >= heap.size())
                child_1 = index;
            if (child_2 >= heap.size())
                child_2 = index;

            // Сравниваем с текущей
            int cmp_1 = compare(heap[index], heap[child_1]);
            int cmp_2 = compare(heap[index], heap[child_2]);
            if ((cmp_1 == 0 || cmp_1 > 0)  && (cmp_2 == 0 || cmp_2 > 0))
                break; // Элемент на нужном месте

            // Производим необходимый обмен
            if (compare (heap[child_1],  heap[child_2]) > 0)
                swap_child = child_1;
            else
                swap_child = child_2;

            swap(heap[index], heap[swap_child]);
            heap[index]->setIndex(index);
            heap[swap_child]->setIndex(swap_child);
            index = swap_child;
        }
    }
};