#include "WimpLib:OrderedSet.h"

#include "WimpLib:Exception.h"
#include "WimpLib:mem.h"
#include "WimpLib:Log.h"

static const char Err_InvalidNode[] = "OrderedSet invalid node";
static const char Err_InvalidIndex[] = "OrderedSet index out of bounds";

//#define TEST

/*
 * Ordered sets are implemented by using red-black binary search trees
 * with the addition of checking the last used node first
 * to improve insertions/lookups of ordered data.
 *
 * A binary search tree is a tree which has the following properties:
 * - Value of node is >= Value of any node in left subtree
 * - Value of node is <= Value of any node in right subtree
 *
 * A red-black tree is a binary search tree which has the following properties:
 * - Every node is either red or black.
 * - Every leaf (NULL) is back.
 * - If a node is red, then both its children are black.
 * - Every path from a node to a descendant contains the same number
 *   of black nodes.
 * Preserving the red-black properties ensure that the tree remains
 * more or less balanced (depth never exceeds  2*log(n+1))
 * and that search and insertion takes O(log n).
 *
 * The implementation uses sentinel nodes to mark black leafs.
 *
 * The nodes have also indexing information giving their relative
 * position compared to their parent. The NULL node on top
 * of the tree can be considered to have the position behind
 * the last element in the tree, allowing a fast calculation
 * of the index of a node by traversing the tree from that
 * node to the top and cummulating the relative indexes.
 */

typedef enum {Node_Red, Node_Black} Node_Colour;

struct OrderedNode
{
	OrderedNode* pParent;
	OrderedNode* pLeft;
	OrderedNode* pRight;
	Node_Colour   Colour;
	const void*  pData;
	int          RelativeIndex;
	int          Count;
};

struct OrderedSet
{
	OrderedNode         NilNode;
	OrderedSet_FCompare Compare;
	OrderedSet_FAlloc   Alloc;
	OrderedSet_FDelete  Delete;
	void*               pHandle;
	int                 NodeCount;
	OrderedNode*        pLastUsedNode;
};

#ifdef TEST

#include <assert.h>

static void ASSERT_NODE(const OrderedSet* This, const OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;
	const OrderedNode* pRef = pNode;

	if ((pNil->pParent != pNil)
	||  (pNil->pRight != pNil)
	||  (pNil->Colour != Node_Black)
	||  !((This->NodeCount > 0) || (pNil->pLeft == pNil))
	||  ((pNil->pLeft != pNil)
		&& ((pNil->pLeft->pParent != pNil) || (pNil->pLeft->RelativeIndex > 0))))
	{
		Log("Bad Nil Node\n");
		assert(false);
	}

	while (pNode != pNil)
	{
		if ((pNode->pParent == pNode)
		||  (pNode->pLeft == pNode)
		||  (pNode->pRight == pNode))
		{
			Log("Autoref Node %p -> %p, Nil: %p\n", pRef, pNode, pNil);
			assert(false);
		}
		if (/*(pNode->pParent != pNil)
			&&*/ (pNode->pParent->pLeft != pNode)
			&& (pNode->pParent->pRight != pNode))
		{
			Log("Bad parent ref Node %p -> %p\n", pRef, pNode);
			assert(false);
		}
		if ((pNode->pLeft != pNil)
			&& ((pNode->pLeft->pParent != pNode) || (pNode->pLeft->RelativeIndex > 0)))
		{
			Log("Bad left ref Node %p -> %p\n", pRef, pNode);
			assert(false);
		}
		if ((pNode->pRight != pNil)
			&& ((pNode->pRight->pParent != pNode) || (pNode->pRight->RelativeIndex < 0)))
		{
			Log("Bad right ref Node\n");
			assert(false);
		}

		pNode = pNode->pParent;
	}
}

#else

#undef  assert
#define assert(x)
#define ASSERT_NODE(x, y)

#endif

/**
 * Creates a new ordered set.
 *
 * @param  pVPtr   Pointer to the function callbacks defining the set.
 *
 * @returns        Pointer to a new OrderedSet.
 */
