#pragma once

#include <queue>
#include <random>
#ifdef __WXMSW__
#include <xutility>
#endif

#include "flexilog.h"

namespace bTree
{

	/// Binary tree functions
	template <class TreeNodeType> TreeNodeType *BinTreeGetSibling(TreeNodeType *node)
	{
		/// Returns node's sibling, or nullptr if node has no parent
		/// This function assumes the tree is parent-child consistent, otherwise it produces undefined results

		if (!node)		return nullptr;

		TreeNodeType *parent = node->GetParent();

		if (parent)		return (parent->GetLChild() == node) ? (parent->GetRChild()) : (parent->GetLChild());
		else return nullptr;
	}
	template <class TreeNodeType> void BinTreeReplaceNode(TreeNodeType *what, TreeNodeType *place)
	{
		/// Replaces 'place' with 'what' and sets parents and child pointers
		/// This function assumes the tree is parent-child consistent, otherwise it produces undefined results

		if (!place)		return;

		TreeNodeType *parent = place->parent;
		if (parent)
		{
			place->parent = nullptr;

			if (what)	what->parent = parent;

			((parent->lChild == place) ? (parent->lChild) : (parent->rChild)) = what;
		}
	}
	template <class TreeNodeType> void BinTreeRemoveNode(TreeNodeType *node, TreeNodeType *& out_parent, TreeNodeType *& out_sibling, TreeNodeType *&in_out_root)
	{
		/// Removes node from tree. Node's sibling is placed at the position on node's parent, and node and its parent is removed.
		/// in_out_root is root of the tree containing node. If node is the root then in_out_root = 0;
		/// if node's parent is the root, then in_out_root = node's sibling, otherwise root is unchanged.
		/// This function assumes the tree is parent-child consistent, otherwise it produces undefined results

		if (!node)		return;

		assert(!in_out_root->parent);		// Root has no parent

		if (node == in_out_root)
		{
			assert(!node->parent);			// Node is root too, i.e. it has no parent
			out_parent = nullptr;
			out_sibling = nullptr;
			in_out_root = nullptr;
			return;
		}
		else
		{
			assert(node->parent);			// Node must have had a parent, because it wasn't the root

			out_sibling = bTree::BinTreeGetSibling(node);

			if (node->parent == in_out_root)
			{
				assert(!node->parent->parent);			// Parent is root too, i.e. it has no parent

				// Parent was root, so make sibling the root
				out_sibling->parent = nullptr;
				in_out_root = out_sibling;
			}
			else
			{
				assert(node->parent->parent);			// Parent is not root, it must have a parent

				// Replace parent with sibling
				bTree::BinTreeReplaceNode(out_sibling, node->parent);
			}

			out_parent = node->parent;
			out_parent->parent = nullptr;
			out_parent->lChild = nullptr;
			out_parent->rChild = nullptr;

			node->parent = nullptr;
		}
	}
	template <class TreeNodeType> void BinTreeInsertNode(TreeNodeType &what, TreeNodeType &place, TreeNodeType &aux, TreeNodeType *&in_out_root)
	{
		/// Inserts node 'what' into tree. 'aux' node is placed instead of node 'place', and 'what' and 'place' and made children of 'aux' node.
		/// If 'place' was 'in_out_root', 'in_out_root' is modified to point to 'aux', otherwise it is unchanged

		assert(!in_out_root->parent);		// Root has no parent

		if (!place.parent)
		{
			assert(in_out_root == &place);	// place is the root
			in_out_root = &aux;
		}
		else
		{
			assert(in_out_root != &place);	// place isn't the root so ...
			assert(place.parent);			// ... place must have a parent

			BinTreeReplaceNode(&aux, &place);
		}

		place.parent = &aux;
		what.parent = &aux;
		aux.lChild = &what;
		aux.rChild = &place;
	}

	template <class TreeNodeType> int BinTreeGetDepth(TreeNodeType *node)
	{
		/// Returns depth of binary
		return (node) ? (1 + std::max(BinTreeGetDepth<TreeNodeType>(node->GetLChild()), BinTreeGetDepth<TreeNodeType>(node->GetRChild()))) : 0;
	}
	template <class TreeNodeType> float BinTreeGetArea(TreeNodeType *node)
	{
		float area = node->GetAABB().SurfaceArea();
		if (!node->IsLeaf())
		{
			area += BinTreeGetArea<TreeNodeType>(node->GetLChild());
			area += BinTreeGetArea<TreeNodeType>(node->GetRChild());
		}
		return area;
	}
	inline float BinTreeDepthOfBalanced(int nodes_count)
	{
		/// Returns depth of balanced binary tree with given number of nodes
		return (nodes_count > 0) ? (logf(float(nodes_count)) / logf(2.0f) + 1.0f) : 0.0f;
	}

