#ifndef __SCENE_SHADOW__H__
#define __SCENE_SHADOW__H__

#include "scenenode.h"
#include "geometrynode.h"
#include "BinTreesCulling.h"
#include "events.h"
#include <queue>
#include <functional>
#include <stack>
#include <set>

namespace VRUT
{
	class SceneNodeShadow
	{
	private:
		Matrix localMatrix;
		bool active;
	public:
		unsigned char lastOut;
	private:
		unsigned char nodeProperties;//bits: 0 - blended, 1 - reflection, 2 - alpha test, 3 - tex coords generation, 4 - TEX_CUBE_MAP, 7 - matrix not unit 
		SceneNode::SCENENODE_TYPE type;
		AABB aabb;
		NODE_ID id;
		NODE_ID parentID;
		std::vector<NODE_ID> children;
		GEOMETRY_ID gid;
		MATERIAL_ID mid;

	public:
		smart_pointer<Geometry> geometry;
		enum ShadowNodeProperties
		{
			NONE = 0,
			BLENDED = 1,
			FLAT_REFLECTION = 2,
			ALPHA_TEST = 4,
			TEX_GEN_REFLECTION = 8,
			TEX_CUBE_MAP = 16,
			VIRTUAL_CAMERA = 32,
			MATRIX_NOT_UNIT = 128
		};

		SceneNodeShadow(smart_pointer_v2<Scene> &scene, const SceneNode &snode) : lastOut(0)
		{
			Assign(scene, snode);
		}

		~SceneNodeShadow()
		{
		}

		void SetMaterialProperties(const Material *mat)
		{
			nodeProperties &= MATRIX_NOT_UNIT;
			if (mat)
			{
				if (mat->flags & Material::BLENDED)
					nodeProperties |= BLENDED;
				if (mat->flags & Material::FLAT_REFLECTION)
					nodeProperties |= FLAT_REFLECTION;
				if (mat->texFlags & Material::ALPHA_TEST)
					nodeProperties |= ALPHA_TEST;
				if (mat->texFlags & Material::TEX_GEN_REFLECTION)
					nodeProperties |= TEX_GEN_REFLECTION;
				if (mat->texFlags & Material::TEX_CUBE_MAP)
					nodeProperties |= TEX_CUBE_MAP;
				else if (mat->shader)
				{
					for (auto &param : mat->shader->paramList) //TODO: Cache if the shader contains a cubemap. This for loop is annoying.
					{
						if (param.type == ShaderParam::SAMPLERCUBE) //TODO: This is not clean. Shaders might want to use cube maps without reflection.
							nodeProperties |= TEX_GEN_REFLECTION;
						else if ((param.type == ShaderParam::INT) && (param.name==wxT("virtualCameraID")))
							nodeProperties |= VIRTUAL_CAMERA;
					}
				}
			}
		}

		void Assign(smart_pointer_v2<Scene> &scene, const SceneNode &snode)
		{
			id = snode.GetID();
			type = snode.GetType();
			localMatrix = *snode.GetLocalTransMatrix();
			if (localMatrix.IsUnit())
				nodeProperties = NONE;
			else
				nodeProperties = MATRIX_NOT_UNIT;
			parentID=snode.GetParentID();
			active = snode.IsActive();
			children.clear();
			scene->GetChildrenNodeIDs(children, snode.GetID());
			gid=GEOMETRY_ID_NONE;
			mid=MATERIAL_ID_NONE;
			if (type==SceneNode::GEOMETRY)
			{
				smart_pointer<GeometryNode> gnode=smart_pointer<GeometryNode>((GeometryNode*)&snode);
				if (gnode)
				{
					gid=gnode->GetGeometryID();
					mid=gnode->GetMaterialID();
					smart_pointer<Material> mat;
					scene->GetMaterial(mat, mid);
					SetMaterialProperties(mat.getPtr());
					scene->GetGeometry(geometry, gnode->GetGeometryID());
					if (geometry)
						aabb=geometry->GetAABB();
					else
						aabb.SetInvalid();
				}
				else
				{
					aabb.SetInvalid();
					geometry.assign(NULL);
				}
			}
			else if (type==SceneNode::BACKGROUND)
			{
				smart_pointer<BackgroundNode> bnode=smart_pointer<BackgroundNode>((BackgroundNode*)&snode);
				if (bnode)
				{
					mid=bnode->GetMaterialID();
					smart_pointer<Material> mat;
					scene->GetMaterial(mat, mid);
					SetMaterialProperties(mat.getPtr());
				}
				aabb.SetInvalid();
				geometry.assign(NULL);
			}
			else
			{
				aabb.SetInvalid();
				geometry.assign(NULL);
			}
		}

		Matrix* GetLocalMatrix()
		{
			return &localMatrix;
		}

		void SetLocalMatrix(const Matrix* matrix)
		{
			this->localMatrix=*matrix;
			if (this->localMatrix.IsUnit())
				this->nodeProperties &= 255 - MATRIX_NOT_UNIT;
			else
				this->nodeProperties |= MATRIX_NOT_UNIT;
		}

		NODE_ID GetID()
		{
			return id;
		}

		SceneNode::SCENENODE_TYPE GetType()
		{
			return type;
		}