OrderedSet* New_OrderedSet(void* pHandle, const OrderedSetVPtr* pVPtr) throws(mem)
{
	OrderedSet* This = throw_mem_alloc(sizeof(*This));

	// Sentinel Node
	This->NilNode.pParent
		= This->NilNode.pLeft
		= This->NilNode.pRight
		= &This->NilNode;
	This->NilNode.Colour = Node_Black;
	This->NilNode.pData = NULL;
	This->NilNode.RelativeIndex = 0;
	This->NilNode.Count = 0;

	This->pHandle = pHandle;
	This->Compare = pVPtr->FCompare;
	This->Alloc = pVPtr->FAlloc;
	This->Delete = pVPtr->FDelete;
	This->NodeCount = 0;
	This->pLastUsedNode = NULL;

	return This;
}

/**
 * Deletes an ordered set.
 * This function also deletes the elements contained in the set.
 */
void Delete_OrderedSet(OrderedSet* This)
{
	if (This)
	{
		OrderedSet_Clear(This);

		mem_free(This);
	}
}

/**
 * Returns the number of different nodes in the set.
 */
int OrderedSet_Count(const OrderedSet* This)
{
	return This->NodeCount;
}

/**
 * Returns the function used to sort elements in the set.
 */
OrderedSet_FCompare OrderedSet_GetFCompare(const OrderedSet* This)
{
	return This->Compare;
}

/**
 * Rebalances the ordered tree by pushing the node below its right child.
 * This means that child takes the place of the node, the child's left child
 * becomes the node's right child and the node becomes the child's left child.
 * This operation keeps the tree ordered.
 *
 * @param  pUpper  Pointer to the node around which the rotation takes place.
 */
static void RotateLeft(OrderedSet* This, OrderedNode* pUpper)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pLower = pUpper->pRight;
	OrderedNode* pLowerLeft = pLower->pLeft;
	OrderedNode* pUpperParent = pUpper->pParent;
	int URelIndex = pUpper->RelativeIndex;

	assert(pUpper != pNil);
	assert(pLower != pNil);
	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pUpper);
	ASSERT_NODE(This, pLower);
	ASSERT_NODE(This, pLowerLeft);
	ASSERT_NODE(This, pUpperParent);

	// First update relative indexes
	if (pLowerLeft != pNil)
		pLowerLeft->RelativeIndex += pLower->RelativeIndex;
	pUpper->RelativeIndex = -pLower->RelativeIndex;
	pLower->RelativeIndex += URelIndex;

	// Update positions
	pUpper->pRight = pLowerLeft;
	if (pLowerLeft!= pNil) pLowerLeft->pParent = pUpper;

	pLower->pParent = pUpperParent;
	if (pUpper == pUpperParent->pLeft)
		pUpperParent->pLeft = pLower;
	else
		pUpperParent->pRight = pLower;

	pLower->pLeft = pUpper;
	pUpper->pParent = pLower;

	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pUpper);
	ASSERT_NODE(This, pLower);
	ASSERT_NODE(This, pLowerLeft);
	ASSERT_NODE(This, pUpperParent);
}

/**
 * Rebalances the ordered tree by pushing the node below its left child.
 * This means that child takes the place of the node, the child's right child
 * becomes the node's left child and the node becomes the child's right child.
 * This operation keeps the tree ordered.
 *
 * @param  pUpper  Pointer to the node around which the rotation takes place.
 */
static void RotateRight(OrderedSet* This, OrderedNode* pUpper)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pLower = pUpper->pLeft;
	OrderedNode* pLowerRight = pLower->pRight;
	OrderedNode* pUpperParent = pUpper->pParent;
	int URelIndex = pUpper->RelativeIndex;

	assert(pUpper != pNil);
	assert(pLower != pNil);
	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pUpper);
	ASSERT_NODE(This, pLower);
	ASSERT_NODE(This, pLowerRight);
	ASSERT_NODE(This, pUpperParent);

	// First update relative indexes
	if (pLowerRight != pNil)
		pLowerRight->RelativeIndex += pLower->RelativeIndex;
	pUpper->RelativeIndex = -pLower->RelativeIndex;
	pLower->RelativeIndex += URelIndex;

	// Update positions
	pUpper->pLeft = pLowerRight;
	if (pLowerRight != pNil) pLowerRight->pParent = pUpper;

	pLower->pParent = pUpperParent;
	if (pUpper == pUpperParent->pLeft)
		pUpperParent->pLeft = pLower;
	else
		pUpperParent->pRight = pLower;

	pLower->pRight = pUpper;
	pUpper->pParent = pLower;

	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pUpper);
	ASSERT_NODE(This, pLower);
	ASSERT_NODE(This, pLowerRight);
	ASSERT_NODE(This, pUpperParent);
}

