#ifndef FLEXIBLEHEAP_H
#define FLEXIBLEHEAP_H

#include <vector>

using namespace std;

namespace GtpVisibilityPreprocessor {

const static int NOT_IN_HEAP = -100;

/** This class implements an element for a heap.
	Classes using the heap should be inherited from this class.
*/
class Heapable
{
public:
	Heapable(): mPriority(0) { NotInHeap(); SetPriority(0.0f); }

	inline bool IsInHeap() { return mPosition != NOT_IN_HEAP; }
	inline void NotInHeap() { mPosition = NOT_IN_HEAP; }
	inline int GetHeapPos() { return mPosition; }
	inline void SetHeapPos(const int t) { mPosition = t; }

	inline void  SetPriority(const float k) { mPriority = k; }
	//inline float GetPriority() const  { return mPriority; }
	virtual float GetPriority() const = 0;
protected:

	float mPriority;
	int mPosition;
};


/** This class implements a flexible heap for
use as a priority queue.

Unlike the stl priority_queue, the class allows access to all 
elements. It implements efficient insertion routines and remove routines
of arbitrary elements.
*/
template<typename T>
class FlexibleHeap
{
public:
	FlexibleHeap() { }
	FlexibleHeap(const unsigned int n): mBuffer(n) { }

	void Push(T t) { Push(t, t->GetPriority()); }
	void Push(T t, const float priority);
	bool Update(T t) { return Update(t, t->GetPriority()); }
	bool Update(T t, const float priority);

	unsigned int Size() const { return (unsigned int)mBuffer.size(); }
	bool Empty() const {return mBuffer.empty(); }
	T Item(const unsigned int i) { return mBuffer[i]; }
	const T Item(const unsigned int i) const { return mBuffer[i]; }
	T Pop();
	T Top() const { return (mBuffer.empty() ? (T)NULL : mBuffer.front()); }
	/** Erases the element from the heap.
	*/
	T Erase(T);

protected:

	void Place(T x, const unsigned int i);
	void Swap(const unsigned int i, const unsigned int j);

	unsigned int Parent(const unsigned int i) const { return (i - 1) / 2; }
	unsigned int Left(const unsigned int i) const { return 2 * i + 1; }
	unsigned int Right(const unsigned int i) const { return 2 * i + 2; }

	void UpHeap(const unsigned int i);
	void DownHeap(const unsigned int i);

	std::vector<T> mBuffer;
};


/*****************************************************************************/
/*                         MyHeap implementation                             *
/*****************************************************************************/

template <typename T>
inline void FlexibleHeap<T>::Place(T x, const unsigned int i)
{
	mBuffer[i] = x;
	x->SetHeapPos(i);
}


template <typename T>
void FlexibleHeap<T>::Swap(const unsigned int i, const unsigned int j)
{
	T tmp = mBuffer[i];

	Place(mBuffer[j], i);
	Place(tmp, j);
}


template <typename T>
void FlexibleHeap<T>::UpHeap(const unsigned int i)
{
	T moving = mBuffer[i];
	unsigned int index = i;
	unsigned int p = Parent(i);

	while ((index > 0) && (moving->GetPriority() > mBuffer[p]->GetPriority()))
	{
		Place(mBuffer[p], index);
		index = p;
		p = Parent(p);
	}

	if (index != i)
	{
		Place(moving, index);
	}
}

template <typename T>
void FlexibleHeap<T>::DownHeap(const unsigned int i)
{
	T moving = mBuffer[i];

	unsigned int index = i;
	unsigned int l = Left(i);
	unsigned int r = Right(i);
	unsigned int largest;

	while (l < (int)mBuffer.size())
	{
		if ((r < (unsigned int)mBuffer.size()) && (mBuffer[l]->GetPriority() < mBuffer[r]->GetPriority()))
			largest = r;
		else 
			largest = l;

		if (moving->GetPriority() < mBuffer[largest]->GetPriority())
		{
			Place(mBuffer[largest], index);
			index = largest;
			l = Left(index);
			r = Right(index);
		}
		else
		{
			break;
		}
	}

	if (index != i)
	{
		Place(moving, index);
	}
}


template <typename T>
void FlexibleHeap<T>::Push(T t, const float v)
{
	t->SetPriority(v);

	unsigned int i = (unsigned int)mBuffer.size();

	t->SetHeapPos(i);
	mBuffer.push_back(t);

	UpHeap(i);
}

template <typename T>
bool FlexibleHeap<T>::Update(T t, const float v)
{
	if (t->IsInHeap())
		return false;

	t->SetPriority(v);

	const unsigned int i = t->GetHeapPos();

	if ((i > 0) && (v > mBuffer[Parent(i)]->GetPriority()))
		UpHeap(i);
	else
		DownHeap(i);

	return true;
}

template <typename T>
T FlexibleHeap<T>::Pop()
{
	if (mBuffer.empty()) 
		return NULL;

	Swap(0, (int)mBuffer.size() - 1);

	T dead = mBuffer.back();
	mBuffer.pop_back();

	DownHeap(0);
	dead->NotInHeap();

	return dead;
}


template <typename T>
T FlexibleHeap<T>::Erase(T t)
{
	if (!t->IsInHeap()) 
		return (T)NULL;

	const int i = t->GetHeapPos();

	Swap(i, (int)mBuffer.size() - 1);
	
	mBuffer.pop_back();
	t->NotInHeap();

	if (mBuffer[i]->GetPriority() < t->GetPriority())
		DownHeap(i);
	else
		UpHeap(i);

	return t;
}

}

// FLEXIBLEHEAP_H
#endif