#ifndef _DYNAMIC_PRIO_QUEUE_H_
#define _DYNAMIC_PRIO_QUEUE_H_

namespace utils
{
	//
	/// Priority list meant to support bidirectional traversal and priority changes on the elements stored
	/// It self sorts itself lazily when needed
	/// Elements are unique
	//
	template<class T>
	class TDynamicPrioList
	{
		// POD
		template
		struct PrioElem
		{
			T _elem;
			int _prio;

			bool operator==(const T& elem) const
			{
				return _elem == elem;
			}
		};

		struct PrioComparator
		{
			bool operator()(const PrioElem& lhs, const PrioElem& rhs)
			{
				return lhs._prio >= rhs._prio;
			}
		};

		typedef PrioElem PrioElement;
		typedef std::list<PrioElement> InternalList;

	public:
		typedef typename InternalList::iterator InternalIterator;
		typedef typename InternalList::const_iterator InternalConstIterator;
		typedef const T& const_reference;
		typedef T& reference;

	public:
		class Iterator : public std::iterator<std::bidirectional_iterator_tag, T>
		{
		public:
			friend class ConstIterator;

			template<class T>
			friend class TDynamicPrioList;

			Iterator(TDynamicPrioList<T>& container) : _it(NULL), _container(container) { _it = container.end(); }
			Iterator(TDynamicPrioList<T>& container, InternalIterator& it) : _it(it), _container(container) {}
			Iterator(const Iterator& it) : _it(it._it), _container(it._container) {}

			int getPrio() const { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); return _it->_prio; }

			Iterator& operator=(const Iterator& it) { _it = it._it; _container = it._container; return *this; }

			Iterator& operator++() { ++_it; return *this; }
			Iterator operator++(int) { Iterator tmp(*this); operator++(); return tmp; }

			Iterator& operator--() { --_it; return *this; }
			Iterator operator--(int) { Iterator tmp(*this); operator--(); return tmp; }

			bool operator==(const Iterator& rhs) const { return _it==rhs._it; }
			bool operator!=(const Iterator& rhs) const { return _it!=rhs._it; }
			//bool operator!=(const InternalIterator& rhs) { return _it!=rhs; }
			reference operator*() { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); return (_it->_elem); }