		bool IsOfType(const SceneNode::SCENENODE_TYPE type)
		{
			return type==this->type;
		}

		NODE_ID GetParentID()
		{
			return parentID;
		}

		void SetParentID(NODE_ID parentID)
		{
			this->parentID=parentID;
		}

		std::vector<NODE_ID>* GetChildren()
		{
			return &children;
		}

		bool IsActive()
		{
			return active;
		}

		void SetActive(const bool active)
		{
			this->active=active;
		}

		unsigned char GetNodeProperties()
		{
			return nodeProperties;
		}

		AABB& GetAABB()
		{
			return aabb;
		}

		GEOMETRY_ID GetGeometryID() const
		{
			return gid;
		}

		MATERIAL_ID GetMaterialID() const
		{
			return mid;
		}

		void ChangeIndex(NODE_ID nodeID, size_t newIndex)
		{
			std::vector<NODE_ID>::iterator nodePos=std::find(children.begin(), children.end(), nodeID);
			if (nodePos==children.end())
			{
				LOGERROR(wxT("<SceneShadowNode> ChangeIndex not found child in parent's list"));
				return;
			}
			children.erase(nodePos);
			children.insert(children.begin()+newIndex, nodeID);
		}

		void SetMaterial(const Material *mat)
		{
			if (mat)
				mid=mat->mid;
			else
				mid=MATERIAL_ID_NONE;
			SetMaterialProperties(mat);
		}

		void SetGeometry(smart_pointer<Geometry> &geo)
		{
			if (type==SceneNode::GEOMETRY)
			{
				geometry.assign(geo);
				if (geo)
				{
					gid=geo->GetID();
					aabb=geo->GetAABB();
				}
				else
				{
					gid=GEOMETRY_ID_NONE;
					aabb.SetInvalid();
				}
			}
		}
	};

	class SceneShadow
	{
	private:
		smart_pointer_v2<Scene> scene;
		std::vector<SceneNodeShadow *> sceneNodes;
		NODE_ID rootID;
		Matrix worldMatTmp;
		std::vector<std::pair<NODE_ID, Matrix> > shadowQueue;
		std::vector<NODE_ID> traverseStack;

		void insert(const SceneNode &snode)
		{
			if (snode.GetID()>=sceneNodes.size())
				sceneNodes.resize(snode.GetID()+1, NULL);
			if (sceneNodes[snode.GetID()])
				sceneNodes[snode.GetID()]->Assign(scene, snode);
			else
				sceneNodes[snode.GetID()]=new SceneNodeShadow(scene, snode);
			//SAFE_DELETE(sceneNodes[snode.GetID()]);
		}

	public:
		struct ItemInfo
		{
			float distance;
			SceneNodeShadow* node;
			Matrix matrix;
			ItemInfo(float distance, SceneNodeShadow *node, const Matrix &matrix) : distance(distance), node(node), matrix(matrix) {}

			static bool SortByDistance(const ItemInfo & p1, const ItemInfo & p2)
			{
				return p1.distance < p2.distance;
			}

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

		explicit SceneShadow(smart_pointer_v2<Scene> &scene) : rootID(scene->GetRootID())
		{
			this->scene = scene;
			std::vector<smart_pointer<SceneNode> > snodes;
			scene->GetNodes(snodes);
			sceneNodes.resize(scene->GetNodeMaxID(), NULL);
			for (auto &snode : snodes)
				sceneNodes[snode->GetID()]=new SceneNodeShadow(scene, *snode.getPtr());
		}
		~SceneShadow()
		{
			for (auto &snode : sceneNodes)
				SAFE_DELETE(snode);
		}

		smart_pointer_v2<Scene> & GetScene()
		{
			return scene;
		}