	/// Binary tree checks
	template <class TreeNodeType> TreeNodeType *BinTreeCheckNoOneChildNode(TreeNodeType *root)
	{
		// This function checks that the binary tree contains no node with only one child.
		// No binary tree have no node with only one child, therefore it passes this check.

		// Returns the node which fails the check. If the tree passes the check, returns nullptr.

		if (!root)		return nullptr;
		if ((!root->GetLChild()) != (!root->GetRChild()))		return root;

		TreeNodeType *failed_node = nullptr;
		if (nullptr != (failed_node = BinTreeCheckNoOneChildNode<TreeNodeType>(root->GetLChild())))		return failed_node;
		if (nullptr != (failed_node = BinTreeCheckNoOneChildNode<TreeNodeType>(root->GetRChild())))		return failed_node;

		return nullptr;
	}
	template <class TreeNodeType> TreeNodeType *BinTreeCheckAABBHierarchy(TreeNodeType *root)
	{
		// This function checks whether all nodes in binary tree satisfy that their aabb is a sum of their children aabb
		// No binary tree have no node that fails the check, therefore it passes this check.

		// Returns the node which fails the check. If the tree passes the check, returns nullptr.

		if (!root)		return nullptr;
		if (root->IsLeaf())		return nullptr;

		// Check this node
		VRUT::AABB aabborig(root->GetAABB());
		VRUT::AABB aabb;
		if (root->GetLChild())		aabb.Merge(root->GetLChild()->GetAABB());
		if (root->GetRChild())		aabb.Merge(root->GetRChild()->GetAABB());
		if (aabborig != aabb)		return root;

		// Check children
		TreeNodeType *failed_node = nullptr;

		TreeNodeType *l_child = root->GetLChild();
		if (nullptr != (failed_node = BinTreeCheckAABBHierarchy<TreeNodeType>(l_child)))	return failed_node;

		TreeNodeType *r_child = root->GetRChild();
		if (nullptr != (failed_node = BinTreeCheckAABBHierarchy<TreeNodeType>(r_child)))	return failed_node;

		return nullptr;
	}
	template <class TreeNodeType> TreeNodeType *BinTreeCheckParentChildNoLoopConsistencyCore(TreeNodeType *node)
	{
		// This function is the core of the check.
		// The point is that if the root is not on a loop, then some node on the loop must be parent-child inconsistent, since
		// it there are at least two other nodes having this node as child: previous node on the loop and previous node on the
		// path from root to this node. If the root is on a loop, then it has either no parent (and the previous node on the loop
		// is parent-child inconsistent), or it has a parent which must be checked by caller.

		if (!node)		return nullptr;

		TreeNodeType *l_child = node->GetLChild();
		if (l_child && (l_child->GetParent() != node))						return l_child;
		TreeNodeType *r_child = node->GetRChild();
		if (r_child && (r_child->GetParent() != node))						return r_child;

		TreeNodeType *failed_node = nullptr;
		if (nullptr != (failed_node = BinTreeCheckParentChildNoLoopConsistencyCore<TreeNodeType>(l_child)))	return failed_node;
		if (nullptr != (failed_node = BinTreeCheckParentChildNoLoopConsistencyCore<TreeNodeType>(r_child)))	return failed_node;

		return nullptr;
	}
	template <class TreeNodeType> TreeNodeType *BinTreeCheckParentChildNoLoopConsistency(TreeNodeType *root)
	{
		// This function checks whether all nodes in binary tree satisfy that parent of their children nodes is the node itself.
		// This function also checks whether the tree contains a loop or not and that root has no parent.
		// No binary tree have no node that fails the check, therefore it passes this check.

		// Returns the node which fails the check (whose child node's parent is not the node itself). Returns a node on loop if the tree
		// contains a loop. Returns root if the root has a parent. If the tree passes the check, returns nullptr.

		if (!root)		return nullptr;
		if (root->GetParent())
		{
			return root;		// Root must have no parent
		}

		return BinTreeCheckParentChildNoLoopConsistencyCore<TreeNodeType>(root);
	}
	template <class TreeNodeType> TreeNodeType *BinTreeCheckLeafHasDataConsistency(TreeNodeType *root)
	{
		// This function checks that each leaf has data and each internal node has no data.
		// No binary tree passes this check.

		// Returns the node which fails the check. If the tree passes the check, returns nullptr.

		if (!root)		return nullptr;

		if ((root->IsLeaf() && !root->HasData()) || (!root->IsLeaf() && root->HasData()))		return root;

		TreeNodeType *failed_node = nullptr;
		if (nullptr != (failed_node = BinTreeCheckLeafHasDataConsistency<TreeNodeType>(root->GetLChild())))		return failed_node;
		if (nullptr != (failed_node = BinTreeCheckLeafHasDataConsistency<TreeNodeType>(root->GetRChild())))		return failed_node;

		return nullptr;
	}