/**
 * Propagates relative index changes to the top from the left.
 *
 * @param  pChild  Pointer to node to detach.
 * @param  value   Value to update by, 1 for add, -1 for remove.
 */
static void OrderedSet_FixParentIndex
	( OrderedSet* This
	, OrderedNode* pChild
	, int value
	)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pNode = pChild->pParent;
	OrderedNode* pParent = pNode->pParent;

	// Update count on each change of direction
	while (pNode != pNil)
	{
		if (pChild == pNode->pLeft)
		{
			if (pNode == pParent->pRight)
				pNode->RelativeIndex += value;
		}
		else
		{
			if (pNode == pParent->pLeft)
				pNode->RelativeIndex -= value;
		}
		pChild = pNode;
		pNode = pParent;
		pParent = pParent->pParent;
	}
}

/**
 * Attaches a node in the ordered tree. The node is marked as red
 * and restoring the red-black properties of the tree rebalances the tree.
 * The relative indexes are updated from the parent node to the top one.
 *
 * @param  pParent  Pointer to the bottom node where to attach in the OrderedSet.
 * @param  val      Compare result determining left or right attachment.
 * @param  pNode    Pointer to the node to attach in the OrderedSet.
 */
static void OrderedSet_Attach(OrderedSet* This, OrderedNode* pParent, int val, OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pWhere = pNode;

	assert(pParent);
	assert(pNode);
	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pParent);

	pWhere = pNode;

	pNode->pParent = pParent;
	pNode->pLeft = (OrderedNode*) pNil;
	pNode->pRight = (OrderedNode*) pNil;
	pNode->Colour = Node_Red;

	if (val < 0)
	{
		assert(pParent->pLeft == pNil);
		pParent->pLeft = pNode;
		pNode->RelativeIndex = -1;
	}
	else
	{
		assert(pParent->pRight == pNil);
		pParent->pRight = pNode;
		pNode->RelativeIndex = +1;
	}
	OrderedSet_FixParentIndex(This, pNode, +1);

	// Red-black adjustments.
	// We introduce a red node at the bottom of the tree node so that we do not
	// increase the black depth but if its parent is red we break the third rule.

	// Note: the sentinel above root is Black
	while (pParent->Colour == Node_Red)
	{
		OrderedNode* pGrandPa = pParent->pParent;
		OrderedNode* pUncle;

		// assert(pParent != pNil);
		// assert(pGrandPa->Colour == Node_Black);

		if (pGrandPa == pNil)
		{
			pParent->Colour = Node_Black;
			break;
		}

		if (pParent == pGrandPa->pLeft)
		{
			pUncle = pGrandPa->pRight;

			if (pUncle->Colour == Node_Black)
			{
				// Red parent, black uncle
				// The tree inbalance starts and depending of the configuration
				// we need to put either the node or its parent in grandpa's place
				if (pNode == pParent->pRight)
				{
					RotateLeft(This, pParent);
					pParent = pNode;
				}
				pParent->Colour = Node_Black;
				pGrandPa->Colour = Node_Red;
				RotateRight(This, pGrandPa);
				break;
			}
		}
		else
		{
			pUncle = pGrandPa->pLeft;

			if (pUncle->Colour == Node_Black)
			{
				// Red parent, black uncle
				// The tree inbalance starts and depending of the configuration
				// we need to put either the node or its parent in grandpa's place
				if (pNode == pParent->pLeft)
				{
					RotateRight(This, pParent);
					pParent = pNode;
				}
				pParent->Colour = Node_Black;
				pGrandPa->Colour = Node_Red;
				RotateLeft(This, pGrandPa);
				break;
			}
		}

		// Red parent and uncle
		// Need to fix Colours and try upper in the tree
		pParent->Colour = Node_Black;
		pUncle->Colour = Node_Black;
		pGrandPa->Colour = Node_Red;

		pNode = pGrandPa;
		pParent = pNode->pParent;
	}

	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pParent);
	ASSERT_NODE(This, pWhere);
}