		// Process events to update Shadow
		// Following events are processed and needs to be registered:
		// - Event::EVT_SCENE_NODE_INSERTED
		// - Event::EVT_SCENE_NODE_REMOVED
		// - Event::EVT_SCENE_NODE_MOVED
		// - Event::EVT_SCENE_NODE_INDEX_CHANGED
		// - Event::EVT_SCENE_NODE_ACTIVE_CHANGED
		// - Event::EVT_SCENE_NODE_SWITCH_ACTIVECHILD_CHANGED
		// - Event::EVT_SCENE_NODE_TRANSFORMED
		// - Event::EVT_SCENE_NODE_GEOMETRY_CHANGED
		// - Event::EVT_SCENE_NODE_MATERIAL_CHANGED
		// - Event::EVT_SCENE_GEOMETRY_ADDED
		// - Event::EVT_SCENE_GEOMETRY_REMOVED
		// - Event::EVT_SCENE_MATERIAL_ADDED
		// - Event::EVT_SCENE_MATERIAL_REMOVED
		void ProcessEvent(Event * evt, const SCENE_ID sceneID)
		{
			switch (evt->type)
			{
			case Event::EVT_SCENE_MATERIAL_ADDED:
				{
					if (sceneID != ((EventSceneMaterialAdded*)evt)->sceneID)
					{
						break;
					}
					std::vector<NODE_ID> nodeIDs;
					if (MATERIAL_ID_NONE == ((EventSceneMaterialAdded*)evt)->materialID)
					{
						break;
					}
					scene->GetNodeIDsWithMaterial(nodeIDs, ((EventSceneMaterialAdded*)evt)->materialID);
					if (nodeIDs.empty())
					{
						break;
					}
					UpdateMaterial(((EventSceneMaterialAdded*)evt)->materialID, nodeIDs);
				}
				break;
			case Event::EVT_SCENE_MATERIAL_REMOVED:
				{
					if (sceneID != ((EventSceneMaterialRemoved*)evt)->sceneID)
					{
						break;
					}
					// Materials are not stored themselfs. may be later.
				}
				break;
			case Event::EVT_SCENE_NODE_INSERTED:
				{
					if (sceneID !=((EventSceneNodeInserted*)evt)->sceneID)
					{
						break;
					}
					Insert(((EventSceneNodeInserted*)evt)->nodeID);
				}
				break;
			case Event::EVT_SCENE_NODE_GEOMETRY_CHANGED:
				{
					if (sceneID !=((EventSceneNodeGeometryChanged*)evt)->sceneID)
					{
						break;
					}
					SetGeometry(((EventSceneNodeGeometryChanged*)evt)->nodeID, ((EventSceneNodeGeometryChanged*)evt)->geometryID);
				}
				break;
			case Event::EVT_SCENE_NODE_REMOVED:
				{
					if (sceneID != ((EventSceneNodeRemoved*)evt)->sceneID)
					{
						break;
					}
					Remove(((EventSceneNodeRemoved*)evt)->nodeID);
				}
				break;
			case Event::EVT_SCENE_NODE_TRANSFORMED:
				{
					EventSceneNodeTransformed *castEvt = (EventSceneNodeTransformed*)evt;
					if (sceneID != castEvt->sceneID)
					{
						break;
					}
					SceneNodeShadow *shnode = GetNode(castEvt->nodeID);
					if (shnode)
					{
						shnode->SetLocalMatrix(&castEvt->transMatrix);
					}
				}
				break;
			case Event::EVT_SCENE_NODE_MOVED:
				{
					if (sceneID != ((EventSceneNodeMoved*)evt)->sceneID)
					{
						break;
					}
					Move(((EventSceneNodeMoved*)evt)->nodeID, ((EventSceneNodeMoved*)evt)->parentID);
				}
				break;
			case Event::EVT_SCENE_NODE_INDEX_CHANGED:
				{
					if (sceneID != ((EventSceneNodeIndexChanged*)evt)->sceneID)
					{
						break;
					}
					ChangeIndex(((EventSceneNodeIndexChanged*)evt)->nodeID, ((EventSceneNodeIndexChanged*)evt)->newIndex);
				}
				break;
			case Event::EVT_SCENE_GEOMETRY_ADDED:
				{
					if (sceneID  != ((EventSceneGeometryAdded*)evt)->sceneID)
					{
						break;
					}
					std::vector<NODE_ID> nodeIDs;
					scene->GetNodeIDsWithGeometry(nodeIDs, ((EventSceneGeometryAdded*)evt)->geometryID);
					UpdateGeometry(((EventSceneGeometryAdded*)evt)->geometryID, nodeIDs);
				}
				break;
			case Event::EVT_SCENE_GEOMETRY_REMOVED:
				{
					if (sceneID != ((EventSceneGeometryRemoved*)evt)->sceneID)
					{
						break;
					}
					std::vector<NODE_ID> nodeIDs;
					scene->GetNodeIDsWithGeometry(nodeIDs, ((EventSceneGeometryRemoved*)evt)->geometryID);
					UpdateGeometry(((EventSceneGeometryRemoved*)evt)->geometryID, nodeIDs);
				}
				break;
			case Event::EVT_SCENE_NODE_ACTIVE_CHANGED:
				{
					// If isImplied is true, we can skip processing of this event.
					// The change of active state is processed in Event:EVT_SCENE_NODE_SWITCH_ACTIVECHILD_CHANGED.
					if ((sceneID != ((EventSceneNodeActiveChanged*)evt)->sceneID) || ((EventSceneNodeActiveChanged*)evt)->isImplied)
					{
						break;
					}
					SceneNodeShadow *shnode = GetNode(((EventSceneNodeActiveChanged*)evt)->nodeID);
					if (shnode)
					{
						shnode->SetActive(((EventSceneNodeActiveChanged*)evt)->active);
					}
				}
				break;
			case Event::EVT_SCENE_NODE_SWITCH_ACTIVECHILD_CHANGED:
				{
					if (sceneID != ((EventSceneNodeSwitchActiveChildChanged*)evt)->sceneID)
					{
						break;
					}
					SetSwitchActiveChild(((EventSceneNodeSwitchActiveChildChanged*)evt)->nodeID, ((EventSceneNodeSwitchActiveChildChanged*)evt)->activeChild);
				}
				break;
			case Event::EVT_SCENE_NODE_MATERIAL_CHANGED:
				{
					if (sceneID != ((EventSceneNodeMaterialChanged*)evt)->sceneID)
					{
						break;
					}
					SetMaterial(((EventSceneNodeMaterialChanged*)evt)->nodeID, ((EventSceneNodeMaterialChanged*)evt)->materialID);
				}
				break;
			default:
				break;
			}
		}

