/*
* $Id: bvh.h 4550 2017-06-01 13:39:14Z antonin.misek-skoda-auto.cz $
*
* Description  : One line description of file.
* Author       : Vaclav Kyba <mail/Jabber: vaseo1@gmail.com>  <ICQ: 98576293>
* 
* Purpose      :
*     Long description of what the file is for.
*/


#ifndef __BVH__H__
#define __BVH__H__

#include "VrutApi.h"

#include <wx/hashmap.h>

#include <map>
#include <deque>
#include <vector>
#include <queue>

#include "smartptr.h"
#include "3dmath.h"

#include "camera.h"
#include "geometrynode.h"
#include "kernel.h"
#include "scene.h"

#include "rwlock.h"
#include "moddefs.h"
#include "VrutApi.h"

#include "BinTreesBVHBase.h"
#include "BinTreesBuilders.h"
#include "BinTreesCulling.h"


namespace VRUT
{
	class Scene;
	class Camera;
	struct RayIntersectionInfo;

	template <class SceneType> class BVHSceneTmpl;
	template <class SceneType> class BVHNodeSceneTmpl;
	template <class SceneType> class BVHNodeSplitterSceneTmpl;
	// Originaly there were non-template classes which were using VRUT::smart_pointer_v2<VRUT::Scene>.
	// They are replaced by templates with SceneType parameter.
	// The typedefs are here for the backward compatibility.
	typedef BVHSceneTmpl<VRUT::smart_pointer_v2<VRUT::Scene>> BVH;
	typedef BVHNodeSceneTmpl<VRUT::smart_pointer_v2<VRUT::Scene>> BVHNode;
	typedef BVHNodeSplitterSceneTmpl<VRUT::smart_pointer_v2<VRUT::Scene>> BVHNodeSplitter;

	template <class SceneType> 
	class /*VRUT_CORE_API*/ BVHNodeSceneTmpl: public bTree::BVHNodeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>>
	{
	protected:
		// Creates internal node
		BVHNodeSceneTmpl(BVHSceneTmpl<SceneType> *_tree, BVHNodeSceneTmpl<SceneType> *parent);
		// Creates leaf node
		BVHNodeSceneTmpl(
			BVHSceneTmpl<SceneType> *_tree, BVHNodeSceneTmpl<SceneType> *parent,
			typename bTree::BVHNodeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>>::BVHNodeBaseCreateData &data);

		friend class BVHSceneTmpl<SceneType>;			// Needs access to constructors
		friend class bTree::BVHTreeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>, SceneType>;	// Needs access to constructors

	public:
		/// unique ID of the node in the BVH - used for various lookups 
		// TODO Is this necessary? RESOLVED No, only CHCrenderer and BVH::Check uses it.
		int id;

		///Class destructor
		~BVHNodeSceneTmpl();

		///Is node valid?
		bool IsValid() const
		{
			return !this->IsAABBDirty();
		}
		///Get last out flag
		unsigned char &LastOut()
		{
			// TODO This method is used only in CHCrenderer
			return this->PerCameraLastCullingPlaneIndex[0];
		}

		///Get associated scene node's ID, note that this ID may change (if ID of geometry node changes)
		NODE_ID GetSceneNodeID() const
		{
			// TODO Consider renaming to this method GetGeometryNodeID or removing this method
			return this->GetGeomNode() ? this->GetGeomNode()->GetID() : NODE_ID_NONE;
		}

		///Get string with node's properties
		wxString ToString() const
		{
			return wxString::Format(wxT("BVHNode id: %i\n"), id)+this->aabb.ToString()+wxString::Format(wxT("\nSceneNode ID: %lu\nLChild: %i, RChild: %i"),
					(unsigned long)GetSceneNodeID(), (this->lChild)?this->lChild->id:-1, (this->rChild)?this->rChild->id:-1);
		}

		//------------- New methods
		float GetCost() const
		{
			// Use implementation of old BVH. Another implementation is in collision detection and in render4x
			const float ci = 2.0f;
			const float ct = 3.0f;

			if (this->IsLeaf())
			{
				return ci;
			}
			else
			{
				if (!this->GetAABB().IsValid())
				{
					return 0.0f;
				}
				else
				{
					float sa = this->GetAABB().SurfaceArea();
					if (sa == 0.0f)
					{
						return FLT_MAX;
					}
					else
					{
						return ct + (
							this->GetLChild()->GetAABB().SurfaceArea() * this->GetLChild()->GetCost() + 
							this->GetRChild()->GetAABB().SurfaceArea() * this->GetRChild()->GetCost()
							) / sa;
					}
				}
			}
		}