/**
 * Detaches a node from the set,
 * rebalances the red-black tree,
 * fixes the relative indexes.
 *
 * @param  pNode  Pointer to node to detach.
 */
static void OrderedSet_Detach(OrderedSet* This , OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pParent;
	OrderedNode* pChild;

	assert(pNode && (pNode != pNil));
	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pNode);
	ASSERT_NODE(This, pNode->pParent);
	ASSERT_NODE(This, pNode->pLeft);
	ASSERT_NODE(This, pNode->pRight);

	This->pLastUsedNode = NULL;

	if ((pNode->pLeft != pNil) && (pNode->pRight != pNil))
	{
		// Deleting a node with 2 childs is difficult so we will
		// exchange its data with its successor's node (which is last
		// node on the chain of left chain of the node's right child)
		// since this one as no left child and delete this one.
		OrderedNode* pNext = OrderedSet_GetSuccessor(This, pNode);
		Node_Colour Colour = pNext->Colour;

		assert(pNext && (pNext != pNil));
		assert(pNext->pLeft == pNil);

		ASSERT_NODE(This, pNext);

		// Substitution makes successor's eventual right child the node's only child.
		// Substitution and removal will move node's parent and left child as parent and
		// left child of successor.
		pChild = pNext->pRight;

		OrderedSet_FixParentIndex(This, pNext, -1);

		if (pChild != pNil)
		{
			pChild->RelativeIndex += pNext->RelativeIndex;
			if (pNode->pLeft == pChild)
			{
				if (pNext->pParent->pLeft == pNext)
					pChild->RelativeIndex++;
			}
			else
			{
				if (pNext->pParent->pRight == pNext)
					pChild->RelativeIndex--;
			}
		}

		if (pNode->pRight == pNext)
		{
			// Substitution makes successor the node's parent.
			// Substitution and removal leaves successor's right child unchanged.
			pParent = pNext;
		}
		else
		{
			// Substitution makes successor's parent the node's parent.
			pParent = pNext->pParent;
			// After removal successor's child take the place of it's parent in the tree.
			if (pParent->pLeft == pNext)
				pParent->pLeft = pChild;
			else
				pParent->pRight = pChild;
			if (pChild != pNil) pChild->pParent = pParent;

			// After removal right child of node becomes right child of successor.
			pNext->pRight = pNode->pRight;
			pNext->pRight->pParent = pNext;
		}

		// After substitition (and removal), parent and left child of node
		// become parent and left child of successor.
		pNext->pParent = pNode->pParent;
		if (pNext->pParent->pLeft == pNode)
			pNext->pParent->pLeft = pNext;
		else
			pNext->pParent->pRight = pNext;

		pNext->pLeft = pNode->pLeft;
		pNext->pLeft->pParent = pNext;

		pNext->RelativeIndex = pNode->RelativeIndex;
		pNext->Colour = pNode->Colour;
		pNode->Colour = Colour;

		ASSERT_NODE(This, pNext);
	}
	else
	{
		// Node with at most one child.
		pParent = pNode->pParent;
		pChild = (pNode->pLeft != pNil) ? pNode->pLeft : pNode->pRight;

		OrderedSet_FixParentIndex(This, pNode, -1);

		if (pChild != pNil)
		{
			pChild->RelativeIndex += pNode->RelativeIndex;
			if (pNode->pLeft == pChild)
			{
				if (pParent->pLeft == pNode)
					pChild->RelativeIndex++;
			}
			else
			{
				if (pParent->pRight == pNode)
					pChild->RelativeIndex--;
			}
		}

		// Let the child take the place of the node in the tree.
		if (pParent->pLeft == pNode)
			pParent->pLeft = pChild;
		else
			pParent->pRight = pChild;

		if (pChild != pNil) pChild->pParent = pParent;
	}

	ASSERT_NODE(This, pNil);
	ASSERT_NODE(This, pParent);
	ASSERT_NODE(This, pChild);

	// Red-black tree adjustments.
	// There is nothing to do if we remove a red node. For a black node either
	// we find (through possible rorations) a red node to turn to black
	// or the whole tree is to end reorganised and end with a decreased black height.

	if (pNode->Colour == Node_Black)
	{
		if (pChild->Colour == Node_Red)
		{
			// The only child is red, turning it black fixes our problem.
			pChild->Colour = Node_Black;
		}
        else
        {
			while(pParent != pNil)
			{
				OrderedNode* pSister;

				if (pChild == pParent->pLeft)
				{
					pSister = pParent->pRight;

					if (pSister->Colour == Node_Red)
					{
						// Means that parent is black and sister childs are black
						// Rotate the sister above brings us to a case below
						pSister->Colour = Node_Black;
						pParent->Colour = Node_Red;
						RotateLeft(This, pParent);
						pSister = pParent->pRight;
					}

					// Now sister is always back

					if (pSister->pRight->Colour == Node_Black)
					{
						if (pSister->pLeft->Colour == Node_Black)
						{
							if (pParent->Colour == Node_Black)
							{
								// Everything is black around us, there is nothing
								// that can be done at our level, so decrease the black count
								// on the sister side too by turning it red and reattempt
								// one level higher.
								pSister->Colour = Node_Red;

								pChild = pParent;
								pParent = pChild->pParent;

								continue;
							}
							else
							{
								// Turn parent black to restore black height
								// and turn sister red to compensate in that direction.
								pSister->Colour = Node_Red;
								pParent->Colour = Node_Black;
								break;
							}
						}
						else
						{
							// Rotate sister right and change Colours to maintain height
							pSister->pLeft->Colour = Node_Black;
							pSister->Colour = Node_Red;
							RotateRight(This, pSister);
							pSister = pParent->pRight;
							// The configuration is now the same as final case
						}
					}

					// Sister is black and has a red right child

					pSister->Colour = pParent->Colour;
					pSister->pRight->Colour = Node_Black;
					pParent->Colour = Node_Black;
					RotateLeft(This, pParent);
					break;
				}
				else
				{
					pSister = pParent->pLeft;

					if (pSister->Colour == Node_Red)
					{
						// Means that parent is black
						// Rotate the sister above brings us to a case below
						pSister->Colour = Node_Black;
						pParent->Colour = Node_Red;
						RotateRight(This, pParent);
						pSister = pParent->pLeft;
					}

					// Now sister is always back

					if (pSister->pLeft->Colour == Node_Black)
					{
						if (pSister->pRight->Colour == Node_Black)
						{
							if (pParent->Colour == Node_Black)
							{
								// Everything is black around us, there is nothing
								// that can be done at our level, so decrease the black count
								// on the sister side too by turning it red and reattempt
								// one level higher.
								pSister->Colour = Node_Red;

								pChild = pParent;
								pParent = pChild->pParent;

								continue;
							}
							else
							{
								// Turn parent black to restore black height
								// and turn sister red to compensate in that direction.
								pSister->Colour = Node_Red;
								pParent->Colour = Node_Black;
								break;
							}
						}
						else
						{
							// Rotate sister left and change Colours to maintain height
							pSister->pRight->Colour = Node_Black;
							pSister->Colour = Node_Red;
							RotateLeft(This, pSister);
							pSister = pParent->pLeft;
							// The configuration is now the same as final case
						}
					}

					// Sister is black and has a red right child

					pSister->Colour = pParent->Colour;
					pSister->pLeft->Colour = Node_Black;
					pParent->Colour = Node_Black;
					RotateRight(This, pParent);
					break;
				}
			}
		}
	}

	pNode->pParent = (OrderedNode*) pNil;
	pNode->pLeft = (OrderedNode*) pNil;
	pNode->pRight = (OrderedNode*) pNil;
}