	/////////////////// Bin Tree Map
	template <class TreeNodeType, class MapFunc, class Stopper, class Container> void BinTreeMapWithStopperContainer(TreeNodeType *node, MapFunc &map_func, Stopper &stopper, Container &container)
	{
		do
		{
			if (node && !stopper.ShouldStop(node))
			{
				map_func.Call(node);

				container.push_back(node->GetRChild());
				node = node->GetLChild();
			}
			else
			{
				if (container.empty())		break;
				node = container.back();
				container.pop_back();
			}
		}
		while (true);			// Ends with break
	}
	template <class TreeNodeType, class MapFunc, class Stopper> void BinTreeMapWithStopperDeque(TreeNodeType *node, MapFunc &map_func, Stopper &stopper)
	{
		std::deque<TreeNodeType *> container;
		BinTreeMapWithStopperContainer<TreeNodeType, MapFunc, Stopper, std::deque<TreeNodeType *> > (node, map_func, stopper, container);
	}
	template <class TreeNodeType, class MapFunc, class Stopper> void BinTreeMapWithStopperVector(TreeNodeType *node, MapFunc &map_func, Stopper &stopper)
	{
		std::vector<TreeNodeType *> container;
		BinTreeMapWithStopperContainer<TreeNodeType, MapFunc, Stopper, std::vector<TreeNodeType *> > (node, map_func, stopper, container);
	}
	template <class TreeNodeType, class MapFunc, class Stopper> void BinTreeMapWithStopperVector(TreeNodeType *node, MapFunc &map_func, Stopper &stopper, unsigned int vector_reserve)
	{
		std::vector<TreeNodeType *> container;
		container.reserve(vector_reserve);
		BinTreeMapWithStopperContainer<TreeNodeType, MapFunc, Stopper, std::vector<TreeNodeType *> > (node, map_func, stopper, container);
	}

	template <class TreeNodeType, bool StopperFunc(TreeNodeType *)> class BinTreeStopperWrapper
	{
	public:
		inline bool ShouldStop(TreeNodeType *node)		{	return StopperFunc(node);		}
	};
	template <class TreeNodeType, class FuncType> class BinTreeMapFuncWrapperClass
	{
	protected:
		FuncType fnc;

	public:
		BinTreeMapFuncWrapperClass(FuncType _fnc): fnc(_fnc) {}

		void Call(TreeNodeType *node)	{	fnc(node);		}
	};

	template <class TreeNodeType, class FuncType> BinTreeMapFuncWrapperClass<TreeNodeType, FuncType> BinTreeMapFuncWrapper(FuncType fnc)
	{
		return BinTreeMapFuncWrapperClass<TreeNodeType, FuncType>(fnc);
	}
	
	template <class TreeNodeType> class BinTreeNoStopStopper
	{
	public:
		inline bool ShouldStop(TreeNodeType *)			{	return false;	}
	};

	template <class TreeNodeType, class MapFunc, class Container> void BinTreeMapContainer(TreeNodeType *node, MapFunc &map_func, Container &container)
	{
		BinTreeNoStopStopper<TreeNodeType> no_stopper;
		BinTreeMapWithStopperContainer<TreeNodeType, MapFunc, BinTreeNoStopStopper<TreeNodeType>, Container> (node, map_func, no_stopper, container);
	}
	template <class TreeNodeType, class MapFunc> void BinTreeMapDeque(TreeNodeType *node, MapFunc &map_func)
	{
		std::deque<TreeNodeType *> container;
		BinTreeMapContainer<TreeNodeType, MapFunc, std::deque<TreeNodeType *> > (node, map_func, container);
	}
	template <class TreeNodeType, class MapFunc> void BinTreeMapVector(TreeNodeType *node, MapFunc &map_func)
	{
		std::vector<TreeNodeType *> container;
		BinTreeMapContainer<TreeNodeType, MapFunc, std::vector<TreeNodeType *> > (node, map_func, container);
	}
	template <class TreeNodeType, class MapFunc> void BinTreeMapVector(TreeNodeType *node, MapFunc &map_func, unsigned int vector_reserve)
	{
		std::vector<TreeNodeType *> container;
		container.reserve(vector_reserve);
		BinTreeMapContainer<TreeNodeType, MapFunc, std::vector<TreeNodeType *> > (node, map_func, container);
	}