		private:
			void setPrio(int prio) const { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); _it->_prio = prio; _container._sorted = false; }

		private:
			InternalIterator _it;
			TDynamicPrioList<T>& _container;
		};

		class ConstIterator : public std::iterator<std::bidirectional_iterator_tag, T>
		{
		public:
			template<class T>
			friend class TDynamicPrioList;


			ConstIterator(const TDynamicPrioList<T>& container) : _it(NULL), _container(container) { _it = container.end(); }
			ConstIterator(const TDynamicPrioList<T>& container, InternalConstIterator& it) : _it(it), _container(container) {}
			ConstIterator(const ConstIterator& it) : _it(it._it), _container(it._container) {}
			ConstIterator(const Iterator& it) : _it(it._it), _container(it._container) {}

			int getPrio() const { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); return _it->_prio; }

			bool operator==(const ConstIterator& rhs) const { return _it==rhs._it; }
			bool operator==(const Iterator& rhs) const { return _it==rhs._it; }
			bool operator!=(const ConstIterator& rhs) const { return _it!=rhs._it; }

			ConstIterator& operator--() { --_it; return *this; }
			ConstIterator operator--(int) { ConstIterator tmp(*this); operator--(); return tmp; }

			ConstIterator& operator++() { ++_it; return *this; }
			ConstIterator operator++(int) { ConstIterator tmp(*this); operator++(); return tmp; }
			const_reference operator*() { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); return (_it->_elem); }

		private:
			void setPrio(int prio) const { NASSERT_EX(_it != _container._list.end(), "Invalid DynamicPrioQueue::iterator access"); _it->_prio = prio; _container._sorted = false; }

		private:
			mutable InternalConstIterator _it;
			const TDynamicPrioList<T>& _container;
		};

		typedef Iterator iterator;
		typedef ConstIterator const_iterator;

	public:
		friend class iterator;
		friend class const_iterator;
		
		void insert(const T& elem, int prio, bool doSort = false);
		iterator erase(const_iterator pos);
		const_iterator find(const T& elem) const;

		void setPrio(const T& elem, int prio);
		void setPrio(const iterator& it, int prio);
		
		iterator begin();
		const_iterator begin() const;

		iterator end();
		const_iterator end() const;

		reference front();
		const_reference front() const;

		reference back();
		const_reference back() const;

		unsigned int size() const;

		void clear();

	protected:
		void sort() const;

	private:
		// Mutability is require to enable lazy self sorting even when accessing through const methods
		mutable InternalList _list;
		mutable bool _sorted;
	};

	template<class T>
	typename TDynamicPrioList<T>::iterator utils::TDynamicPrioList<T>::erase( typename TDynamicPrioList<T>::const_iterator pos )
	{
		NASSERT_EX(&(pos._container) == this, "Trying to erase through an iterator to a different prio list");
		NASSERT_EX(pos != end(), "Trying to erase through an invalid iterator to a different prio list");
		bool sortingNeeded = true;
		// Detect cases where sorting won't be needed
		if(pos == begin() || pos == (begin() + (size()-1)))
		{
			sortingNeeded = false;
		}
		
		if(_sorted && sortingNeeded)
		{
			_sorted = false;
		}
	}

	template<class T>
	void TDynamicPrioList<T>::insert( const T& elem, int prio, bool doSort /*= false*/ )
	{
		PrioElement p = {elem, prio};

		// This is an invariant, no duplicated elements allowed
		NASSERT_EX(find(elem) == end(), "Trying to insert a duplicate to the prio list");
		// TODO: This is naive, efficient insertion is possible if the list is sorted using lower bound
		_list.push_front(p);
		if(doSort)
		{
			sort();
		}
		else
		{
			_sorted = false;
		}
	}

	template<class T>
	void TDynamicPrioList<T>::sort() const
	{
		if(!_sorted)
		{		
			_list.sort(PrioComparator());
			_sorted = true;
		}
	}

	template<class T>
	typename TDynamicPrioList<T>::const_iterator TDynamicPrioList<T>::find( const T& elem ) const
	{
		return const_iterator(*this, std::find(_list.begin(), _list.end(), elem));
	}	

	template<class T>
	typename TDynamicPrioList<T>::iterator TDynamicPrioList<T>::begin()
	{	
		if(!_sorted) sort();
		return iterator(*this, _list.begin());
	}

	template<class T>
	typename TDynamicPrioList<T>::const_iterator TDynamicPrioList<T>::begin() const
	{
		if(!_sorted) sort();
		return const_iterator(*this, _list.begin());
	}

	template<class T>
	typename TDynamicPrioList<T>::iterator TDynamicPrioList<T>::end()
	{	
		if(!_sorted) sort();
		return iterator(*this, _list.end());
	}

	template<class T>
	typename TDynamicPrioList<T>::const_iterator TDynamicPrioList<T>::end() const
	{
		if(!_sorted) sort();
		return const_iterator(*this, _list.end());
	}

	template<class T>
	typename TDynamicPrioList<T>::reference utils::TDynamicPrioList<T>::front()
	{
		NASSERT_EX(!_list.empty(), "Breaking precondition in TDynamicPrioList<T>::front");
		if(!_sorted) sort();
		return _list.front()._elem;
	}

	template<class T>
	typename TDynamicPrioList<T>::const_reference utils::TDynamicPrioList<T>::front() const
	{
		NASSERT_EX(!_list.empty(), "Breaking precondition in TDynamicPrioList<T>::front");
		if(!_sorted) sort();
		return _list.front()._elem;
	}

	template<class T>
	typename TDynamicPrioList<T>::reference utils::TDynamicPrioList<T>::back()
	{
		NASSERT_EX(!_list.empty(), "Breaking precondition in TDynamicPrioList<T>::back");
		if(!_sorted) sort();
		return _list.back()._elem;
	}

	template<class T>
	typename TDynamicPrioList<T>::const_reference utils::TDynamicPrioList<T>::back() const
	{
		NASSERT_EX(!_list.empty(), "Breaking precondition in TDynamicPrioList<T>::back");
		if(!_sorted) sort();
		return _list.back()._elem;
	}

	template<class T>
	unsigned int utils::TDynamicPrioList<T>::size() const
	{
		return _list.size();
	}


	template<class T>
	void utils::TDynamicPrioList<T>::clear()
	{
		_list.clear();
		_sorted = false;
	}

	template<class T>
	void utils::TDynamicPrioList<T>::setPrio( const iterator& it, int prio )
	{
		NASSERT_EX(&(it._container) == this, "Setting prio to an iterator of a different container");
		if(it != end())
		{
			it.setPrio(prio);
		}
	}

	template<class T>
	void utils::TDynamicPrioList<T>::setPrio( const T& elem, int prio )
	{
		const_iterator i = find(elem);
		if(i != end())
		{
			it.setPrio(prio);
		}
		else
		{
			insert(elem, prio);
		}
	}

}

#endif