/**
 * Increments the existing node's duplication counter.
 *
 * @param  pNode  Pointer to a node from the OrderedSet.
 */
void OrderedSet_AddNode(OrderedSet* This, OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;

	ASSERT_NODE(This, pNil);

	ASSERT_NODE(This, pNode);

	pNode->Count++;
}

/**
 * Inserts a new node in the ordered set.
 * For duplicates of existing nodes, it simply increments the existing node's
 * duplication counter.
 *
 * @param  pData  Pointer to the data to insert in the OrderedSet.
 *
 * @returns       Pointer to a node from the OrderedSet.
 */
OrderedNode* OrderedSet_Add(OrderedSet* This, const void* pData) throws(mem)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pWhere = pNil->pLeft;
	OrderedNode* pParent = (OrderedNode*) pNil;
	int val = -1;

	ASSERT_NODE(This, pNil);

	// Normal ordered tree insertion.
	// Move down the binary search tree till we found a match
	// or till we reach the bottom of the tree.
	while (pWhere != pNil)
	{
		val = This->Compare(pData, pWhere->pData);

		if (!val)
		{
			pWhere->Count++;
			This->pLastUsedNode = pWhere;
			return pWhere;
		}

		pParent = pWhere;

		if (val < 0)
			pWhere = pWhere->pLeft;
		else
			pWhere = pWhere->pRight;
	}

	// Reached the bottom of the tree.
	// Add the new entry as child of the last node.
	pWhere = throw_mem_alloc(sizeof(*pWhere));
	pWhere->Count = 1;
	pWhere->pData = This->Alloc ? This->Alloc(This->pHandle, pData) : pData;

	This->NodeCount++;

	OrderedSet_Attach(This, pParent, val, pWhere);

	This->pLastUsedNode = pWhere;
	return pWhere;
}