		NODE_ID GetRootID()
		{
			return rootID;
		}

		SceneNodeShadow* GetNode(NODE_ID nodeID) const
		{
			if (nodeID<sceneNodes.size())
				return sceneNodes[nodeID];
			return NULL;
		}

		void Insert(NODE_ID nodeID)
		{
			if (nodeID==NODE_ID_NONE)
			{
				LOG(wxT("<SceneShadow>bad nodeID"));
				return;
			}
			//update node
			smart_pointer<SceneNode> snode;
			scene->GetNode(snode, nodeID);
			if (!snode)
				return;
			insert(*snode.getPtr());
			//update parent
			/*smart_pointer<SceneNode> psnode;
			scene->GetNode(psnode, snode->GetParentID());
			if (!psnode)
			return;
			insert(scene, *psnode.getPtr());*/
			SceneNodeShadow *pshnode=GetNode(snode->GetParentID());
			if (pshnode)
			{
				std::vector<NODE_ID> *children = pshnode->GetChildren();
				std::vector<NODE_ID>::iterator pos = std::find(children->begin(), children->end(), nodeID);
				if (pos != children->end())
				{
					size_t posi = children->size() - (pos - children->begin());
					if (posi > 2)
					{
						children->erase(pos);
						children->push_back(nodeID);
					}
					else if (posi == 2)
						std::swap(*pos, children->back());
				}
				else
					pshnode->GetChildren()->push_back(nodeID);
			}
		}

		void Remove(NODE_ID nodeID)
		{
			if (nodeID>=sceneNodes.size())
				return;
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (shnode)
			{
				//update parent
				/*smart_pointer<SceneNode> psnode;
				scene.GetNode(psnode, shnode->GetParentID());
				if (psnode)
				insert(scene, *psnode.getPtr());*/
				SceneNodeShadow *psnode=GetNode(shnode->GetParentID());
				if (psnode)
				{
					std::vector<NODE_ID> *children = psnode->GetChildren();
					std::vector<NODE_ID>::iterator pos = std::find(children->begin(), children->end(), nodeID);
					if (pos != children->end())
						children->erase(pos);
				}
				//remove node
				SAFE_DELETE(sceneNodes[nodeID]);
			}
		}

		void Move(NODE_ID nodeID, NODE_ID parentID)
		{
			if (nodeID==NODE_ID_NONE)
			{
				LOG(wxT("<SceneShadow>Move: bad nodeID"));
				return;
			}
			//update old parent
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (shnode)
			{
				SceneNodeShadow *opsnode=GetNode(shnode->GetParentID());
				if (opsnode)
				{
					std::vector<NODE_ID> *children=opsnode->GetChildren();
					std::vector<NODE_ID>::iterator pos=std::find(children->begin(), children->end(), nodeID);
					if (pos!=children->end())
						children->erase(pos);
				}
				//update node's parent
				shnode->SetParentID(parentID);
				//update new parent
				SceneNodeShadow *npsnode=GetNode(parentID);
				npsnode->GetChildren()->push_back(nodeID);
			}
			else
				LOG(wxT("<SceneShadow>Move: node not exist"));
		}

		void ChangeIndex(NODE_ID nodeID, size_t newIndex)
		{
			if (nodeID==NODE_ID_NONE)
			{
				LOGERROR(wxT("<SceneShadow>ChangeIndex bad nodeID"));
				return;
			}
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (!shnode)
			{
				LOGERROR(wxT("<SceneShadow>ChangeIndex node not exists"));
				return;
			}
			SceneNodeShadow *shpnode=GetNode(shnode->GetParentID());
			if (!shpnode)
			{
				LOGERROR(wxT("<SceneShadow>ChangeIndex node parent not exists"));
				return;
			}
			shpnode->ChangeIndex(nodeID, newIndex);
		}

		void SetMaterial(NODE_ID nodeID, MATERIAL_ID materialID)
		{
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (shnode)
			{
				smart_pointer<Material> mat;
				scene->GetMaterial(mat, materialID);
				shnode->SetMaterial(mat.getPtr());
			}
		}

		void SetGeometry(NODE_ID nodeID, GEOMETRY_ID geometryID)
		{
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (shnode)
			{
				smart_pointer<Geometry> geo;
				scene->GetGeometry(geo, geometryID);
				shnode->SetGeometry(geo);
			}
		}

		void UpdateMaterial(MATERIAL_ID mid, const std::vector<NODE_ID> &nodeIDs)
		{
			if (!scene)
				return;
			smart_pointer<Material> mat;
			scene->GetMaterial(mat, mid);
			for (auto nodeID : nodeIDs)
			{
				SceneNodeShadow *shnode=GetNode(nodeID);
				if (shnode)
					shnode->SetMaterialProperties(mat.getPtr());
			}
		}

		void UpdateGeometry(GEOMETRY_ID gid, const std::vector<NODE_ID> &nodeIDs)
		{
			if (!scene)
				return;
			smart_pointer<Geometry> geo;
			scene->GetGeometry(geo, gid);
			for (auto nodeID : nodeIDs)
			{
				SceneNodeShadow *shnode=GetNode(nodeID);
				if (shnode)
					shnode->SetGeometry(geo);
			}
		}