	////////////////// FOLD
	template <class TreeNodeType, typename CumulatorTargetType, typename CumulatorObjectType, void CumulatorFunc(CumulatorTargetType &, CumulatorObjectType), CumulatorObjectType TransformFunc(TreeNodeType *)>
	class BinTreeFoldWrapper
	{
	protected:
		CumulatorTargetType &cumulator;
	public:
		BinTreeFoldWrapper(CumulatorTargetType &_cumulator): cumulator(_cumulator) {}

		void Call(TreeNodeType *node)
		{
			CumulatorFunc(cumulator, TransformFunc(node));
		}
	};

	///////////////// FOLD - CUMULATOR FUNCTIONS
	template <class ValueType> inline void BinTreeCumulatorFuncAdd(ValueType &v1, ValueType v2)		{	v1 += v2;					}
	template <class ValueType> inline void BinTreeCumulatorFuncMin(ValueType &v1, ValueType v2)		{	v1 = std::min(v1, v2);		}
	template <class ArrayType, class ValueType> inline void BinTreeCumulatorFuncPushBack(ArrayType &arr, ValueType val)		{	if (val)		arr.push_back(val);		}
	template <class ArrayType, class ValueType> inline void BinTreeCumulatorFuncPush(ArrayType &arr, ValueType val)			{	if (val)		arr.push(val);			}

	///////////////// FOLD - TRANSTORM FUNCTIONS
	template <class TreeNodeType> inline TreeNodeType *BinTreeTransformNode(TreeNodeType *node)			{		return node;		}
	template <class TreeNodeType> inline TreeNodeType *BinTreeTransformLeafNode(TreeNodeType *node)		{		return (node && node->IsLeaf()) ? node : nullptr;			}
	template <class TreeNodeType> inline TreeNodeType *BinTreeTransformInternalNode(TreeNodeType *node)	{		return (node && node->IsInternal()) ? node : nullptr;		}

	template <class TreeNodeType> inline int BinTreeTransformNodeInt(TreeNodeType *node)				{		return (node) ? 1 : 0;		}
	template <class TreeNodeType> inline int BinTreeTransformLeafNodeInt(TreeNodeType *node)			{		return (node && node->IsLeaf()) ? 1 : 0;		}
	template <class TreeNodeType> inline int BinTreeTransformInternalNodeInt(TreeNodeType *node)		{		return (node && node->IsInternal()) ? 1 : 0;	}

	template <class TreeNodeType> inline float BinTreeTransformAABBSurfaceArea(TreeNodeType *node)		{		return node ? (node->GetAABB().SurfaceArea()) : 0.0f;		}

	///////////////// FOLD - PREDEFINED FOLDER CLASSES FOR NODES
	template <class TreeNodeType> class BinTreeFoldBasicWrappers
	{
	public:
		template <class TreeNodeArrayType> class GetAllNodes
		{
		public:
			typedef BinTreeFoldWrapper<TreeNodeType, TreeNodeArrayType, TreeNodeType *, BinTreeCumulatorFuncPushBack<TreeNodeArrayType, TreeNodeType *>, BinTreeTransformNode<TreeNodeType> >
				Wrapper;
		};
		template <class TreeNodeArrayType> class GetAllLeafNodes
		{
		public:
			typedef BinTreeFoldWrapper<TreeNodeType, TreeNodeArrayType, TreeNodeType *, BinTreeCumulatorFuncPushBack<TreeNodeArrayType, TreeNodeType *>, BinTreeTransformLeafNode<TreeNodeType> >
				Wrapper;
		};
		template <class TreeNodeArrayType> class GetAllInternalNodes
		{
		public:
			typedef BinTreeFoldWrapper<TreeNodeType, TreeNodeArrayType, TreeNodeType *, BinTreeCumulatorFuncPushBack<TreeNodeArrayType, TreeNodeType *>, BinTreeTransformInternalNode<TreeNodeType> >
				Wrapper;
		};

		typedef BinTreeFoldWrapper<TreeNodeType, int, int, BinTreeCumulatorFuncAdd<int>, BinTreeTransformNodeInt<TreeNodeType> >			CountNodes;
		typedef BinTreeFoldWrapper<TreeNodeType, int, int, BinTreeCumulatorFuncAdd<int>, BinTreeTransformLeafNodeInt<TreeNodeType> >		CountLeaves;
		typedef BinTreeFoldWrapper<TreeNodeType, int, int, BinTreeCumulatorFuncAdd<int>, BinTreeTransformInternalNodeInt<TreeNodeType> >	CountInternalNodes;
	};