/**
 * Reorder a node in the ordered set.
 * If the node becomes a duplicate of another it is merged with this one.
 * It should be called after the modification of the data attached
 * to the node.
 *
 * @param  pNode  Pointer to node to resort in the OrderedSet.
 *
 * @returns       Pointer to a node from the OrderedSet.
 */
OrderedNode* OrderedSet_Update(OrderedSet* This, OrderedNode* pNode) throws(null)
{
	const OrderedNode* pNil = &This->NilNode;

	if (!pNode
	||  (pNode == pNil))
		throw_runtime(Err_InvalidNode);

	ASSERT_NODE(This, pNil);

	OrderedSet_Detach(This, pNode);

	OrderedNode* pWhere = pNil->pLeft;
	OrderedNode* pParent = (OrderedNode*) pNil;
	int val = -1;

	assert(pNil->pRight == pNil);
	assert(pNil->Colour == Node_Black);
	assert(pNil->RelativeIndex == 0);

	// Normal ordered tree insertion.
	// Move down the binary search tree till we found a match
	// or till we reach the bottom of the tree.
	while (pWhere != pNil)
	{
		val = This->Compare(pNode->pData, pWhere->pData);

		if (!val)
		{
			// Merge the nodes
			pWhere->Count += pNode->Count;

			// Delete the node.
			if (This->Delete) This->Delete(This->pHandle, (void*) pNode->pData);
			mem_free(pNode);
			This->NodeCount--;

			return pWhere;
		}

		pParent = pWhere;

		if (val < 0)
			pWhere = pWhere->pLeft;
		else
			pWhere = pWhere->pRight;
	}

	// Reached the bottom of the tree.
	OrderedSet_Attach(This, pParent, val, pNode);

	return pNode;
}

/**
 * Decreases a node's count and once it reaches zero, removes the node from the set
 * and deletes the associated object.
 *
 * @param  pNode  Pointer to node to remove.
 *                NULL is ignored.
 */
void OrderedSet_RemoveNode(OrderedSet* This , OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;

	if (!pNode
	||  (pNode == pNil))
		return;

	if (--pNode->Count)
		return;

	OrderedSet_Detach(This, pNode);

	// Delete the node.
	if (This->Delete) This->Delete(This->pHandle, (void*) pNode->pData);
	mem_free(pNode);
	This->NodeCount--;
}

/**
 * Decreases a node's count and once it reaches zero, removes the node from the set
 * and deletes the associated object.
 *
 * @param  index  Index of the node to remove.
 *                Index must be valid, -1 is ignored.
 */
void OrderedSet_Remove(OrderedSet* This , int index) throws(index)
{
	if (index == -1)
		return;

	OrderedSet_RemoveNode(This, OrderedSet_GetNode(This, index));
}

/**
 * Recursively deletes all the nodes.
 * This function also delete the data associated to the node.
 */