		void Update(NODE_ID nodeID)
		{
			if (nodeID==NODE_ID_NONE)
			{
				LOG(wxT("<SceneShadow>bad nodeID"));
				return;
			}
			//update node
			smart_pointer<SceneNode> snode;
			scene->GetNode(snode, nodeID);
			if (!snode)
				return;
			insert(*snode.getPtr());
		}

		void SetSwitchActiveChild(const NODE_ID nodeID, const size_t activeChild)
		{
			SceneNodeShadow *shnode=GetNode(nodeID);
			if (!shnode || (shnode->GetType()!=SceneNode::SWITCH))
				return;

			if ((activeChild==(size_t)-1) || (activeChild==(size_t)-2) || (shnode->GetChildren()->size()<=activeChild) || !shnode->IsActive())
			{
				bool active=(activeChild==(size_t)-2);
				if (!shnode->IsActive())
					active=false;
				for (auto childID : *shnode->GetChildren())
				{
					SceneNodeShadow *shchnode=GetNode(childID);
					if (shchnode)
						shchnode->SetActive(active);
				}
			}
			else
			{
				size_t childID=0;
				for (auto child : *shnode->GetChildren())
				{
					SceneNodeShadow *shchnode=GetNode(child);
					if (shchnode)
						shchnode->SetActive((activeChild==childID));
					++childID;
				}
			}
		}

		Matrix* GetWorldMatrix(NODE_ID nodeID)
		{
			worldMatTmp=Matrix();
			SceneNodeShadow *snode=GetNode(nodeID);
			while (snode)
			{
				if (snode->GetNodeProperties() & SceneNodeShadow::MATRIX_NOT_UNIT)
					worldMatTmp=worldMatTmp*(*snode->GetLocalMatrix());
				snode=GetNode(snode->GetParentID());
			}
			return &worldMatTmp;
		}

		bool IsActive(NODE_ID nodeID) const
		{
			SceneNodeShadow *snode=GetNode(nodeID);
			bool isActive=false;
			while (snode)
			{
				isActive=snode->IsActive();
				if (!isActive)
					return false;
				snode=GetNode(snode->GetParentID());
			}
			return isActive;
		}

		void GetSubNodes(std::vector<SceneNodeShadow*> &nodes, const NODE_ID id, bool includeRoot = false) const
		{
			size_t i = nodes.size();
			SceneNodeShadow * parent = GetNode(id);
			if (includeRoot && parent)
			{
				nodes.push_back(parent);
			}
			while (true)
			{
				if (parent)
				{
					for (auto childID : *parent->GetChildren())
					{
						SceneNodeShadow *child = GetNode(childID);
						if (child)
							nodes.push_back(child);
					}
				}
				if (i >= nodes.size())
					return;
				parent = nodes[i];
				++i;
			}
		}

        //Get IDs of all geometry and light nodes from a subtree
        //Root is implicit, as it also can be a geometry node
        //Why set? Because when simulating let's say an arm with 2 joints that move,
        // geometry on the end of the arm is affected by two transforms changes, so in a vector
        // the nodes that have changed might be duplicit, if this is called multiple times on update
        void GetGeometryLightSubNodeIDs(std::set<NODE_ID> &geoms, std::set<NODE_ID> &lights, const NODE_ID id) const
        {
            std::stack<NODE_ID> stack;
            stack.push(id);

            while (!stack.empty())
            {
                NODE_ID nID = stack.top();
                stack.pop();

                SceneNodeShadow* node = GetNode(nID);

                if (node)
                {
                    //Push all its children on the stack
                    for (auto childID : *node->GetChildren())
                        stack.push(childID);

                    //Get node type and insert if Geometry or Light
                    if (node->GetType() == SceneNode::SCENENODE_TYPE::GEOMETRY)
                        geoms.insert(nID);

                    if (node->GetType() == SceneNode::SCENENODE_TYPE::LIGHT)
                        lights.insert(nID);
                }
            }
        }
        
        //Get all node IDs of certain type
        void GetNodeIDs(std::vector<NODE_ID>& ids, SceneNode::SCENENODE_TYPE type) const
        {
            for(auto n : sceneNodes)
            {
                if (n && (n->GetType() == type))
                    ids.push_back(n->GetID());
            }
        }

		void TraverseSubNodes(std::function<void(SceneNodeShadow *)> process, const NODE_ID id, bool includeRoot = false)
		{
			traverseStack.clear();

			SceneNodeShadow *node=GetNode(id);
			if (!node)
			{
				return;
			}
			if (!includeRoot)
			{
				if (!node->GetChildren()->empty())
				{
					for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					{
						traverseStack.push_back(node->GetChildren()->at(i));
					}
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					return;
				}
			}
			while (true)
			{
				if (!node)
				{
					if (!traverseStack.empty())
					{
						node = GetNode(traverseStack.back());
						traverseStack.pop_back();
						continue;
					}
					break;
				}
				process(node);
				if (!node->GetChildren()->empty())
				{
					for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					{
						traverseStack.push_back(node->GetChildren()->at(i));
					}
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!traverseStack.empty())
					{
						node = GetNode(traverseStack.back());
						traverseStack.pop_back();
					}
					else
					{
						break;
					}
				}
			}
		}