	///////////////// FOLD - FUNCTIONS USING FOLDER
	template <class TreeNodeType, class TreeNodeArrayType> void BinTreeGetAllNodes(TreeNodeType *node, TreeNodeArrayType &in_out_array)
	{
		/// Inserts node and all its children into in_out_array (nullptr as node or child is not inserted). The order in which
		/// nodes and their children are inserted is not specified.
		typename BinTreeFoldBasicWrappers<TreeNodeType>::template GetAllNodes<TreeNodeArrayType>::Wrapper fnc(in_out_array);			// If this fails, add typename
		BinTreeMapDeque(node, fnc);
	}
	template <class TreeNodeType, class TreeNodeArrayType> void BinTreeGetAllLeafNodes(TreeNodeType *node, TreeNodeArrayType &in_out_array)
	{
		/// Inserts all leaf nodes of node subtree into in_out_array (nullptr as child is not inserted). The order in which
		/// nodes and their children are inserted is not specified.
		typename BinTreeFoldBasicWrappers<TreeNodeType>::template GetAllLeafNodes<TreeNodeArrayType>::Wrapper fnc(in_out_array);			// If this fails, add typename
		BinTreeMapDeque(node, fnc);
	}
	template <class TreeNodeType, class TreeNodeArrayType> void BinTreeGetAllInternalNodes(TreeNodeType *node, TreeNodeArrayType &in_out_array)
	{
		/// Goes through node and its subtree and inserts all internal nodes into in_out_array. The order in which
		/// nodes and their children are inserted is not specified.
		typename BinTreeFoldBasicWrappers<TreeNodeType>::template GetAllInternalNodes<TreeNodeArrayType>::Wrapper fnc(in_out_array);			// If this fails, add typename
		BinTreeMapDeque(node, fnc);
	}

	template <class TreeNodeType> int BinTreeCountNodes(TreeNodeType *node)
	{
		/// Returns number of nodes in binary tree
		int val = 0;
		typename BinTreeFoldBasicWrappers<TreeNodeType>::CountNodes fnc(val);
		BinTreeMapDeque(node, fnc);
		return val;
	}
	template <class TreeNodeType> int BinTreeCountLeaves(TreeNodeType *node)
	{
		/// Returns number of leaves in binary tree
		int val = 0;
		typename BinTreeFoldBasicWrappers<TreeNodeType>::CountLeaves wrapper(val);
		BinTreeMapDeque(node, wrapper);
		return val;
	}
	template <class TreeNodeType> int BinTreeCountInternalNodes(TreeNodeType *node)
	{
		/// Returns number of internal nodes in binary tree
		int val = 0;
		typename BinTreeFoldBasicWrappers<TreeNodeType>::CountInternalNodes wrapper(val);
		BinTreeMapDeque(node, wrapper);
		return val;
	}

	template <class TreeNodeType> float BinTreeSumAABBOfSubTrees(TreeNodeType *node)
	{
		float val = 0.0;
		BinTreeMapDeque(node, BinTreeFoldWrapper<TreeNodeType, float, float, BinTreeCumulatorFuncAdd<float>, BinTreeTransformAABBSurfaceArea<TreeNodeType> >(val));
		return val;
	}
	template <class TreeNodeType> float BinTreeMinAABBOfSubTrees(TreeNodeType *node)
	{
		float val = 0.0f;
		BinTreeMapDeque(node, BinTreeFoldWrapper<TreeNodeType, float, float, BinTreeCumulatorFuncMin<float>, BinTreeTransformAABBSurfaceArea<TreeNodeType> >(val));
		return val;
	}

