/*
	Scan Tailor - Interactive post-processing tool for scanned pages.
	Copyright (C) 2015  Joseph Artsimovich <joseph.artsimovich@gmail.com>

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef PRIORITY_QUEUE_H_
#define PRIORITY_QUEUE_H_

#include "foundation_config.h"
#include <vector>
#include <algorithm>
#include <stddef.h>
#include <assert.h>

/**
 * \brief A priority queue implemented as a binary heap.
 *
 * \tparam T The type of objects to be stored in the priority queue.
 * \param SubClass A sub class of this class that will be implementing the following:
 *        \li void setIndex(T& obj, size_t heap_idx);
 *        \li bool higherThan(T const& lhs, T const& rhs) const;
 *
 * Also note that this implementation will benefit from a standalone
 * \code
 * void swap(T& o1, T& o2);
 * \endcode
 * function in the same namespace as T.
 */
template<typename T, typename SubClass>
class PriorityQueue
{
	// Member-wise copying is OK.
public:
	PriorityQueue() {}

	void reserve(size_t capacity) { m_index.reserve(capacity); }

	bool empty() const { return m_index.empty(); }

	size_t size() const { return m_index.size(); }

	/**
	 * \brief Provides access to the head of priority queue.
	 *
	 * Modification of an object is allowed, provided your modifications don't
	 * affect the logical order of objects, or you will be calling reposition(),
	 * pop() or erase() on the modified object before any other operation that
	 * involves comparing objects.
	 */
	T& front() { return m_index.front(); }

	T const& front() const { return m_index.front(); }

	void push(T const& obj);

	/**
	 * Like push(), but implemented through swapping \p obj with a default
	 * constructed instance of T. This will make sence if copying a default
	 * constructed instance of T is much cheaper than copying \p obj.
	 */
	void pushDestructive(T& obj);

	void pop();

	/**
	 * Retrieve-and-pop, implemented through swapping \p obj with the instance
	 * at the front of the queue. There are no special requirements to
	 * the state of the object being passed to this function.
	 */
	void retrieveFront(T& obj);

	void swapWith(PriorityQueue& other) { m_index.swap(other.m_index); }
protected:
	void erase(size_t idx);

	void reposition(size_t idx);
private:
	static size_t parent(size_t idx) { return (idx - 1) / 2; }

	static size_t left(size_t idx) { return idx * 2 + 1; }

	static size_t right(size_t idx) { return idx * 2 + 2; }

	SubClass* subClass() { return static_cast<SubClass*>(this); }

	SubClass const* subClass() const { return static_cast<SubClass const*>(this); }

	size_t bubbleUp(size_t idx);

	size_t bubbleDown(size_t idx);

	std::vector<T> m_index;
};


template<typename T, typename SubClass>
inline void swap(PriorityQueue<T, SubClass>& o1, PriorityQueue<T, SubClass>& o2)
{
	o1.swap(o2);
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::push(T const& obj)
{
	size_t const idx = m_index.size();
	m_index.push_back(obj);
	subClass()->setIndex(m_index.back(), idx);
	bubbleUp(idx);
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::pushDestructive(T& obj)
{
	using namespace std;

	size_t const idx = m_index.size();
	m_index.push_back(T());
	swap(m_index.back(), obj);
	subClass()->setIndex(m_index.back(), idx);
	bubbleUp(idx);
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::pop()
{
	using namespace std;

	assert(!empty());

	swap(m_index.front(), m_index.back());
	subClass()->setIndex(m_index.front(), 0);

	m_index.pop_back();
	if (!empty()) {
		bubbleDown(0);
	}
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::retrieveFront(T& obj)
{
	using namespace std;

	assert(!empty());

	swap(m_index.front(), obj);
	swap(m_index.front(), m_index.back());
	subClass()->setIndex(m_index.front(), 0);

	m_index.pop_back();
	if (!empty()) {
		bubbleDown(0);
	}
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::erase(size_t const idx)
{
	using namespace std;

	swap(m_index[idx], m_index.back());
	subClass()->setIndex(m_index[idx], idx);

	m_index.pop_back();
	reposition(m_index[idx]);
}

template<typename T, typename SubClass>
void
PriorityQueue<T, SubClass>::reposition(size_t const idx)
{
	bubbleUp(bubbleDown(idx));
}

template<typename T, typename SubClass>
size_t
PriorityQueue<T, SubClass>::bubbleUp(size_t idx)
{
	using namespace std;

	// Iteratively swap the element with its parent,
	// if it's greater than the parent.

	assert(idx < m_index.size());

	while (idx > 0) {
		size_t const parent_idx = parent(idx);
		if (!subClass()->higherThan(m_index[idx], m_index[parent_idx])) {
			break;
		}
		swap(m_index[idx], m_index[parent_idx]);
		subClass()->setIndex(m_index[idx], idx);
		subClass()->setIndex(m_index[parent_idx], parent_idx);
		idx = parent_idx;
	}

	return idx;
}

template<typename T, typename SubClass>
size_t
PriorityQueue<T, SubClass>::bubbleDown(size_t idx)
{
	using namespace std;

	size_t const len = m_index.size();
	assert(idx < len);

	// While any child is greater than the element itself,
	// swap it with the greatest child.

	for (;;) {
		size_t const lft = left(idx);
		size_t const rgt = right(idx);
		size_t best_child;

		if(rgt < len) {
			best_child = subClass()->higherThan(m_index[lft], m_index[rgt]) ? lft : rgt;
		} else if (lft < len) {
			best_child = lft;
		} else {
			break;
		}
        
		if (subClass()->higherThan(m_index[best_child], m_index[idx])) {
			swap(m_index[idx], m_index[best_child]);
			subClass()->setIndex(m_index[idx], idx);
			subClass()->setIndex(m_index[best_child], best_child);
            idx = best_child;
		} else {
			break;
		}
    }

	return idx;
}

#endif