		//culling of all geometry
		void Cull(const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList, ItemsList &backgroundItemsList, ItemsList &virtualCameraItemsList,
			NODE_ID renderingRootID)
		{
			itemsList.clear();
			blendItemsList.clear();
			reflectiveItemsList.clear();
			backgroundItemsList.clear();
			virtualCameraItemsList.clear();
			shadowQueue.clear();

			SceneNodeShadow *node = nullptr;
			if (renderingRootID == NODE_ID_NONE)
				node = GetNode(GetRootID());
			else
			{
				node = GetNode(renderingRootID);
				if (!node)
					node = GetNode(GetRootID());
			}
			if (!node)
				return;
			Matrix worldMat=*node->GetLocalMatrix();
			while (true)
			{
				if (!node || (!node->IsActive()))
				{
					if (!shadowQueue.empty())
					{
						worldMat=shadowQueue.back().second;
						node=GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				Matrix nodeWorldMat;
				if (node->GetNodeProperties() & SceneNodeShadow::MATRIX_NOT_UNIT)
					nodeWorldMat = (*node->GetLocalMatrix())*worldMat;
				else
					nodeWorldMat = worldMat;
				if (node->GetType()==SceneNode::GEOMETRY)
				{
					if (node->GetNodeProperties()&SceneNodeShadow::VIRTUAL_CAMERA)
						virtualCameraItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					if (node->GetNodeProperties()&SceneNodeShadow::FLAT_REFLECTION)
						reflectiveItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					else if (node->GetNodeProperties()&SceneNodeShadow::BLENDED)
						blendItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					else
						itemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
				}
				else if (node->GetType()==SceneNode::BACKGROUND)
				{
					backgroundItemsList.push_back(ItemInfo(0, node, nodeWorldMat));
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), nodeWorldMat));
				if (!node->GetChildren()->empty())
				{
					worldMat=nodeWorldMat;
					node=GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						worldMat=shadowQueue.back().second;
						node=GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}

			std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::SortByDistance);
		}

		//culling of all geometry STEREO (the background and virtual camera lists are the same for both eyes)
		void Cull(const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList, ItemsList &backgroundItemsList, ItemsList &virtualCameraItemsList,
			const Vector3 &cameraTranslationR, ItemsList &itemsListR, ItemsList &blendItemsListR, ItemsList &reflectiveItemsListR, NODE_ID renderingRootID)
		{
			itemsList.clear();
			blendItemsList.clear();
			reflectiveItemsList.clear();
			backgroundItemsList.clear();
			virtualCameraItemsList.clear();
			itemsListR.clear();
			blendItemsListR.clear();
			reflectiveItemsListR.clear();
			shadowQueue.clear();

			SceneNodeShadow *node = nullptr;
			if (renderingRootID == NODE_ID_NONE)
				node = GetNode(GetRootID());
			else
			{
				node = GetNode(renderingRootID);
				if (!node)
					node = GetNode(GetRootID());
			}
			if (!node)
				return;
			Matrix worldMat = *node->GetLocalMatrix();
			while (true)
			{
				if (!node || (!node->IsActive()))
				{
					if (!shadowQueue.empty())
					{
						worldMat = shadowQueue.back().second;
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				Matrix nodeWorldMat;
				if (node->GetNodeProperties() & SceneNodeShadow::MATRIX_NOT_UNIT)
					nodeWorldMat = (*node->GetLocalMatrix())*worldMat;
				else
					nodeWorldMat = worldMat;
				if (node->GetType() == SceneNode::GEOMETRY)
				{
					Vector3 geometryCenter(nodeWorldMat.TransformCoord(node->GetAABB().Center()));
					if (node->GetNodeProperties()&SceneNodeShadow::VIRTUAL_CAMERA)
						virtualCameraItemsList.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
					if (node->GetNodeProperties()&SceneNodeShadow::FLAT_REFLECTION)
					{
						reflectiveItemsList.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
						reflectiveItemsListR.push_back(ItemInfo((cameraTranslationR - geometryCenter).LengthSq(), node, nodeWorldMat));
					}
					else if (node->GetNodeProperties()&SceneNodeShadow::BLENDED)
					{
						blendItemsList.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
						blendItemsListR.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
					}
					else
					{
						itemsList.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
						itemsListR.push_back(ItemInfo((cameraTranslation - geometryCenter).LengthSq(), node, nodeWorldMat));
					}
				}
				else if (node->GetType() == SceneNode::BACKGROUND)
				{
					backgroundItemsList.push_back(ItemInfo(0, node, nodeWorldMat));
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), nodeWorldMat));
				if (!node->GetChildren()->empty())
				{
					worldMat = nodeWorldMat;
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						worldMat = shadowQueue.back().second;
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}

			std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::SortByDistance);
			std::sort(blendItemsListR.begin(), blendItemsListR.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsListR.begin(), reflectiveItemsListR.end(), ItemInfo::SortByDistance);
		}

