#ifndef B3_RESIZABLE_POOL_H
#define B3_RESIZABLE_POOL_H

#include "Bullet3Common/b3AlignedObjectArray.h"

enum
{
	B3_POOL_HANDLE_TERMINAL_FREE = -1,
	B3_POOL_HANDLE_TERMINAL_USED = -2
};

template <typename U>
struct b3PoolBodyHandle : public U
{
	B3_DECLARE_ALIGNED_ALLOCATOR();

	int m_nextFreeHandle;
	void setNextFree(int next)
	{
		m_nextFreeHandle = next;
	}
	int getNextFree() const
	{
		return m_nextFreeHandle;
	}
};

template <typename T>
class b3ResizablePool
{
protected:
	b3AlignedObjectArray<T> m_bodyHandles;
	int m_numUsedHandles;   // number of active handles
	int m_firstFreeHandle;  // free handles list

	T* getHandleInternal(int handle)
	{
		return &m_bodyHandles[handle];
	}
	const T* getHandleInternal(int handle) const
	{
		return &m_bodyHandles[handle];
	}

public:
	b3ResizablePool()
	{
		initHandles();
	}

	virtual ~b3ResizablePool()
	{
		exitHandles();
	}
	///handle management

	int getNumHandles() const
	{
		return m_bodyHandles.size();
	}

	void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
	{
		for (int i = 0; i < m_bodyHandles.size(); i++)
		{
			if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
			{
				usedHandles.push_back(i);
			}
		}
	}

	T* getHandle(int handle)
	{
		b3Assert(handle >= 0);
		b3Assert(handle < m_bodyHandles.size());
		if ((handle < 0) || (handle >= m_bodyHandles.size()))
		{
			return 0;
		}

		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
		{
			return &m_bodyHandles[handle];
		}
		return 0;
	}
	const T* getHandle(int handle) const
	{
		b3Assert(handle >= 0);
		b3Assert(handle < m_bodyHandles.size());
		if ((handle < 0) || (handle >= m_bodyHandles.size()))
		{
			return 0;
		}

		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
		{
			return &m_bodyHandles[handle];
		}
		return 0;
	}

	void increaseHandleCapacity(int extraCapacity)
	{
		int curCapacity = m_bodyHandles.size();
		//b3Assert(curCapacity == m_numUsedHandles);
		int newCapacity = curCapacity + extraCapacity;
		m_bodyHandles.resize(newCapacity);

		{
			for (int i = curCapacity; i < newCapacity; i++)
				m_bodyHandles[i].setNextFree(i + 1);

			m_bodyHandles[newCapacity - 1].setNextFree(-1);
		}
		m_firstFreeHandle = curCapacity;
	}
	void initHandles()
	{
		m_numUsedHandles = 0;
		m_firstFreeHandle = -1;

		increaseHandleCapacity(1);
	}

	void exitHandles()
	{
		m_bodyHandles.resize(0);
		m_firstFreeHandle = -1;
		m_numUsedHandles = 0;
	}

	int allocHandle()
	{
		b3Assert(m_firstFreeHandle >= 0);

		int handle = m_firstFreeHandle;
		m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
		m_numUsedHandles++;

		if (m_firstFreeHandle < 0)
		{
			//int curCapacity = m_bodyHandles.size();
			int additionalCapacity = m_bodyHandles.size();
			increaseHandleCapacity(additionalCapacity);

			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
		}
		getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
		getHandleInternal(handle)->clear();
		return handle;
	}

	void freeHandle(int handle)
	{
		b3Assert(handle >= 0);

		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
		{
			getHandleInternal(handle)->clear();
			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
			m_firstFreeHandle = handle;
			m_numUsedHandles--;
		}
	}
};
///end handle management

#endif  //B3_RESIZABLE_POOL_H