void OrderedSet_Clear(OrderedSet* This)
{
	OrderedNode* pNode = This->NilNode.pLeft;
	OrderedNode* pNil = &This->NilNode;
	OrderedNode* pDelNode;

	This->pLastUsedNode = NULL;

	while(pNode != pNil)
	{
		// goto leftmost child
		while (pNode->pLeft != pNil)
			pNode = pNode->pLeft;

		// a right child might have a left child
		if (pNode->pRight != pNil)
		{
			pNode = pNode->pRight;
			continue;
		}

		// has no child we may delete it
		pDelNode = pNode;
		pNode = pNode->pParent;
		if (pNode->pLeft != pNil)
			pNode->pLeft = pNil;
		else
			pNode->pRight = pNil;

		if (This->Delete) This->Delete(This->pHandle, (void*) pDelNode->pData);
		mem_free(pDelNode);
	}

	This->NodeCount = 0;
	ASSERT_NODE(This, pNil);
}

/**
 * Returns the order of a node in the set, index starts at 0 for the lowest node.
 *
 * @param  pNode  Pointer to the node.
 *
 * @returns       Index.
 */
int OrderedSet_GetNodeIndex(const OrderedSet* This, const OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;
	int index;

	if (!pNode
	||  (pNode == pNil))
		throw_runtime(Err_InvalidNode);

	ASSERT_NODE(This, pNil);

	index = This->NodeCount;

	while (pNode != pNil)
	{
		index += pNode->RelativeIndex;
		pNode = pNode->pParent;
	}

	return index;
}

/**
 * Returns the data associated to a given node.
 *
 * @param  pNode  Pointer to the node.
 *
 * @returns       Pointer to the data associated to the node.
 */
const void* OrderedSet_GetNodeData(const OrderedSet* This, const OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;

	ASSERT_NODE(This, pNil);

	if (!pNode
	||  (pNode == pNil))
		throw_runtime(Err_InvalidNode);

	ASSERT_NODE(This, pNode);

	return pNode->pData;
}

/**
 * Returns the next node in order (>=) to a given node.
 *
 * @param  pNode  Pointer to the node or NULL to get the first (lowest) node.
 *
 * @returns       Pointer to the next node or NULL if none.
 */
OrderedNode* OrderedSet_GetSuccessor(const OrderedSet* This, const OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;

	ASSERT_NODE(This, pNil);

	if (!pNode)
	{
		// Find the leftmost child
		pNode = pNil;

		while (pNode->pLeft != pNil)
			pNode = pNode->pLeft;
	}
	else if (pNode->pRight != pNil)
	{
		// Find the leftmost child of right child
		pNode = pNode->pRight;

		while (pNode->pLeft != pNil)
			pNode = pNode->pLeft;
	}
	else
	{
		// Find this first parent in the chain for which we are at the left side
		const OrderedNode* pParent = pNode->pParent;

		while ((pParent != pNil) && (pNode == pParent->pRight))
		{
			pNode = pParent;
			pParent = pNode->pParent;
		}

		pNode = pParent;
	}

	return (pNode == pNil) ? NULL : (OrderedNode*) pNode;
}

/**
 * Returns the previous node in order (<=) to a given node.
 *
 * @param  pNode  Pointer to the node or NULL to get the last (highest) node.
 *
 * @returns       Pointer to the previous node or NULL if none.
 */
OrderedNode* OrderedSet_GetPredeccessor(const OrderedSet* This, const OrderedNode* pNode)
{
	const OrderedNode* pNil = &This->NilNode;

	ASSERT_NODE(This, pNil);

	if (!pNode)
	{
		// Find the rightmost child
		pNode = pNil;

		while (pNode->pRight != pNil)
			pNode = pNode->pRight;
	}
	else if (pNode->pLeft != pNil)
	{
		// Find the rightmost child of left child
		pNode = pNode->pLeft;

		while (pNode->pRight != pNil)
			pNode = pNode->pRight;
	}
	else
	{
		// Find this first parent in the chain for which we are at the right side
		const OrderedNode* pParent = pNode->pParent;

		while ((pParent != pNil) && (pNode == pParent->pLeft))
		{
			pNode = pParent;
			pParent = pNode->pParent;
		}

		pNode = pParent;
	}

	return (pNode == pNil) ? NULL : (OrderedNode*) pNode;
}