		//culling with planes and sphere
		void Cull(const bTree::CullingPlanes &planes, const BSphere &bsphere, const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList,
							ItemsList &reflectiveItemsList, ItemsList &backgroundItemsList, ItemsList &virtualCameraItemsList, NODE_ID renderingRootID, const NODE_ID cullNodeID = NODE_ID_NONE, const bool useRootMatrix = true)
		{
			itemsList.clear();
			blendItemsList.clear();
			reflectiveItemsList.clear();
			backgroundItemsList.clear();
			virtualCameraItemsList.clear();
			shadowQueue.clear();

			SceneNodeShadow *node = nullptr;
			if (renderingRootID == NODE_ID_NONE)
				node = GetNode(GetRootID());
			else
			{
				node = GetNode(renderingRootID);
				if (!node)
					node = GetNode(GetRootID());
			}
			if (!node)
				return;

			Matrix worldMat = Matrix::Unit; // Initialized as Identity matrix 

			// there was a bug that the transform was used twice!
			//			if (useRootMatrix)
			//				worldMat=*node->GetLocalMatrix();

			while (true)
			{
				if (!node || (!node->IsActive()) || (node->GetID() == cullNodeID))
				{
					if (!shadowQueue.empty())
					{
						worldMat=shadowQueue.back().second;
						node=GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				Matrix nodeWorldMat;
				if ((node->GetNodeProperties() & SceneNodeShadow::MATRIX_NOT_UNIT) && (useRootMatrix || (node->GetID() != renderingRootID)))
					nodeWorldMat = (*node->GetLocalMatrix())*worldMat;
				else
					nodeWorldMat = worldMat;
				if (node->GetType() == SceneNode::GEOMETRY)
				{
					//Vector3 center(nodeWorldMat.TransformCoord(node->GetAABB().Center()));
					//float radius(geometry->GetAABB().Radius());
					unsigned char planeMask = 0;
					if (/*bsphere.Intersects(center, radius) && */bTree::CullingPlanes::InFrustum(planes, node->GetAABB().Transform(&nodeWorldMat), &node->lastOut, &planeMask))
					{
						if (node->GetNodeProperties()&SceneNodeShadow::VIRTUAL_CAMERA)
							virtualCameraItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat)); 
						if (node->GetNodeProperties()&SceneNodeShadow::FLAT_REFLECTION)
							reflectiveItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else if (node->GetNodeProperties()&SceneNodeShadow::BLENDED)
							blendItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else
							itemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					}
				}
				else if (node->GetType()==SceneNode::BACKGROUND)
				{
					backgroundItemsList.push_back(ItemInfo(0, node, nodeWorldMat));
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), nodeWorldMat));
				if (!node->GetChildren()->empty())
				{
					worldMat=nodeWorldMat;
					node=GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						worldMat=shadowQueue.back().second;
						node=GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}