	///////////////// OTHER FUNCTIONS
	template <class TreeNodeType> class NodeFloatPairing
	{
	public:
		struct NodeFloatPair: public std::pair<TreeNodeType*, float>
		{
			operator bool()		{	return (this->first != nullptr);		}
			
			NodeFloatPair(TreeNodeType* _first = nullptr, float _second = 0.0f): std::pair<TreeNodeType*, float>(_first, _second) {}
		};
		class NodeFloatComparatorPreferLarger
		{
		public:
			bool operator()(NodeFloatPair &a1, NodeFloatPair &a2)
			{
				return a1.second < a2.second;				// Nodes with larger value have greater priority
			}
		};
		class NodeFloatComparatorPreferSmaller
		{
		public:
			bool operator()(NodeFloatPair &a1, NodeFloatPair &a2)
			{
				return a1.second > a2.second;				// Nodes with smaller value have greater priority
			}
		};

		typedef std::priority_queue<NodeFloatPair, std::vector<NodeFloatPair>,  NodeFloatComparatorPreferLarger> NodeFloatPQPreferLarger;
		typedef std::priority_queue<NodeFloatPair, std::vector<NodeFloatPair>,  NodeFloatComparatorPreferSmaller> NodeFloatPQPreferSmaller;
	};
	template <class TreeNodeType> class NodeIntPairing
	{
	public:
		struct NodeIntPair: public std::pair<TreeNodeType*, int>
		{
			operator bool()		{	return (this->first != nullptr);		}
			
			NodeIntPair(TreeNodeType* _first = nullptr, int _second = 0): std::pair<TreeNodeType*, int>(_first, _second) {}
		};
		class NodeIntComparatorPreferLarger
		{
		public:
			bool operator()(NodeIntPair &a1, NodeIntPair &a2)
			{
				return a1.second < a2.second;				// Nodes with larger value have greater priority
			}
		};
		class NodeIntComparatorPreferSmaller
		{
		public:
			bool operator()(NodeIntPair &a1, NodeIntPair &a2)
			{
				return a1.second > a2.second;				// Nodes with smaller value have greater priority
			}
		};

		typedef std::priority_queue<NodeIntPair, std::vector<NodeIntPair>,  NodeIntComparatorPreferLarger> NodeIntPQPreferLarger;
		typedef std::priority_queue<NodeIntPair, std::vector<NodeIntPair>,  NodeIntComparatorPreferSmaller> NodeIntPQPreferSmaller;
	};
	template <class TreeNodeType, class _NodeFloatPairing> void BinTreeInsertNodeValid(TreeNodeType *node, TreeNodeType *aux, TreeNodeType *&root)
	{
		if (!root)
		{
			root = node;
			SAFE_DELETE(aux);
			return;
		}
		root->UpdateBVSubtree();
		node->UpdateBVSubtree();
		TreeNodeType *bestNode = root;						// A.k.a. 'V' in Bittner's algorithm
		float bestCost = root->GetAABB().SurfaceArea();		// A.k.a. 'Vcost' in Bittner's algorithm
		VRUT::AABB node_aabb = node->GetAABB();
		float node_area = node_aabb.SurfaceArea();			// A.k.a. 'SA(box(N))' in Bittner's algorithm

		std::vector<typename _NodeFloatPairing::NodeFloatPair> PQvector;
		PQvector.reserve(100);
		typename _NodeFloatPairing::NodeFloatPQPreferSmaller PQ(typename _NodeFloatPairing::NodeFloatComparatorPreferSmaller(), std::move(PQvector));
		//typename _NodeFloatPairing::NodeFloatPQPreferSmaller PQ;
		//typename _NodeFloatPairing::NodeFloatPQPreferLarger PQ;

		typename _NodeFloatPairing::NodeFloatPair currentEntry;
		PQ.push(typename _NodeFloatPairing::NodeFloatPair(root, 0.0f));
		while (!PQ.empty()) {
			currentEntry = PQ.top();
			PQ.pop();

			if (currentEntry.second + node_area >= bestCost)
				break;

			float cost = currentEntry.second + VRUT::AABB::MergedSurfaceArea(node_aabb, currentEntry.first->GetAABB());

			if (cost < bestCost)
			{
				bestCost = cost;
				bestNode = currentEntry.first;
			}

			if (!currentEntry.first->IsLeaf())
			{
				float cost_diff = cost - currentEntry.first->GetAABB().SurfaceArea();
				if (cost_diff + node_area < bestCost)
				{
					PQ.push(typename _NodeFloatPairing::NodeFloatPair(currentEntry.first->GetLChild(), cost_diff));
					PQ.push(typename _NodeFloatPairing::NodeFloatPair(currentEntry.first->GetRChild(), cost_diff));
				}
			}
		}

		assert(bestNode);		// Some best node must exist

		bTree::BinTreeInsertNode(*node, *bestNode, *aux, root);
		node->UpdateBVUptree();
	}
	template <class TreeNodeType, class _NodeIntPairing> void BinTreeInsertNodeInvalid(TreeNodeType *node, TreeNodeType *aux, TreeNodeType *&root)
	{
		if (!root)
		{
			root = node;
			SAFE_DELETE(aux);
			return;
		}
		root->UpdateBVSubtree();
		node->UpdateBVSubtree();
		TreeNodeType * bestNode = nullptr;
		TreeNodeType * currentEntry;
		std::queue<TreeNodeType*> Q;
		Q.push(root);
		while (!Q.empty()) {
			currentEntry = Q.front();
			Q.pop();
			if (currentEntry->IsLeaf())
			{
				bestNode = currentEntry;
				break;
			}
			else
			{
				Q.push(currentEntry->GetLChild());
				Q.push(currentEntry->GetRChild());
			}
		}
		assert(bestNode);		// Some best node must exist
		bTree::BinTreeInsertNode(*node, *bestNode, *aux, root);
		//node->UpdateBVUptree();
	}
	template <class TreeNodeType> void BinTreeInsertNodeOptim(TreeNodeType *node, TreeNodeType *aux, TreeNodeType *&root)
	{
		if (!node->GetAABB().IsValid())
			BinTreeInsertNodeInvalid<TreeNodeType, NodeFloatPairing<TreeNodeType> >(node, aux, root);
		else
			BinTreeInsertNodeValid<TreeNodeType, NodeFloatPairing<TreeNodeType> >(node, aux, root);
	}