		inline BVHNodeSceneTmpl<SceneType> *CreateLeftInternal();
		inline BVHNodeSceneTmpl<SceneType> *CreateRightInternal();
		inline BVHNodeSceneTmpl<SceneType> *CreateLeftLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data);
		inline BVHNodeSceneTmpl<SceneType> *CreateRightLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data);

		//const VRUT::Material *GetMaterial() const		{	return geom_node ? geom_node->GetMaterial().getPtr() : nullptr;		}

		bool HasData() const			{	return this->hasData();			}
		void UpdateBVNode()				{	this->updateBVNode();		}
		static void UpdateBVNodeStatic(BVHNodeSceneTmpl *node)	{	node->UpdateBVNode();		}
	};
	
	// TODO Add types of BVH
	// TODO Check whether the tree is really thread-safe
	template <class SceneType> 
	class /*VRUT_CORE_API*/ BVHSceneTmpl: public bTree::BVHTreeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>, SceneType>
	{
	public:
		struct ItemInfo
		{
			float distance;
			smart_pointer<GeometryNode> geometryNode;
			Matrix matrix;
			ItemInfo(float distance, smart_pointer<GeometryNode> geometryNode, const Matrix &matrix) : distance(distance), geometryNode(geometryNode), matrix(matrix)
			{
			}

			class ItemInfoPreferCloser
			{
			public:
				bool operator() (const ItemInfo &p1, const ItemInfo &p2)		{		return p1.distance < p2.distance;	}
			};

			static bool SortByMaterialID(const ItemInfo & p1, const ItemInfo & p2) {
				return (p1.geometryNode->GetMaterialID()<p2.geometryNode->GetMaterialID());
			}
		};
		typedef std::vector<ItemInfo> ItemsList;

		enum BVH_TYPE {
			BVH_FAST,
			BVH_NORMAL,
			BVH_BEST,
		};

	protected:
		ReadWriteMutexOptional bvhMutex;

		void removeNodeFromMap(BVHNodeSceneTmpl<SceneType> *node)
		{
			// TODO Should this method lock mutexes? Aren't they already locked? Are they always locked?
			this->removeNodeFromMap_internal(node);
		}

		BVHNodeSceneTmpl<SceneType> *createLeftInternal(BVHNodeSceneTmpl<SceneType> *parent);
		BVHNodeSceneTmpl<SceneType> *createRightInternal(BVHNodeSceneTmpl<SceneType> *parent);
		BVHNodeSceneTmpl<SceneType> *createLeftLeaf(BVHNodeSceneTmpl<SceneType> *parent, typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data);
		BVHNodeSceneTmpl<SceneType> *createRightLeaf(BVHNodeSceneTmpl<SceneType> *parent, typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data);

		void logStats() const;

		//------- Node id's group - a group of attributes and methods to handle node's id
		int minUnassignedId;		// Minimal id, which is not assigned yet
		void reassignNodeIds();		// Reassigns ids to all nodes
		int getNewNodeId()		{	return minUnassignedId++;		}

	public:

		//------ New methods

		/// Optimizes the tree
		template <class NodeGetter> void OptimizeIteration(int ratio)
		{
			// Note: THIS IS NOT THREAD-SAFE
			bTree::BinTreeOptimizeIteration<BVHNodeSceneTmpl<SceneType>, NodeGetter>(this->root, ratio);
		}

		template <class NodeGetter> void Optimize(int ratio, int maxIterations, int threshold, int randomThreshold)
		{
			// Note: THIS IS NOT THREAD-SAFE
			bTree::BinTreeOptimize<BVHNodeSceneTmpl<SceneType>, NodeGetter>(this->root, ratio, maxIterations, threshold, randomThreshold);
		}

		//void Optimize(int ratio)
		//{
		//	// Note: THIS IS NOT THREAD-SAFE
		//	bTree::BinTreeOptimize<BVHNodeSceneTmpl<SceneType>, bTree::MsumGetter<BVHNodeSceneTmpl<SceneType>> >(root, ratio);
		//}

		BVHNodeSceneTmpl<SceneType> *CreateRootInternal();
		BVHNodeSceneTmpl<SceneType> *CreateRootLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data);

		void RemoveSafe(BVHNodeSceneTmpl<SceneType> *node, NODE_ID nodeIDHint = NODE_ID_NONE)
		{
			auto removed = Remove(node, nodeIDHint);
			SAFE_DELETE(removed);
		}
		void RemoveSafe(NODE_ID nodeID)
		{
			auto removed = Remove(nodeID);
			SAFE_DELETE(removed);
		}
		void DeleteRemovedNode(BVHNodeSceneTmpl<SceneType> *node, NODE_ID nodeID)
		{
			CREATE_WRITE_LOCKER(locker, bvhMutex);
			if (node)
			{
				this->removeNodeFromMapByID_internal(nodeID);
				node->geom_node.assign(NULL);
				delete node;
			}
		}

		//------ Old methods

		///Class constructor
		BVHSceneTmpl(SceneType &scene, BVH_TYPE type, bool useLock=true, bool skipBuild=false);

		///Class destructor
		~BVHSceneTmpl()
		{
			this->deleteTree_internal();
		}

		/// Get maximal assigned ID of BVH node, used in RenderGLModule.cpp (and maybe elsewhere)
		int GetNumberOfNodes()
		{
			return minUnassignedId;
		}

		///Get BVH node assigned to given scene node
		BVHNodeSceneTmpl<SceneType> *GetBVHNodeAssignedTo(NODE_ID sceneNodeID)
		{
			CREATE_READ_LOCKER(locker, bvhMutex);
			return this->getNodeByGeometryNodeID_internal(sceneNodeID);
		}
		const BVHNodeSceneTmpl<SceneType> *GetBVHNodeAssignedTo(NODE_ID sceneNodeID) const
		{
			CREATE_READ_LOCKER(locker, bvhMutex);
			return this->getNodeByGeometryNodeID_internal(sceneNodeID);
		}

		// Destroys the tree using locks
		void Destroy()
		{
			bvhMutex.WriteLock();
			this->deleteTree_internal();
			bvhMutex.WriteUnlock();
		}

		///Completely rebuild BVH
		void Rebuild(int ratio, int maxIterations, int threshold, int randomThreshold);

		///Remove given node from BVH
		///\return Node detached from scene graph
		// TODO What is the returned value good for?
		BVHNodeSceneTmpl<SceneType> * Remove(BVHNodeSceneTmpl<SceneType> * node, NODE_ID nodeIDHint=NODE_ID_NONE);

		///Remove node given by nodeID from BVH
		///\return Node detached from scene graph
		// TODO What is the returned value good for?
		BVHNodeSceneTmpl<SceneType> * Remove(NODE_ID nodeID);

		///Insert GeometryNodes by using optInsertNode
		void Insert(std::vector<smart_pointer<GeometryNode> > &items);

		///Insert GeometryNode by using optInsertNode
		void Insert(smart_pointer<GeometryNode> &node);

		///Build BVH from given geometry list
		void Build(std::vector<smart_pointer<GeometryNode> > &items);

		///Traverse BVH and update geometry in viewing frustum given by camera
		///\param[in] camera Use viewing frustum of given camera, camera should be updated
		///\param[out] itemsList List of geometry nodes inside/intersecting VF will be added
		///\param[out] blendItemsList List of geometry nodes inside/intersecting VF with transparency will be added
		void Cull(Scene *camera_scene, smart_pointer<Camera> &camera, float aspectRatio, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList);

		///Traverse BVH and update geometry in viewing frustum given by planes
		void Cull(const bTree::CullingPlanes &planes, const BSphere &cameraBsphere, const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList);
		
		///Update given \c BVHNode and connected \c SceneNode if not valid
		///\param[in] node BVH node to be updated, if \c NULL whole \c BVH is updated
		void UpdateBV(BVHNodeSceneTmpl<SceneType> * node = nullptr)
		{
			bvhMutex.ReadLock();
			this->updateTree_internal(node ? node : this->GetRoot(), 50);
			bvhMutex.ReadUnlock();
		}

		///Update the bounding volumes of a given node and of its parents
		///useful when full update is not necessary
		//void UpdateBVPartial(BVHNodeSceneTmpl<SceneType> * node);			// Noone uses this

		///Set node and its supernodes invalid
		void SetInvalid(NODE_ID nodeID);

		///Set node and its supernodes invalid
		void SetInvalid(BVHNodeSceneTmpl<SceneType> *node);

		///Check validity of BVH
		void Check(const wxString &place, bool reportInvalid=false) const;

		///Get string with node's properties
		wxString ToString() const;

		friend class BVHNodeSceneTmpl<SceneType>;
	};

	template<class SceneType>
	inline BVHNodeSceneTmpl<SceneType> *BVHNodeSceneTmpl<SceneType>::CreateLeftInternal()										
	{
		return this->tree->createLeftInternal(this);			
	}

	template<class SceneType>
	inline BVHNodeSceneTmpl<SceneType> *BVHNodeSceneTmpl<SceneType>::CreateRightInternal()
	{
		return this->tree->createRightInternal(this);			
	}

	template<class SceneType>
	inline BVHNodeSceneTmpl<SceneType> *BVHNodeSceneTmpl<SceneType>::CreateLeftLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data)
	{
		return this->tree->createLeftLeaf(this, data);		
	}

	template<class SceneType>
	inline BVHNodeSceneTmpl<SceneType> *BVHNodeSceneTmpl<SceneType>::CreateRightLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data)
	{
		return this->tree->createRightLeaf(this, data);
	}

	template<class SceneType>
	class BVHNodeSplitterSceneTmpl
	{
	protected:
		Vector3			cameraPosition;
		typename BVHSceneTmpl<SceneType>::ItemsList	&solidNodes;
		typename BVHSceneTmpl<SceneType>::ItemsList	&transparentNodes;
		typename BVHSceneTmpl<SceneType>::ItemsList	&reflectiveNodes;
	public:
		BVHNodeSplitterSceneTmpl(Vector3 _cameraPosition, typename BVHSceneTmpl<SceneType>::ItemsList &_solidNodes, typename BVHSceneTmpl<SceneType>::ItemsList &_transparentNodes, typename BVHSceneTmpl<SceneType>::ItemsList &_reflectiveNodes)
			: cameraPosition(_cameraPosition), solidNodes(_solidNodes), transparentNodes(_transparentNodes), reflectiveNodes(_reflectiveNodes) {}

		void InsertSubTree(const BVHNodeSceneTmpl<SceneType> *node)
		{
			assert(node);
			auto fnc = bTree::BinTreeMapFuncWrapper<const BVHNodeSceneTmpl<SceneType>>([this](const BVHNodeSceneTmpl<SceneType> *node) { if (node && node->IsLeaf())	this->InsertLeaf(node); });
			bTree::BinTreeMapVector(node, fnc, 50);
		}

		void InsertLeaf(const BVHNodeSceneTmpl<SceneType> *node)
		{
			LOGWARNING(wxT("BVHNodeSplitterSceneTmpl::InsertLeaf is not implemented"));
			/*assert(node && node->IsLeaf() && node->GetGeomNode());			// Each leaf must have geometry node

			if (!node->GetGeomNode()->IsActive())
				return;			// Geometry node is not active, there is no need to draw the node

//			BVH::ItemInfo to_insert((cameraPosition - node->GetBSphere().Center).LengthSq(), node->GetGeomNodeSmtPtr(), node->GetWorldMatrix());
			BVH::ItemInfo to_insert((cameraPosition - node->GetBSphere().Center).LengthSq(), nullptr, node->GetWorldMatrix());

			const VRUT::Material *mat = node->GetMaterial();
			if (mat && (mat->flags & Material::BLENDED))
			{
				transparentNodes.push_back(to_insert);
				transparentNodes.back().geometryNode.assign(node->GetGeomNodeSmtPtr());
			}
			else if (mat && (mat->flags & Material::FLAT_REFLECTION))
			{
				reflectiveNodes.push_back(to_insert);
				reflectiveNodes.back().geometryNode.assign(node->GetGeomNodeSmtPtr());
			}
			else
			{
				solidNodes.push_back(to_insert);
				solidNodes.back().geometryNode.assign(node->GetGeomNodeSmtPtr());
			}*/
		}

	};

	template<class SceneType>
	const BVHNodeSceneTmpl<SceneType>* BVHTreeCheckValidID(const BVHNodeSceneTmpl<SceneType> *root)
	{
		// This function checks that each node has valid id.
		// 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->id < 0)		return root;

		const BVHNodeSceneTmpl<SceneType> *failed_node = nullptr;
		if (nullptr != (failed_node = BVHTreeCheckValidID(root->GetLChild())))		return failed_node;
		if (nullptr != (failed_node = BVHTreeCheckValidID(root->GetRChild())))		return failed_node;

		return nullptr;
	}

	/***********************
	****    BVH NODE    ****
	***********************/

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType>::BVHNodeSceneTmpl(BVHSceneTmpl<SceneType> *_tree, BVHNodeSceneTmpl<SceneType> *parent)
		: bTree::BVHNodeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>>(_tree, parent), id(-1)
	{
		assert(this->tree);
		id = this->tree->getNewNodeId();

		this->PerCameraLastCullingPlaneIndex.resize(1);
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType>::BVHNodeSceneTmpl(
		BVHSceneTmpl<SceneType> *_tree, BVHNodeSceneTmpl<SceneType> *parent,
		typename bTree::BVHNodeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>>::BVHNodeBaseCreateData &data)
		: bTree::BVHNodeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>>(_tree, parent, data), id(-1)
	{
		assert(this->tree);
		id = this->tree->getNewNodeId();

		this->PerCameraLastCullingPlaneIndex.resize(1);
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType>::~BVHNodeSceneTmpl()
	{
		this->SafeDeleteChilds();
		assert(this->tree);
		this->tree->removeNodeFromMap(this);
	}

	/******************
	****    BVH    ****
	******************/

	template<class SceneType>
	BVHSceneTmpl<SceneType>::BVHSceneTmpl(SceneType &scene, BVH_TYPE type, bool useLock, bool skipBuild)
		: bTree::BVHTreeBase<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>, SceneType>(scene, nullptr), bvhMutex(useLock), minUnassignedId(0)
	{
		if (!skipBuild)
		{
			std::vector<smart_pointer<GeometryNode> > gnodes;
			scene->GetNodes(gnodes, SceneNode::GEOMETRY);
			this->Build(gnodes);
		}
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::createLeftInternal(BVHNodeSceneTmpl<SceneType> *parent)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		BVHNodeSceneTmpl<SceneType> *node = this->createLeftInternal_internal(parent);
		return node;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::createRightInternal(BVHNodeSceneTmpl<SceneType> *parent)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		BVHNodeSceneTmpl<SceneType> *node = this->createRightInternal_internal(parent);
		return node;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::createLeftLeaf(BVHNodeSceneTmpl<SceneType> *parent, typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		BVHNodeSceneTmpl<SceneType> *node = this->createLeftLeaf_internal(parent, data);
		return node;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::createRightLeaf(BVHNodeSceneTmpl<SceneType> *parent, typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		BVHNodeSceneTmpl<SceneType> *node = this->createRightLeaf_internal(parent, data);
		return node;
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::logStats() const
	{
		LOG(wxString::Format(wxT("<BVH> leaf nodes %lu, depth %lu, cost %f"),
			(unsigned long)this->geomNodeIdToBVHNodeMap.size(),
			(unsigned long)bTree::BinTreeGetDepth(this->GetRoot()),
			this->root ? this->root->GetCost() : 0.0f));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::reassignNodeIds()
	{
		int id = 0;
		auto fnc = bTree::BinTreeMapFuncWrapper<BVHNodeSceneTmpl<SceneType>>([&id](BVHNodeSceneTmpl<SceneType> *node) { if (node) node->id = id++; } );
		bTree::BinTreeMapDeque(this->root, fnc);
		minUnassignedId = id;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::CreateRootInternal()
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		this->createRootInternal_internal();
		return this->root;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::CreateRootLeaf(typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData &data)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		this->createRootLeaf_internal(data);
		return this->root;
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Rebuild(int ratio, int maxIterations, int threshold, int randomThreshold)
	{
		//WriteLocker wlock(scene->sceneNodesMutex);
		CREATE_WRITE_LOCKER(locker, bvhMutex);
		this->template optimize_internal<bTree::MsumGetter<BVHNodeSceneTmpl<SceneType> > >(ratio, maxIterations, threshold, randomThreshold);
		reassignNodeIds();
		logStats();
		//Check(wxT("Rebuild"));
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::Remove(BVHNodeSceneTmpl<SceneType> * node, NODE_ID nodeIDHint)
	{
		// Note: Since noone used nodeIDHint in previous version of BVHSceneTmpl<SceneType>, neither I use it. This version cannot use the fact I could somehow know
		// previous ID of a node. If somebody wanted to use that version, I suggest adding a variable 'geomNodeID' into BVHNode, which contains
		// ID of the node, so that a preserve the information even when the ID changes.

		if (!node)		return nullptr;
		CREATE_WRITE_LOCKER(locker, bvhMutex);

		BVHNodeSceneTmpl<SceneType> *sibling;
		this->removeNodeUnsafe_internal(node, sibling);
		if (sibling)	sibling->DirtyAABBAndWorldMatrixThisAndUptree();
		//Check(wxT("Remove"));
		return node;
	}

	template<class SceneType>
	BVHNodeSceneTmpl<SceneType> *BVHSceneTmpl<SceneType>::Remove(NODE_ID nodeID)
	{
		return Remove(GetBVHNodeAssignedTo(nodeID), nodeID);
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Insert(std::vector<smart_pointer<GeometryNode> > &items)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);

		// Insert bigger nodes first
		typename bTree::NodeFloatPairing<BVHNodeSceneTmpl<SceneType>>::NodeFloatPQPreferLarger pq;
		for (auto iter = items.begin(); iter != items.end(); ++iter)
		{
			// All nodes must exist and must have their ids
			if (!(*iter) || ((*iter)->GetID() == NODE_ID_NONE))
			{
				LOG(wxT("<BVHSceneTmpl<SceneType>::Insert> - Attempt to insert a node that does not exist or does not have its ID assigned"));
				continue;
			}

			// Do not insert a node that is already in the tree
			if (this->geomNodeIdToBVHNodeMap.find((*iter)->GetID()) != this->geomNodeIdToBVHNodeMap.end())
			{
				continue;
			}

			typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData data;
			data.geom_node = (*iter);
			data.world_matrix = Matrix();
			BVHNodeSceneTmpl<SceneType> *node = this->createNode_internal(data);

			pq.push(bTree::NodeFloatPairing<BVHNodeSceneTmpl<SceneType>>::NodeFloatPair(node, node->GetAABB().SurfaceArea()));
		}
		while (!pq.empty())
		{
			insertNode_internal(pq.top().first);
			pq.pop();
		}
		//if (type!=BVHSceneTmpl<SceneType>::BVHSceneTmpl_FAST)
		//{
		//	updateBV(root);
		//	optimize(items.size()*5);
		//}
		//Check(wxT("Insert"));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Insert(smart_pointer<GeometryNode> &node)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);

		// All nodes must exist and must have their ids
		if (!node || (node->GetID() == NODE_ID_NONE))
		{
			LOG(wxT("<BVH::Insert> - Attempt to insert a node that does not exist or does not have its ID assigned"));
		}
		else if (this->geomNodeIdToBVHNodeMap.find(node->GetID()) == this->geomNodeIdToBVHNodeMap.end())
		{
			typename BVHNodeSceneTmpl<SceneType>::BVHNodeBaseCreateData data;
			data.geom_node = node;
			data.world_matrix = this->scene->GetWorldTransMatrix(node->GetID());
			smart_pointer<Geometry> geo;
			this->scene->GetGeometry(geo, node->GetGeometryID());
			data.geom_AABB = (geo?geo->GetAABB():AABB());
			this->insertNode_internal(this->createNode_internal(data));

			//if (type!=BVH::BVH_FAST)
			//{
			//	updateBV(root);
			//	optimize(5);
			//}
		}
		//Check(wxT("Insert2"));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Build(std::vector<smart_pointer<GeometryNode> > &items)
	{
		CREATE_WRITE_LOCKER(locker, bvhMutex);

		bTree::BVHNodeBaseBuilder<BVHNodeSceneTmpl<SceneType>, BVHSceneTmpl<SceneType>, SceneType>::Build(*this, items);

		if (this->root)
			this->root->UpdateBVSubtree();

		//if (type==BVHSceneTmpl<SceneType>::BVHSceneTmpl_BEST)
		//	optimize(items.size()*3);

		logStats();
		//Check(wxT("Build"));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Cull(Scene *camera_scene, smart_pointer<Camera> &camera, float aspectRatio, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList)
	{
		CREATE_READ_LOCKER(locker, bvhMutex);
		if (!this->root)
		{
			return;
		}

		// Remove all items from arrays, but preserve the allocated memory
		itemsList.clear();
		blendItemsList.clear();
		reflectiveItemsList.clear();

		// Prepare for cull
		Matrix world_matrix = camera_scene->GetWorldTransMatrix(camera->GetID());
		Matrix proj_matrix = camera_scene->GetProjectionMatrix(camera->GetID(), aspectRatio, false);
		bTree::CullingPlanes planes;
		planes.FromMatrixVinv_P(world_matrix, proj_matrix);

		Vector3 cam_position = world_matrix.ExtractTranslation();
		BSphere camera_bsphere = *camera->GetBSphere();

		// Cull
		BVHNodeSplitterSceneTmpl<SceneType> splitter(cam_position, itemsList, blendItemsList, reflectiveItemsList);
		bTree::Cull(this->GetRoot(), planes, 0, camera_bsphere, splitter);

		// Sort geometries
		std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::ItemInfoPreferCloser());
		std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::ItemInfoPreferCloser());
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Cull(const bTree::CullingPlanes &planes, const BSphere &cameraBsphere, const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList)
	{
		CREATE_READ_LOCKER(locker, bvhMutex);
		if (!this->root)
			return;

		// Remove all items from arrays, but preserve the allocated memory
		itemsList.clear();
		blendItemsList.clear();
		reflectiveItemsList.clear();

		// Cull
		BVHNodeSplitterSceneTmpl<SceneType> splitter(cameraTranslation, itemsList, blendItemsList, reflectiveItemsList);
		bTree::Cull(this->GetRoot(), planes, 0, cameraBsphere, splitter);

		// Sort geometries
		std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::ItemInfoPreferCloser());
		std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::ItemInfoPreferCloser());
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::SetInvalid(NODE_ID nodeID)
	{
		CREATE_READ_LOCKER(locker, bvhMutex);
		BVHNodeSceneTmpl<SceneType> *node = this->getNodeByGeometryNodeID_internal(nodeID);
		if (node)
			node->DirtyAABBAndWorldMatrixThisAndUptree();
		//Check(wxT("SetInvalid"));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::SetInvalid(BVHNodeSceneTmpl<SceneType> *node)
	{
		CREATE_READ_LOCKER(locker, bvhMutex);
		if (node)
			node->DirtyAABBAndWorldMatrixThisAndUptree();
		//Check(wxT("SetInvalid2"));
	}

	template<class SceneType>
	void BVHSceneTmpl<SceneType>::Check(const wxString &place, bool reportInvalid) const
	{
		CREATE_READ_LOCKER(locker, bvhMutex);

		if (minUnassignedId<0)
			LOGERROR(place+wxT("BVH::Check: negative minUnassignedId"));

		const BVHNodeSceneTmpl<SceneType> *failed_node;

		failed_node = BVHTreeCheckValidID(this->GetRoot());
		if (failed_node)
		{
			LOGERROR(place+wxT("BVH::Check: node has invalid ID"));
		}

		failed_node = bTree::BinTreeCheckLeafHasDataConsistency(this->GetRoot());
		if (failed_node)
		{
			LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has data and also children"), failed_node->id));
		}

		failed_node = bTree::BinTreeCheckParentChildNoLoopConsistency(this->GetRoot());
		if (failed_node)
		{
			LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has only one child"), failed_node->id));
		}

		if (this->geomNodeIdToBVHNodeMap.size() != bTree::BinTreeCountLeaves(this->GetRoot()))
		{
			LOGERROR(place+wxT("BVH::Check: hashtable has different number of nodes then tree"));
		}

		std::vector<const BVHNodeSceneTmpl<SceneType> *> container;
		bTree::BinTreeGetAllNodes(this->GetRoot(), container);
		for (size_t i=0; i<container.size(); ++i)
		{
			if (container[i]->GetParent() && container[i]->IsAABBDirty() && !container[i]->GetParent()->IsAABBDirty())
				LOGERROR(place+wxT("BVH::Check: Bad dirty tree"));
		}

		// Add any other checks ...

		/*
		size_t numberOfSceneNodes=0;
		std::deque<const BVHNodeSceneTmpl<SceneType> *> queue;
		queue.push_back(NULL);
		const BVHNodeSceneTmpl<SceneType> * node = root;
		while (node)
		{
			if (node->id==-1)
				LOGERROR(place+wxT("BVH::Check: node has invalid ID"));
			if (node->sceneNode && (node->lChild || node->rChild))
				LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has sceneNode but also children"), node->id));
			if (node->parent)
				if (!node->valid && node->parent->valid)
					LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i is invalid bud his parent %i is valid"), node->id, node->parent->id));
			if (reportInvalid && !node->valid)
				LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i is invalid"), node->id));
			if (!node->sceneNode && node->valid)
			{
				AABB aabborig(node->aabb);
				AABB aabb(node->lChild->aabb);
				aabb.Merge(node->rChild->aabb);
				if (aabborig!=aabb)
					LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has invalid AABB"), node->id));
			}
			if (node->lChild)
			{
				if (node->rChild)
					queue.push_back(node->rChild);
				else
					LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has not right child"), node->id));
				node=node->lChild;
			}
			else
			{
				if (node->rChild)
				{
					LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has not left child"), node->id));
					node=node->rChild;
				}
				else
				{
					if (!node->sceneNode)
						LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i has not children but neither sceneNode"), node->id));
					else
					{
						const smart_pointer<GeometryTriangles> &geo=node->sceneNode->GetGeometry();
						smart_pointer<GeometryTriangles> geos;
						scene->GetGeometry(geos, node->sceneNode->GetGeometryID());
						if (!geo || !geos)
						{
							if (geo || geos)
								LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i sceneNode geometry not match the one from scene "), node->id)+(geo?wxT("(geos is NULL)"):wxT("geo is NULL")));
						}
						else
						{
							if (!geo->IsSameAs(geos))
								LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i sceneNode geometry not match the one from scene"), node->id));
							AABB aabborig(geo->GetAABB());
							AABB aabb;
							if (!geo->vertices.empty())
							{
								std::vector<Vector3>::const_iterator v = geo->vertices.begin();
								aabb.MinBound = aabb.MaxBound = *v;
								Vector3 * minB = &aabb.MinBound;
								Vector3 * maxB = &aabb.MaxBound;
								for (++v; v != geo->vertices.end(); v++)
								{
									if (v->x < minB->x)
										minB->x = v->x;
									else if (v->x > maxB->x)
										maxB->x = v->x;
									if (v->y < minB->y)
										minB->y = v->y;
									else if (v->y > maxB->y)
										maxB->y = v->y;
									if (v->z < minB->z)
										minB->z = v->z;
									else if (v->z > maxB->z)
										maxB->z = v->z;
								}
								///Fix zero volume AABB, TODO: check if zero volume cause real problems
								if (fabs(minB->x - maxB->x) < EPSILON)
								{
									minB->x -= EPSILON;
									maxB->x += EPSILON;
								}
								if (fabs(minB->y - maxB->y) < EPSILON)
								{
									minB->y -= EPSILON;
									maxB->y += EPSILON;
								}
								if (fabs(minB->z - maxB->z) < EPSILON)
								{
									minB->z -= EPSILON;
									maxB->z += EPSILON;
								}
							}
							else
								aabb.SetInvalid();
							if (aabborig!=aabb)
								LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i sceneNode has invalid AABB"), node->id));
						}

						SceneToBVHMap::const_iterator mapIt = bvhNodes.find(node->sceneNode->GetID());
						if (mapIt == bvhNodes.end())
							LOGERROR(place+wxString::Format(wxT("BVH::Check: node %i sceneNode is not in hashtable"), node->id));
						++numberOfSceneNodes;
					}
					node = queue.back();
					queue.pop_back();
				}
			}
		}
		if (bvhNodes.size()!=numberOfSceneNodes)
			LOGERROR(place+wxT("BVH::Check: hashtable has different number of nodes then tree"));
		*/
	}

	template<class SceneType>
	wxString BVHSceneTmpl<SceneType>::ToString() const
	{
		CREATE_READ_LOCKER(locker, bvhMutex);

		wxString res(wxT("BVH\n"));
		auto fnc = bTree::BinTreeMapFuncWrapper<const BVHNodeSceneTmpl<SceneType>>([&res](const BVHNodeSceneTmpl<SceneType> *node) { if (node) res+=node->ToString()+wxT("\n"); });
		bTree::BinTreeMapDeque(this->GetRoot(), fnc);
		res+=wxT("bvhNodes\n");
		for (auto iter = this->geomNodeIdToBVHNodeMap.begin(); iter != this->geomNodeIdToBVHNodeMap.end(); ++iter)
		{
			res += wxString::Format(wxT("First: %lu, Second: %lu\n"), (unsigned long)iter->first, (unsigned long)iter->second->id);
		}
		//std::for_each(geomNodeIdToBVHNodeMap.begin(), geomNodeIdToBVHNodeMap.end(), [&res](const GeomNodeIDToBVHNodeMap::value_type &pair) {
		//	res+=wxString::Format(wxT("First: %i, Second: %i\n"), pair.first, pair.second->id);
		//});

		return res;
	}
}

#endif