/**
 * Attempts to locate data in a set.
 *
 * @param  pNode  Pointer to node to start from or NULL to start from the beginning.
 * @param  pData  Pointer to data to locate.
 *
 * @returns       Pointer to the set's node containing the data or NULL if not found.
 */
OrderedNode* OrderedSet_FindNode(const OrderedSet* This, const void* pData)
{
	const OrderedNode* pNil = &This->NilNode;
	const OrderedNode* pNode = pNil->pLeft;
	int val;

	while (pNode != pNil)
	{
		val = This->Compare(pData, pNode->pData);

		if (!val)
		{
			return (OrderedNode*) pNode;
		}
		else if (val < 0)
			pNode = pNode->pLeft;
		else
			pNode = pNode->pRight;
	}

	return NULL;
}

/**
 * Returns the data associated to a the node with the given index.
 *
 * @param  index  Index of the node to locate.
 *
 * @returns       Pointer to the data associated to the node.
 */
const void* OrderedSet_Get(const OrderedSet* This, int index) throws(index)
{
	OrderedNode* pNode = OrderedSet_GetNode(This, index);

	if (!pNode) return NULL;

	return pNode->pData;
}

/**
 * Returns the node with the given index.
 *
 * @param  index  Index of the node to locate.
 *
 * @returns       Pointer to the node.
 */
OrderedNode* OrderedSet_GetNode(const OrderedSet* This, int index) throws(index)
{
	const OrderedNode* pNil = &This->NilNode;
	OrderedNode* pNode = pNil->pLeft;

	ASSERT_NODE(This, pNil);

	index = This->NodeCount - index;

	while (pNode != pNil)
	{
		index += pNode->RelativeIndex;
		if (!index) return pNode;

		if (index > 0)
			pNode = pNode->pLeft;
		else
			pNode = pNode->pRight;
	}

	throw_runtime(Err_InvalidIndex);

	return NULL;
}

/**
 * Attempts to locate data in a set.
 *
 * @param  pData  Pointer to data to locate.
 *
 * @returns       Index of the set's node containing the data or -1 if not found.
 */
int OrderedSet_Find(const OrderedSet* This, const void* pData) throws(index)
{
	OrderedNode* pNode = OrderedSet_FindNode(This, pData);

	if (pNode) return OrderedSet_GetNodeIndex(This, pNode);

	return -1;
}

/**
 * Attempts to locate the lower bound for data in the set.
 *
 * @param  cmp    Function to use to find a partial match.
 * @param  pData  Pointer to data to bound.
 *
 * @returns       Pointer to the set's node containing the first data greater or equal
 *                to the given data.
 */
OrderedNode* OrderedSet_LowerBound(const OrderedSet* This, OrderedSet_FCompare cmp, const void* pData)
{
	const OrderedNode* pNil = &This->NilNode;
	const OrderedNode* pNode = pNil->pLeft;
	const OrderedNode* pFound = NULL;
	int val;

	while (pNode != pNil)
	{
		val = cmp(pData, pNode->pData);
		if (!val) pFound = pNode;

		if (val <= 0)
			pNode = pNode->pLeft;
		else
			pNode = pNode->pRight;
	}

	return (OrderedNode*) pFound;
}

/**
 * Attempts to locate the upper bound for data in the set.
 *
 * @param  cmp    Function to use to find a partial match.
 * @param  pData  Pointer to data to bound.
 *
 * @returns       Pointer to the set's node containing the last data lower or equal
 *                to the given data.
 */
OrderedNode* OrderedSet_UpperBound(const OrderedSet* This, OrderedSet_FCompare cmp, const void* pData)
{
	const OrderedNode* pNil = &This->NilNode;
	const OrderedNode* pNode = pNil->pLeft;
	const OrderedNode* pFound = NULL;
	int val;

	while (pNode != pNil)
	{
		val = cmp(pData, pNode->pData);
		if (!val) pFound = pNode;

		if (val < 0)
			pNode = pNode->pLeft;
		else
			pNode = pNode->pRight;
	}

	return (OrderedNode*) pFound;
}