	///////////////// Optimize - needs TreeNodeType::UpdateBVUptree

	template <class Getter, class TreeNodeType> class NodeGetterBase
	{
	public:
		// Define "static float Getter::GetNodeValue(GeometryTreeNode *node)"

		typedef NodeFloatPairing<TreeNodeType> TNNodeFloatPairing;

		static typename TNNodeFloatPairing::NodeFloatPair TransformFunc(TreeNodeType *node)
		{
			if (!node || !node->IsInternal())
			{
				return typename TNNodeFloatPairing::NodeFloatPair(nullptr, 0.0f);
			}
			else
			{
				return typename TNNodeFloatPairing::NodeFloatPair(node, Getter::GetNodeValue(node));
			}
		}

		static void GetNodes(TreeNodeType *root, typename TNNodeFloatPairing::NodeFloatPQPreferLarger &out_nodes)
		{
			BinTreeFoldWrapper<TreeNodeType, typename TNNodeFloatPairing::NodeFloatPQPreferLarger, typename TNNodeFloatPairing::NodeFloatPair,
				BinTreeCumulatorFuncPush<typename TNNodeFloatPairing::NodeFloatPQPreferLarger, typename TNNodeFloatPairing::NodeFloatPair>,
				NodeGetterBase<Getter, TreeNodeType>::TransformFunc> fnc(out_nodes);
			BinTreeMapDeque(root, fnc);

			//bTree::BinTreeFold<TreeNodeType, typename TNNodeFloatPairing::NodeFloatPQPreferLarger,
			//	bTree::BinTreeValueFuncPush<typename TNNodeFloatPairing::NodeFloatPQPreferLarger, typename TNNodeFloatPairing::NodeFloatPair>,
			//	NodeGetterBase<Getter, TreeNodeType> >(root, out_nodes);
		}

	};
	template <class TreeNodeType> class MsumGetter: public NodeGetterBase<MsumGetter<TreeNodeType>, TreeNodeType>
	{
	public:
		static float GetNodeValue(TreeNodeType *node)
		{
			if (!node || !node->GetLChild() || !node->GetRChild())		return 0.0f;

			float node_sa = node->GetAABB().SurfaceArea();
			float l_child_sa = node->GetLChild()->GetAABB().SurfaceArea();
			float r_child_sa = node->GetRChild()->GetAABB().SurfaceArea();

			return node_sa / (l_child_sa + r_child_sa) * 2.0f;
		}
	};
	template <class TreeNodeType> class MminGetter: public NodeGetterBase<MminGetter<TreeNodeType>, TreeNodeType>
	{
	public:
		static float GetNodeValue(TreeNodeType *node)
		{
			if (!node || !node->GetLChild() || !node->GetRChild())		return 0.0f;

			float node_sa = node->GetAABB().SurfaceArea();
			float l_child_sa = node->GetLChild()->GetAABB().SurfaceArea();
			float r_child_sa = node->GetRChild()->GetAABB().SurfaceArea();

			return node_sa / std::min(l_child_sa, r_child_sa);
		}
	};
	template <class TreeNodeType> class MareaGetter: public NodeGetterBase<MareaGetter<TreeNodeType>, TreeNodeType>
	{
	public:
		static float GetNodeValue(TreeNodeType *node)
		{
			if (!node || !node->GetLChild() || !node->GetRChild())		return 0.0f;

			float node_aabb = node->GetAABB().SurfaceArea();
			return node_aabb;
		}
	};
	template <class TreeNodeType> class McombGetter: public NodeGetterBase<McombGetter<TreeNodeType>, TreeNodeType>
	{
	public:
		static float GetNodeValue(TreeNodeType *node)
		{
			return MsumGetter<TreeNodeType>::GetNodeValue(node) * MminGetter<TreeNodeType>::GetNodeValue(node) * MareaGetter<TreeNodeType>::GetNodeValue(node);
		}
	};
	template <class TreeNodeType> class MrandomGetter: public NodeGetterBase<MrandomGetter<TreeNodeType>, TreeNodeType>
	{
	public:
		static float GetNodeValue(TreeNodeType *node)
		{
			/*static thread_local*/ std::mt19937 generator;
			std::uniform_int_distribution<int> distribution(0.0f, 100.0f);
			return distribution(generator);
			// TODO: Use C++ 11 random generators globally??
			// FIXME: This is not thred-safe!
			//return random(0.0f, 100.0f);
		}
	};