			std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::SortByDistance);
		}

		//culling with planes STEREO (the background and virtual camera lists are the same for both eyes)
		void Cull(const bTree::CullingPlanes &planes, const Vector3 &cameraTranslation, ItemsList &itemsList, ItemsList &blendItemsList, ItemsList &reflectiveItemsList, ItemsList &backgroundItemsList,
			ItemsList &virtualCameraItemsList, const bTree::CullingPlanes &planesR, const Vector3 &cameraTranslationR, ItemsList &itemsListR, ItemsList &blendItemsListR, ItemsList &reflectiveItemsListR,
			NODE_ID renderingRootID)
		{
			itemsList.clear();
			blendItemsList.clear();
			reflectiveItemsList.clear();
			backgroundItemsList.clear();
			virtualCameraItemsList.clear();
			itemsListR.clear();
			blendItemsListR.clear();
			reflectiveItemsListR.clear();
			shadowQueue.clear();

			SceneNodeShadow *node = nullptr;
			if (renderingRootID == NODE_ID_NONE)
				node = GetNode(GetRootID());
			else
			{
				node = GetNode(renderingRootID);
				if (!node)
					node = GetNode(GetRootID());
			}
			if (!node)
				return;
			Matrix worldMat = *node->GetLocalMatrix();
			while (true)
			{
				if (!node || (!node->IsActive()))
				{
					if (!shadowQueue.empty())
					{
						worldMat = shadowQueue.back().second;
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				Matrix nodeWorldMat;
				if (node->GetNodeProperties() & SceneNodeShadow::MATRIX_NOT_UNIT)
					nodeWorldMat = (*node->GetLocalMatrix())*worldMat;
				else
					nodeWorldMat = worldMat;
				if (node->GetType() == SceneNode::GEOMETRY)
				{
					//Vector3 center(nodeWorldMat.TransformCoord(node->GetAABB().Center()));
					//float radius(geometry->GetAABB().Radius());
					unsigned char planeMask = 0;
					if (bTree::CullingPlanes::InFrustum(planes, node->GetAABB().Transform(&nodeWorldMat), &node->lastOut, &planeMask))
					{
						if (node->GetNodeProperties()&SceneNodeShadow::VIRTUAL_CAMERA)
							virtualCameraItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						if (node->GetNodeProperties()&SceneNodeShadow::FLAT_REFLECTION)
							reflectiveItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else if (node->GetNodeProperties()&SceneNodeShadow::BLENDED)
							blendItemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else
							itemsList.push_back(ItemInfo((cameraTranslation - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					}
					if (bTree::CullingPlanes::InFrustum(planesR, node->GetAABB().Transform(&nodeWorldMat), &node->lastOut, &planeMask))
					{
						if (node->GetNodeProperties()&SceneNodeShadow::FLAT_REFLECTION)
							reflectiveItemsListR.push_back(ItemInfo((cameraTranslationR - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else if (node->GetNodeProperties()&SceneNodeShadow::BLENDED)
							blendItemsListR.push_back(ItemInfo((cameraTranslationR - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
						else
							itemsListR.push_back(ItemInfo((cameraTranslationR - nodeWorldMat.TransformCoord(node->GetAABB().Center())).LengthSq(), node, nodeWorldMat));
					}
				}
				else if (node->GetType() == SceneNode::BACKGROUND)
				{
					backgroundItemsList.push_back(ItemInfo(0, node, nodeWorldMat));
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), nodeWorldMat));
				if (!node->GetChildren()->empty())
				{
					worldMat = nodeWorldMat;
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						worldMat = shadowQueue.back().second;
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}

			std::sort(blendItemsList.begin(), blendItemsList.end(), ItemInfo::SortByDistance);
			std::sort(blendItemsListR.begin(), blendItemsListR.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsList.begin(), reflectiveItemsList.end(), ItemInfo::SortByDistance);
			std::sort(reflectiveItemsListR.begin(), reflectiveItemsListR.end(), ItemInfo::SortByDistance);
		}

		////////////////////////////////////////////////////////////////////////////
		/// Scene API for compatibility
		////////////////////////////////////////////////////////////////////////////
	public:
		// Add any required method if somewhere necessary to emulate Scene API

		const Matrix GetWorldTransMatrix(NODE_ID nodeID)
		{
			return *GetWorldMatrix(nodeID);
		}

		template <class NodeType>
		void GetWorldTransMatrices(const std::vector<VRUT::smart_pointer<NodeType> > &in_nodes, std::vector<Matrix> &out_matrices)
		{
			// Allocate memory in output array
			if (out_matrices.size() < in_nodes.size())
				out_matrices.resize(in_nodes.size());

			for (unsigned int i = 0; i < in_nodes.size(); ++i)
			{
				// If some node does not exist (or something like that), we skip the entry in both in_nodes and out_matrices,
				// so that matrices in out_matrices correspond to nodes in in_nodes
				if (in_nodes[i].getPtr() != nullptr)
					out_matrices[i] = *GetWorldMatrix(in_nodes[i]->GetID());
			}
		}

		template <class NodeType>
		std::vector<Matrix> GetWorldTransMatrices(const std::vector<VRUT::smart_pointer<NodeType> > &in_nodes)
		{
			std::vector<Matrix> result;
			GetWorldTransMatrices(in_nodes, result);
			return result;		// RVO or move constructor are used
		}

		//get all nodes (also non active)
		template <class NodeType>
		void GetNodes(std::vector<smart_pointer<NodeType> > &nodes, const SceneNode::SCENENODE_TYPE type)
		{
			//scene->GetNodes(nodes, type);
			shadowQueue.clear();

			SceneNodeShadow *node = GetNode(GetRootID());
			if (!node)
				return;
			std::vector<NODE_ID> nodeIDs;
			while (true)
			{
				if (!node)
				{
					if (!shadowQueue.empty())
					{
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				if (node->GetType() == type)
				{
					nodeIDs.push_back(node->GetID());
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), Matrix()));
				if (!node->GetChildren()->empty())
				{
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}
			if (!nodeIDs.empty())
				scene->GetNodes(nodes, nodeIDs);
		}

		template <class NodeType>
		void GetNodesActive(std::vector<smart_pointer<NodeType> > &nodes, const SceneNode::SCENENODE_TYPE type)
		{
			shadowQueue.clear();

			SceneNodeShadow *node = GetNode(GetRootID());
			if (!node)
				return;
			std::vector<NODE_ID> nodeIDs;
			while (true)
			{
				if (!node || (!node->IsActive()))
				{
					if (!shadowQueue.empty())
					{
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
						continue;
					}
					break;
				}
				if (node->GetType() == type)
				{
					nodeIDs.push_back(node->GetID());
				}
				for (size_t i = 1; i < node->GetChildren()->size(); ++i)
					shadowQueue.push_back(std::pair<NODE_ID, Matrix>(node->GetChildren()->at(i), Matrix()));
				if (!node->GetChildren()->empty())
				{
					node = GetNode(node->GetChildren()->at(0));
				}
				else
				{
					if (!shadowQueue.empty())
					{
						node = GetNode(shadowQueue.back().first);
						shadowQueue.pop_back();
					}
					else
						break;
				}
			}
			if (!nodeIDs.empty())
				scene->GetNodes(nodes, nodeIDs);
		}
		
		template <class GeometryType>
		const void GetGeometry(smart_pointer<GeometryType> & geo, GEOMETRY_ID gid) const
		{
			scene->GetGeometry(geo, gid);
		}

		void GetGeometries(const std::vector<smart_pointer<GeometryNode> > &gnodes, std::vector<smart_pointer<Geometry> > &geoms) const
		{
			scene->GetGeometries(gnodes, geoms);
		}

	};
}
#endif