	template <class TreeNodeType, class NodeGetter> void BinTreeOptimizeIteration(TreeNodeType *& root, int ratio)
	{
		if (bTree::BinTreeCountLeaves(root) < 4)		return;		// Too few nodes to optimize

		int internal_nodes_count = bTree::BinTreeCountInternalNodes(root);
		assert(internal_nodes_count >= 2);			// If number of leaves is at least 4, number of internal nodes should be at least 2

		// Select nodes to remove - part 3 of Bittner's algorithm
		typename NodeFloatPairing<TreeNodeType>::NodeFloatPQPreferLarger nodes_to_remove;
		NodeGetter::GetNodes(root, nodes_to_remove);

		for (int i = 0; i < internal_nodes_count/ratio; ++i)
		{
			// Part 4 of Bittner's algorithm - for each selected node
			TreeNodeType *node_to_remove = nullptr;
			do
			{
				if (!nodes_to_remove.empty())
				{
					node_to_remove = nodes_to_remove.top().first;
					nodes_to_remove.pop();
				}
				else
				{
					node_to_remove = nullptr;
					break;
				}
			} while (node_to_remove == root);			// I don't want root
			if (!node_to_remove)
			{
				break;			// No node left to remove
			}
			
			// Part 4a of Bittner's algorithm - remove selected node
			TreeNodeType *parent, *sibling;
			bTree::BinTreeRemoveNode(node_to_remove, parent, sibling, root);
			assert(parent);
			
			TreeNodeType *l_child_subtree = node_to_remove->lChild;
			TreeNodeType *r_child_subtree = node_to_remove->rChild;
			node_to_remove->lChild = nullptr;
			node_to_remove->rChild = nullptr;

			TreeNodeType *aux1 = parent;
			TreeNodeType *aux2 = node_to_remove;

			// Since there is no node in the tree that has only one child, every node which that has a parent has a sibling
			assert(sibling);

			if (sibling->GetParent())			sibling->GetParent()->UpdateBVUptree();					// Update BVH of new tree

			// Part 4b and 4c of Bittner's algorithm - insert selected node
			bTree::BinTreeInsertNodeOptim<TreeNodeType>(l_child_subtree, aux1, root);
			aux1->UpdateBVUptree();
			bTree::BinTreeInsertNodeOptim<TreeNodeType>(r_child_subtree, aux2, root);
			aux2->UpdateBVUptree();
		}
	}
	template <class TreeNodeType, class NodeGetter> void BinTreeOptimize(TreeNodeType *& root, int ratio, int maxIterations, int threshold, int randomThreshold)
	{
		if (!root)
			return;
		int i = 0;
		int j = 0;
		float bestArea = FLT_MAX;
		while (i < threshold && j < maxIterations)
		{
			BinTreeOptimizeIteration<TreeNodeType, NodeGetter>(root, ratio);
			float area = BinTreeGetArea<TreeNodeType>(root);
			if (area < bestArea)
			{
				bestArea = area;
				i = 0;
			}
			else
				++i;
			++j;
		} 
		i = 0;
		j = 0;
		while (i < randomThreshold && j < maxIterations)
		{
			BinTreeOptimizeIteration<TreeNodeType, MrandomGetter<TreeNodeType> >(root, ratio);
			float area = BinTreeGetArea<TreeNodeType>(root);
			if (area < bestArea)
			{
				bestArea = area;
				i = 0;
			}
			else
				++i;
			++j;
		} 
	}
}
