#include "Bvh.h"
#include "Camera.h"
#include "Plane3.h"
#include "glInterface.h"
#include "Triangle3.h"
#include "SceneEntity.h"
#include "Geometry.h"
#include "RenderState.h"
#include "Material.h"
#include "BvhConstructor.h"
#include "gzstream.h"

#include <queue>
#include <stack>
#include <fstream>
#include <iostream>
#include <iomanip>


#ifdef _CRT_SET
	#define _CRTDBG_MAP_ALLOC
	#include <stdlib.h>
	#include <crtdbg.h>

	// redefine new operator
	#define DEBUG_NEW new(_NORMAL_BLOCK, __FILE__, __LINE__)
	#define new DEBUG_NEW
#endif

#define INVALID_TEST ((unsigned int)-1)

using namespace std;


namespace CHCDemoEngine
{

int BvhNode::sCurrentState = 0;


/*
3 x---------x 2
  |\         \
  | \         \
  |  \         \
  |	4 x---------x 5
  |   |         | 
0 x   |     x 1 |
   \  |         |
    \ |         |
     \|         |
  	7 x---------x 6             
*/

static unsigned int sIndices[] =	
{7, // degenerated
 7, 6, 4, 5, 3, 2, 0, 1,
 1, 4, // degenerated
 4, 3, 7, 0, 6, 1, 5, 2,
 2 // degenerated
};


const static int NUM_INDICES_PER_BOX = 20;

/*	Order of vertices
	0 = (0, 0, 0)
	1 = (1, 0, 0)
	2 = (1, 1, 0)
	3 = (0, 1, 0)
	4 = (0, 1, 1)
	5 = (1, 1, 1)
	6 = (1, 0, 1)
	7 = (0, 0, 1)
*/

static Plane3 sNearPlane;
static float sNear;
static Frustum sFrustum;

/// these values are valid for all nodes
static int sClipPlaneAABBVertexIndices[12];

//#define ALIGN_INDICES


BvhNode::BvhNode(BvhNode *parent): 
mParent(parent),
mAxis(-1), 
mDepth(0), 
mFirst(-1), 
mLast(-1),
mNumTestNodes(1),
mTestNodesIdx(-1),
mIndicesPtr(-1),
mId(0),
mIsMaxDepthForVirtualLeaf(false),
mIsVirtualLeaf(false)
{
	for (int i = 0; i < NUM_STATES; ++ i)
	{
		mPlaneMask[i] = 0;
		mPreferredPlane[i]= 0;
		mLastRenderedFrame[i] = -1;
	}
}


BvhNode::~BvhNode() 
{
}


void BvhNode::ResetVisibility()
{
	for (int i = 0; i < NUM_STATES; ++ i)
	{
		mVisibility[i].Reset();
		mLastRenderedFrame[i] = -1;
		mPlaneMask[i] = 0;
		mPreferredPlane[i]= 0;
	}
}


void BvhNode::VisibilityInfo::Reset()
{
	mIsVisible = false;
	mAssumedVisibleFrameId = 0;
	mLastVisitedFrame = -1;
	mTimesInvisible = 0;
	mIsFrustumCulled = false;
	mIsNew = true;
	mLastQueriedFrame = -1;
}


BvhInterior::~BvhInterior() 
{
	DEL_PTR(mBack);
	DEL_PTR(mFront); 
}


BvhLeaf::~BvhLeaf()
{
}

BvhInterior::BvhInterior(BvhNode *parent):
mBack(NULL), mFront(NULL), BvhNode(parent)
{}


bool BvhInterior::IsLeaf() const
{
	return false; 
}



BvhLeaf::BvhLeaf(BvhNode *parent): BvhNode(parent)
{}


bool BvhLeaf::IsLeaf() const
{
	return true; 
}



/**********************************************************/
/*                class Bvh implementation                */
/**********************************************************/


Bvh::Bvh()
{
	Init();
}


Bvh::Bvh(const SceneEntityContainer &staticEntities, 
		 const SceneEntityContainer &dynamicEntities)
{
	Init();

	mGeometrySize = staticEntities.size() + dynamicEntities.size();
	mGeometry = new SceneEntity*[mGeometrySize];

	mStaticGeometrySize = staticEntities.size();
	mDynamicGeometrySize = dynamicEntities.size();

	for (size_t i = 0; i < mStaticGeometrySize; ++ i)
	{
		mGeometry[i] = staticEntities[i];
	}

	for (size_t i = 0; i < mDynamicGeometrySize; ++ i)
	{
		mGeometry[mStaticGeometrySize + i] = dynamicEntities[i];
	}

	//cout << "max depth for testing children " << mMaxDepthForTestingChildren << endl;
}


Bvh::Bvh(const SceneEntityContainer &staticEntities, 
		 const SceneEntityContainer &dynamicEntities,
		 int maxDepthForTestingChildren)
{
	Init();

	mGeometrySize = staticEntities.size() + dynamicEntities.size();
	mGeometry = new SceneEntity*[mGeometrySize];

	mStaticGeometrySize = staticEntities.size();
	mDynamicGeometrySize = dynamicEntities.size();

	for (size_t i = 0; i < mStaticGeometrySize; ++ i)
	{
		mGeometry[i] = staticEntities[i];
	}

	for (size_t i = 0; i < mDynamicGeometrySize; ++ i)
	{
		mGeometry[mStaticGeometrySize + i] = dynamicEntities[i];
	}

	mMaxDepthForTestingChildren = maxDepthForTestingChildren;
	//cout << "max depth for testing children " << mMaxDepthForTestingChildren << endl;
}


Bvh::~Bvh() 
{
	DEL_ARRAY_PTR(mVertices);
	DEL_ARRAY_PTR(mIndices);
	DEL_ARRAY_PTR(mTestIndices);
	DEL_ARRAY_PTR(mGeometry);

	if (mRoot) delete mRoot;
	// delete vbo
	glDeleteBuffersARB(1, &mVboId);
}


void Bvh::Init()
{
	mStaticRoot = NULL;
	mDynamicRoot = NULL;
	mRoot = NULL;

	mVertices = NULL;
	mIndices = NULL;
	mTestIndices = NULL;
	mCurrentIndicesPtr = 0;
	mNumNodes = 0;
	
	// nodes are tested using the subnodes from 3 levels below
	mMaxDepthForTestingChildren = 3;
	// the ratio of area between node and subnodes where 
	// testing the subnodes as proxy is still considered feasable
	mAreaRatioThreshold = 2.0f;
	//mAreaRatioThreshold = 1.4f;

	mVboId = -1;
	// bounds the maximal depth of the dynamic branch
	mMaxDepthForDynamicBranch = 10;
}



//////////////////////
//-- functions that are used during the main traversal

void Bvh::PullUpLastVisited(BvhNode *node, int frameId) const
{		
	BvhNode *parent = node->GetParent();

	while (parent && (parent->GetLastVisitedFrame() != frameId))
	{
		parent->SetLastVisitedFrame(frameId);
		parent = parent->GetParent();
	}
}


void Bvh::MakeParentsVisible(BvhNode *node)
{
	BvhNode *parent = node->GetParent();

	while (parent && (!parent->IsVisible()))
	{
		parent->SetVisible(true);
		parent = parent->GetParent();
	}
}


////////////////////////////////

void Bvh::CollectLeaves(BvhNode *node, BvhLeafContainer &leaves)
{
	stack<BvhNode *> tStack;
	tStack.push(node);

	while (!tStack.empty())
	{
		BvhNode *node = tStack.top();

		tStack.pop();

		if (!node->IsLeaf())
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			tStack.push(interior->mFront);
			tStack.push(interior->mBack);
		}
		else
		{
			leaves.push_back(static_cast<BvhLeaf *>(node));
		}
	}
}


int Bvh::CountNumNodes(BvhNode *node) const
{
	int numNodes = 0;

	stack<BvhNode *> tStack;

	tStack.push(node);

	while (!tStack.empty())
	{
		BvhNode *node = tStack.top();
		tStack.pop();

		++ numNodes;

		if (!node->IsLeaf())
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			tStack.push(interior->mFront);
			tStack.push(interior->mBack);
		}
	}

	return numNodes;
}


void Bvh::CollectNodes(BvhNode *node, BvhNodeContainer &nodes)
{
	stack<BvhNode *> tStack;

	tStack.push(node);

	while (!tStack.empty())
	{
		BvhNode *node = tStack.top();
		tStack.pop();

		nodes.push_back(node);

		if (!node->IsLeaf())
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			tStack.push(interior->mFront);
			tStack.push(interior->mBack);
		}
	}
}


typedef pair<BvhNode *, int> tPair;

void Bvh::CollectNodes(BvhNode *root, BvhNodeContainer &nodes, int depth)
{
	stack<tPair> tStack;
	tStack.push(tPair(root, 0));

	while (!tStack.empty())
	{
		BvhNode *node = tStack.top().first;
		const int d = tStack.top().second;

		tStack.pop();

		// found node in specified depth => take this node
		if ((d == depth) || (node->IsLeaf()))
		{
			nodes.push_back(node);
		}
		else
		{ 
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			tStack.push(tPair(interior->mFront, d + 1));
			tStack.push(tPair(interior->mBack, d + 1));
		}
	}
}


SceneEntity **Bvh::GetGeometry(BvhNode *node, int &geometrySize) const
{
	geometrySize = node->CountPrimitives();
	return mGeometry + node->mFirst;
}


bool Bvh::TestPlane(BvhNode *node, int i, bool &bIntersect)
{
	// do the test only if necessary
	if (!(node->mPlaneMask[BvhNode::sCurrentState] & (1 << i)))
	{
		return true;
	}


	////////
	//-- test the n-vertex
		
	if ((node->mBox.GetDistance(sClipPlaneAABBVertexIndices[i * 2 + 0], sFrustum.mClipPlanes[i]) > 0.0f))
	{
		// outside
		node->mPreferredPlane[BvhNode::sCurrentState] = i;
		return false;
	}

	////////////
	//-- test the p-vertex

	if (node->mBox.GetDistance(sClipPlaneAABBVertexIndices[i * 2 + 1], sFrustum.mClipPlanes[i]) <= 0.0f)
	{
		// completely inside: children don't need to check against this plane no more
		node->mPlaneMask[BvhNode::sCurrentState] ^= 1 << i;
	}
	else 
	{
		bIntersect = true;
	}
	
	return true;
}


int	Bvh::IsWithinViewFrustum(BvhNode *node)
{
	bool bIntersect	= false;

	if (node->GetParent())
		node->mPlaneMask[BvhNode::sCurrentState] = node->GetParent()->mPlaneMask[BvhNode::sCurrentState];

	////////
	//-- apply frustum culling for the planes with index mPreferredPlane to 6

	for (int i = node->mPreferredPlane[BvhNode::sCurrentState]; i < 6; ++ i)
		if (!TestPlane(node, i, bIntersect)) return 0;
	

	//////////
	//-- apply frustum culling for the planes with index 0 to mPreferredPlane

	for (int i = 0; i < node->mPreferredPlane[BvhNode::sCurrentState]; ++ i)
		if (!TestPlane(node, i, bIntersect)) return 0;
	
	return bIntersect ? -1 : 1;
}


static void CalcNPVertexIndices(const Frustum &frustum, int *indices)
{
	for (int i = 0; i < 6; ++ i)
	{
		// n-vertex
		indices[i * 2 + 0] = AxisAlignedBox3::GetIndexNearestVertex(frustum.mClipPlanes[i].mNormal);
		// p-vertex
		indices[i * 2 + 1] = AxisAlignedBox3::GetIndexFarthestVertex(frustum.mClipPlanes[i].mNormal);	
	}
}


void Bvh::InitFrame(Camera *cam, RenderState *state)
{
	// = 0011 1111 which means that at the beginning, all six planes have to frustum culled
	mRoot->mPlaneMask[BvhNode::sCurrentState] = 0x3f;

	cam->CalcFrustum(sFrustum);
	CalcNPVertexIndices(sFrustum, sClipPlaneAABBVertexIndices);

	// store near plane
	sNearPlane = Plane3(cam->GetDirection(), cam->GetPosition());
	sNear = cam->GetNear();

	// update dynamic part of the hierarchy
	//if (!mDynamicEntities.empty())
	if (mDynamicGeometrySize)
	{
		UpdateBoundingBoxes(mDynamicRoot);
		UpdateDynamicBounds(state);
	}
}


void Bvh::UpdateDistance(BvhNode *node) const
{
	// q: should we use distance to center rather than the distance to the near plane?
	// distance to near plane can also be used for checking near plane intersection
	//node->mDistance = sNearPlane.Distance(node->GetBox().Center());
	node->mDistance = node->GetBox().GetMinDistance(sNearPlane);
}


float Bvh::CalcMaxDistance(BvhNode *node) const
{
#if 1
	return node->GetBox().GetMaxDistance(sNearPlane);

#else
	// use bounding boxes of geometry to determine max dist
	float maxDist = .0f;

	int geometrySize;
	SceneEntity **entities = GetGeometry(node, geometrySize);

	for (int i = 0; i < geometrySize; ++ i)
	{
		SceneEntity *ent = entities[i];
		float dist = ent->GetWorldBoundingBox().GetMaxDistance(sNearPlane);

		if (dist > maxDist) maxDist = dist;
	}

	return maxDist;
#endif
}


void Bvh::RenderBounds(BvhNode *node, RenderState *state, bool useTightBounds)
{
	// hack: use dummy contayiner as wrapper in order to use multibox function
	static BvhNodeContainer dummy(1);
	dummy[0] = node;
	RenderBounds(dummy, state, useTightBounds);
}


int Bvh::RenderBounds(const BvhNodeContainer &nodes, 
					  RenderState *state, 
					  bool useTightBounds)
{
	int renderedBoxes;

	if (!useTightBounds)
	{
		// if not using tight bounds, rendering boxes in immediate mode
		// is preferable to vertex arrays (less setup time)
		BvhNodeContainer::const_iterator nit, nit_end = nodes.end();
			
		for (nit = nodes.begin(); nit != nit_end; ++ nit)
		{
			RenderBoundingBoxImmediate((*nit)->GetBox());
		}

		renderedBoxes = (int)nodes.size();
	}
	else
	{
		renderedBoxes = PrepareBoundsWithDrawArrays(nodes);
		RenderBoundsWithDrawArrays(renderedBoxes, state);
	}

	return renderedBoxes;
}


int Bvh::PrepareBoundsWithDrawArrays(const BvhNodeContainer &nodes)
{
	///////////////////
	//-- for the first time we come here => create vbo and indices

	if (!mIndices)
	{	
		// create list of indices
		CreateIndices();
	}

	if (mVboId == -1)
	{	
		// prepare the vbo 
		PrepareVertices();
	}

	///////////////

	int numNodes = 0;

	BvhNodeContainer::const_iterator nit, nit_end = nodes.end();

	for (nit = nodes.begin(); nit != nit_end; ++ nit)
	{
		BvhNode *node = *nit;
        const int numIndices = node->mNumTestNodes * NUM_INDICES_PER_BOX;
		
		// copy indices
		memcpy(mIndices + numNodes * NUM_INDICES_PER_BOX, 
			   mTestIndices + node->mIndicesPtr, 
			   numIndices * sizeof(unsigned int));

		numNodes += node->mNumTestNodes;
	}

	return numNodes;
}


void Bvh::RenderBoundsWithDrawArrays(int numNodes, RenderState *state)
{
	/////////
	//-- Render the vbo

	if (state->GetCurrentVboId() != mVboId)
	{
		glBindBufferARB(GL_ARRAY_BUFFER_ARB, mVboId);
		// set the vertex pointer to the vertex buffer
		glVertexPointer(3, GL_FLOAT, 0, (char *)NULL);	

		state->SetCurrentVboId(mVboId);
	}

	// we don't use the last or the first index (they are generate and only used to connect strips)
	int numElements = numNodes * NUM_INDICES_PER_BOX - 1;
	// don't render first degenerate index
	glDrawElements(GL_TRIANGLE_STRIP, numElements, GL_UNSIGNED_INT, mIndices + 1); 
}


void Bvh::CreateIndices()
{
	// collect bvh nodes
	BvhNodeContainer nodes;
	// first collect dynamic nodes so we make sure that they are in the beginning
	CollectNodes(mDynamicRoot, nodes);
	// then collect static nodes
	CollectNodes(mStaticRoot, nodes);
	//CollectNodes(mRoot, nodes);

	cout << "creating new indices" << endl;

	int numMaxIndices = 0;

	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit)
	{
		int offset = (*lit)->mNumTestNodes * NUM_INDICES_PER_BOX;
#ifdef ALIGN_INDICES
		// align with 32 in order to speed up memcopy
		offset = (offset / 32) * 32 + 32; 
#endif
		numMaxIndices += offset;
	}

	cout << "creating global indices buffer" << endl;

	if (mIndices)     delete [] mIndices;
	if (mTestIndices) delete [] mTestIndices;

	// global buffer: create it once so we don't have
	// to allocate memory for each individual chunk of the node
	mIndices = new unsigned int[numMaxIndices];
	// create new index buffer for the individual nodes
	mTestIndices = new unsigned int[numMaxIndices];
	
	mCurrentIndicesPtr = 0;

	for (lit = nodes.begin(); lit != lit_end; ++ lit)
	{
		BvhNode *node = *lit;
		
		// resize array
		node->mIndicesPtr = mCurrentIndicesPtr;
		int numIndices = 0;

		// the bounding boxes of the test nodes are rendered instead of the node itself
		// => store their indices
		for (int i = 0; i < node->mNumTestNodes; ++ i, numIndices += NUM_INDICES_PER_BOX)
		{
			BvhNode *testNode = mTestNodes[node->mTestNodesIdx + i];

			// add vertex indices of boxes to root node
			for (int j = 0; j < NUM_INDICES_PER_BOX; ++ j)
			{
				mTestIndices[mCurrentIndicesPtr + numIndices + j] = sIndices[j] + testNode->GetId() * 8;
			}
		}

		// align with 32
#ifdef ALIGN_INDICES
		const int offset = (numIndices / 32) * 32 + 32;
#else
		const int offset = numIndices;
#endif
		mCurrentIndicesPtr += offset;
	}
}


void Bvh::ComputeIds()
{
	// collect all nodes
	BvhNodeContainer nodes;
	//CollectNodes(mRoot, nodes);
	// first collect dynamic nodes so we make sure that they are in the beginning
	CollectNodes(mDynamicRoot, nodes);
	// then collect static nodes
	CollectNodes(mStaticRoot, nodes);
	// also add root
	nodes.push_back(mRoot);

	// assign ids to all nodes of the hierarchy
	int i = 0;
	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit, ++ i)
	{
		(*lit)->SetId(i);
	}
}


void Bvh::PrepareVertices()
{
	// collect all nodes
	BvhNodeContainer nodes;

	nodes.reserve(GetNumNodes());
	// first collect dynamic nodes so we make sure that they are in the beginning
	CollectNodes(mDynamicRoot, nodes);
	// then collect static nodes
	CollectNodes(mStaticRoot, nodes);
	// also add root
	nodes.push_back(mRoot);

	const unsigned int bufferSize = 8 * (int)nodes.size();
	mVertices = new Vector3[bufferSize];
	
	int i = 0;
	
	// store bounding box vertices
	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit, i += 8)
	{
		BvhNode *node = *lit;
		Vector3 v;

		for (int j = 0; j < 8; ++ j)
		{
			node->GetBox().GetVertex2(j, v);
			(static_cast<Vector3 *>(mVertices))[node->GetId() * 8 + j] = v;
		}
	}

	glGenBuffersARB(1, &mVboId);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, mVboId);
	glVertexPointer(3, GL_FLOAT, 0, (char *)NULL);
	
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, 
		            bufferSize * sizeof(Vector3), 
		            mVertices, 
					//GL_STATIC_DRAW_ARB);
					GL_DYNAMIC_DRAW_ARB);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	// data handled by graphics driver from now on
	DEL_ARRAY_PTR(mVertices);

	cout << "******** created vbos for tighter bounds *********" << endl;
}


void Bvh::UpdateDynamicBounds(RenderState *state)
{
	// vbos not created yet
	if (mVboId == -1) return;

	// collect all nodes
	static BvhNodeContainer nodes;
	nodes.clear();

	CollectNodes(mDynamicRoot, nodes);

	const unsigned int bufferSize = 8 * (int)nodes.size();
	if (!mVertices)	mVertices = new Vector3[bufferSize];
	
	int i = 0;
	
	// store bounding box vertices
	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit, i += 8)
	{
		BvhNode *node = *lit;

		for (int j = 0; j < 8; ++ j)
			(static_cast<Vector3 *>(mVertices))[node->GetId() * 8 + j] = node->GetBox().GetVertex(j);
	}
	
	if (state->GetCurrentVboId() != mVboId)
	{
		glBindBufferARB(GL_ARRAY_BUFFER_ARB, mVboId);
		// set the vertex pointer to the vertex buffer
		glVertexPointer(3, GL_FLOAT, 0, (char *)NULL);	
		state->SetCurrentVboId(mVboId);
	}

	glBufferSubDataARB(GL_ARRAY_BUFFER_ARB, 0,
					   bufferSize * sizeof(Vector3), 
					   mVertices);
}


void Bvh::SetMaxDepthForTestingChildren(int maxDepth) 
{
	if (maxDepth != mMaxDepthForTestingChildren)
	{
		mMaxDepthForTestingChildren = maxDepth;
		RecomputeBounds();
	}
}


void Bvh::SetAreaRatioThresholdForTestingChildren(float ratio) 
{
	if (ratio != mAreaRatioThreshold)
	{
		mAreaRatioThreshold = ratio;
		RecomputeBounds();
	}
}


void Bvh::RecomputeBounds()
{
	// collect all nodes
	BvhNodeContainer nodes;
	CollectNodes(mRoot, nodes);

	cout << "\nrecomputing bounds, children will be tested in depth " << mMaxDepthForTestingChildren << endl;

	int success = 0;
	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit)
	{
		BvhNode *node = *lit;

		// recreate list of nodes that will be queried as a proxy
		if (CreateNodeRenderList(node))
			++ success;
	}

	float p = 100.0f * (float)success / nodes.size();
	cout << "created tighter bounds for " << p << " percent of the nodes" << endl;

	// recreate indices used for indirect rendering mode
	if (mIndices) CreateIndices();
}

	
bool Bvh::CreateNodeRenderList(BvhNode *node)
{
	BvhNodeContainer children;

	// collect nodes that will be tested instead of the leaf node
	// in order to get a tighter bounding box test
	CollectNodes(node, children, mMaxDepthForTestingChildren);
	

	// using the tighter bounds is not feasable in case
	// that the tighter bounds represent nearly the same projected area
	// as the old bounding box. Test this property using either 
	// volume or area heuristics

	float vol = 0;
	float area = 0;

	BvhNodeContainer::const_iterator cit;

	for (cit = children.begin(); cit != children.end(); ++ cit)
		area += (*cit)->GetBox().SurfaceArea();

	const float areaRatio = area / node->GetBox().SurfaceArea();
	
	bool success;

	if (areaRatio < mAreaRatioThreshold)
		success = true;
	else
	{
		// hack: only store node itself
		children.clear();
		children.push_back(node);

		success = false;
	}

	// the new test nodes are added at the end of the vector
	node->mTestNodesIdx = (int)mTestNodes.size();

	// use the collected nodes as proxy for the occlusion tests
	for (cit = children.begin(); cit != children.end(); ++ cit)
	{
		BvhNode *child = *cit;
		mTestNodes.push_back(child);
	}

	node->mNumTestNodes = (int)children.size();

	return success;
}


void Bvh::ResetNodeClassifications()
{
	BvhNodeContainer nodes;

	nodes.reserve(GetNumNodes());
	CollectNodes(mRoot, nodes);

	BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

	for (lit = nodes.begin(); lit != lit_end; ++ lit)
	{
		(*lit)->ResetVisibility();
	}
}


void Bvh::ComputeBvhStats(BvhNode *root, BvhStats &bvhStats) 
{
	bvhStats.Reset();
	std::stack<BvhNode *> nodeStack;
	nodeStack.push(root);

	int numVirtualLeaves = 0;
	int numGeometry = 0;

	while (!nodeStack.empty()) 
	{
		BvhNode *node = nodeStack.top();
		nodeStack.pop();

		if (node->IsVirtualLeaf()) 
		{
			++ numVirtualLeaves;
			numGeometry += node->CountPrimitives();

			BvhLeaf *leaf = static_cast<BvhLeaf *>(node);

			bvhStats.mTriangles += CountTriangles(leaf);
			bvhStats.mLeafSA += leaf->mBox.SurfaceArea();
			bvhStats.mLeafVol += leaf->mBox.GetVolume();
		} 
		else 
		{
			bvhStats.mInteriorSA += node->mBox.SurfaceArea();
			bvhStats.mInteriorVol += node->mBox.GetVolume();

			BvhInterior *interior = static_cast<BvhInterior *>(node);
			
			nodeStack.push(interior->mBack);
			nodeStack.push(interior->mFront);
		}
	}

	bvhStats.mGeometryRatio = (float)numGeometry / numVirtualLeaves;
	bvhStats.mTriangleRatio = (float)bvhStats.mTriangles / numVirtualLeaves;
	bvhStats.mLeaves = numVirtualLeaves;
}


void Bvh::PrintBvhStats(const BvhStats &bvhStats, BvhNode *root) const
{
	cout << "interiorNodesSA = " << bvhStats.mInteriorSA / root->mBox.SurfaceArea() << endl;
	cout << "leafNodesSA = " << bvhStats.mLeafSA / root->mBox.SurfaceArea() << endl;
	cout << "interiorNodesVolume = " << bvhStats.mInteriorVol / root->mBox.GetVolume() << endl;
	cout << "leafNodesVolume = " << bvhStats.mLeafVol / root->mBox.GetVolume() << endl;

	cout << "geometry per leaf: " << bvhStats.mGeometryRatio << endl;
	cout << "triangles per leaf: " << bvhStats.mTriangleRatio << endl;
	cout << "===========================================" << endl << endl;
}


int Bvh::CountTriangles(BvhNode *node) const
{
	int numTriangles = 0;

	for (int i = node->mFirst; i <= node->mLast; ++ i)
	{
		numTriangles += mGeometry[i]->CountNumTriangles(0);
	}

	return numTriangles;
}


float Bvh::GetArea(BvhNode *node) const
{
	return node->mArea;
}


void Bvh::UpdateNumLeaves(BvhNode *node) const
{
	if (node->IsLeaf())
	{
		node->mNumLeaves = 1;
	}
	else
	{
		BvhNode *f = static_cast<BvhInterior *>(node)->GetFront();
		BvhNode *b = static_cast<BvhInterior *>(node)->GetBack();

		UpdateNumLeaves(f);
		UpdateNumLeaves(b);

		node->mNumLeaves = f->mNumLeaves + b->mNumLeaves;
	}
}


void Bvh::CollectVirtualLeaves(BvhNode *node, BvhNodeContainer &leaves)
{
	stack<BvhNode *> tStack;
	tStack.push(node);

	while (!tStack.empty())
	{
		BvhNode *node = tStack.top();
		tStack.pop();

		if (node->mIsVirtualLeaf)
		{
			leaves.push_back(node);
		}
		else if (!node->IsLeaf())
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			tStack.push(interior->mFront);
			tStack.push(interior->mBack);
		}
	}
}


void Bvh::SetVirtualLeaves(int numTriangles) 
{
	// first invalidate old virtual leaves
	BvhNodeContainer leaves;
	CollectVirtualLeaves(mRoot, leaves);

	BvhNodeContainer::const_iterator bit, bit_end = leaves.end();

	for (bit = leaves.begin(); bit != bit_end; ++ bit)
	{
		(*bit)->mIsVirtualLeaf = false;
	}

	mNumVirtualNodes = 0;
	// assign new virtual leaves based on specified #triangles per leaf
	std::stack<BvhNode *> nodeStack;
	nodeStack.push(mStaticRoot);
	nodeStack.push(mDynamicRoot);

	while (!nodeStack.empty()) 
	{
		BvhNode *node = nodeStack.top();
		nodeStack.pop();

		++ mNumVirtualNodes;

		if (node->IsLeaf()) 
		{
			BvhLeaf *leaf = static_cast<BvhLeaf *>(node);
			leaf->mIsVirtualLeaf = true;
		} 
		else 
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			BvhNode *f = interior->mFront;
			BvhNode *b = interior->mBack;

			if (node->mIsMaxDepthForVirtualLeaf || 
				(CountTriangles(node) <= numTriangles))
			{
				//cout << " l " << CountTriangles(node) << " " << node->mIsMaxDepthForVirtualLeaf << endl;
				node->mIsVirtualLeaf = true;
			}
			else
			{
				nodeStack.push(interior->mBack);
				nodeStack.push(interior->mFront);
			}
		}
	}

	/// Reset the node states
	ResetNodeClassifications();
}


void Bvh::ComputeMaxDepthForVirtualLeaves()
{
	// We initialize the maximal depth for virtual leaves
	// of this bvh, i.e., the nodes that are used as
	// leaves of the hierarchy during traversal.

	// Initially they are set either
	// a) to the real leaves of the hierarchy or
	// b) the point where the subdivision on object level ends
	// and the subsequent nodes are just used to provide tighter bounds
	// (see article for the notations)

	std::stack<BvhNode *> nodeStack;

	nodeStack.push(mStaticRoot);
	nodeStack.push(mDynamicRoot);

	while (!nodeStack.empty()) 
	{
		BvhNode *node = nodeStack.top();
		nodeStack.pop();

		if (node->IsLeaf()) 
		{
			node->mIsMaxDepthForVirtualLeaf = true;
		} 
		else 
		{
			BvhInterior *interior = static_cast<BvhInterior *>(node);

			BvhNode *f = interior->mFront;
			BvhNode *b = interior->mBack;

			if ((f->mFirst == b->mFirst) && (f->mLast == b->mLast))
			{
				// point reached where beyond there would be no further reduction 
				// as both subtrees contain the same objects => stop here
				// The tree beyond the current node is used to describe
				// tighter bounds on the geometry contained  in it
				node->mIsMaxDepthForVirtualLeaf = true;
			}
			else
			{
				nodeStack.push(f);
				nodeStack.push(b);
			}
		}
	}
}


// this function must be called once after hierarchy creation
void Bvh::PostProcess() 
{
	CreateRoot();

	ComputeMaxDepthForVirtualLeaves();
	// set virtual leaves for specified number of triangles
	SetVirtualLeaves(INITIAL_TRIANGLES_PER_VIRTUAL_LEAVES);
	/// for each node compute the number of leaves under this node
	UpdateNumLeaves(mRoot);
	// compute new ids
	ComputeIds();
	// specify bounds for occlusion tests
	RecomputeBounds();

	mBox = mRoot->GetBox();

	// compute and print stats for whole (static + dynamic) bvh
	ComputeBvhStats(mRoot, mBvhStats);

	BvhStats staticBvhStats;
	ComputeBvhStats(mStaticRoot, staticBvhStats);
	cout << "\n============ Static BVH statistics =============" << endl;

	PrintBvhStats(staticBvhStats, mStaticRoot);

	// also output dynamic stats
	if (mDynamicGeometrySize)
	{
		BvhStats dynBvhStats;
		ComputeBvhStats(mDynamicRoot, dynBvhStats);
	
		cout << "\n=========== Dynamic BVH statistics: =========" << endl;
		PrintBvhStats(dynBvhStats, mDynamicRoot);
	}
}


void Bvh::RenderBoundingBoxImmediate(const AxisAlignedBox3 &box)
{
	const Vector3 l = box.Min();
	const Vector3 u = box.Max();

	///////////
	//-- render AABB as triangle strips

	glBegin(GL_TRIANGLE_STRIP);

	// render first half of AABB
	glVertex3f(l.x, l.y, u.z);
	glVertex3f(u.x, l.y, u.z);
	glVertex3f(l.x, u.y, u.z);
	glVertex3f(u.x, u.y, u.z);
	glVertex3f(l.x, u.y, l.z); 
	glVertex3f(u.x, u.y, l.z);
	glVertex3f(l.x, l.y, l.z); 
	glVertex3f(u.x, l.y, l.z);

	glPrimitiveRestartNV();

	// render second half of AABB
	glVertex3f(l.x, u.y, u.z); 
	glVertex3f(l.x, u.y, l.z);
	glVertex3f(l.x, l.y, u.z);
	glVertex3f(l.x, l.y, l.z);
	glVertex3f(u.x, l.y, u.z); 
	glVertex3f(u.x, l.y, l.z);
	glVertex3f(u.x, u.y, u.z); 
	glVertex3f(u.x, u.y, l.z);

	glEnd();
}


static void RenderBoxForViz(const AxisAlignedBox3 &box)
{
	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Min().x, box.Max().y, box.Min().z);
	glVertex3d(box.Max().x, box.Max().y, box.Min().z);
	glVertex3d(box.Max().x, box.Min().y, box.Min().z);
	glVertex3d(box.Min().x, box.Min().y, box.Min().z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Min().x, box.Min().y, box.Max().z);
	glVertex3d(box.Max().x, box.Min().y, box.Max().z);
	glVertex3d(box.Max().x, box.Max().y, box.Max().z);
	glVertex3d(box.Min().x, box.Max().y, box.Max().z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Max().x, box.Min().y, box.Min().z);
	glVertex3d(box.Max().x, box.Min().y, box.Max().z);
	glVertex3d(box.Max().x, box.Max().y, box.Max().z);
	glVertex3d(box.Max().x, box.Max().y, box.Min().z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Min().x, box.Min().y, box.Min().z);
	glVertex3d(box.Min().x, box.Min().y, box.Max().z);
	glVertex3d(box.Min().x, box.Max().y, box.Max().z);
	glVertex3d(box.Min().x, box.Max().y, box.Min().z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Min().x, box.Min().y, box.Min().z);
	glVertex3d(box.Max().x, box.Min().y, box.Min().z);
	glVertex3d(box.Max().x, box.Min().y, box.Max().z);
	glVertex3d(box.Min().x, box.Min().y, box.Max().z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	glVertex3d(box.Min().x, box.Max().y, box.Min().z);
	glVertex3d(box.Max().x, box.Max().y, box.Min().z);
	glVertex3d(box.Max().x, box.Max().y, box.Max().z);
	glVertex3d(box.Min().x, box.Max().y, box.Max().z);

	glEnd();
}


static Technique GetVizTechnique()
{
	Technique tech;
	tech.Init();

	//tech.SetLightingEnabled(false);
	//tech.SetDepthWriteEnabled(false);

	tech.SetEmmisive(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));
	tech.SetDiffuse(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));
	tech.SetAmbient(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));

	return tech;
}


void Bvh::RenderBoundsForViz(BvhNode *node, 
							 RenderState *state, 
							 bool useTightBounds)
{
	Technique *oldTech = state->GetState();
	// we set a simple material
	static Technique boxMat = GetVizTechnique();
	boxMat.Render(state);

	if (!useTightBounds)
	{
#if 1
		RenderBoxForViz(node->GetBox());
#else
		glPolygonMode(GL_FRONT, GL_LINE);
		RenderBoundingBoxImmediate(node->GetBox());
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
#endif
	}
	else
	{
		for (int i = 0; i < node->mNumTestNodes; ++ i)
		{
			RenderBoxForViz(mTestNodes[node->mTestNodesIdx + i]->GetBox());
		}
	}

	if (oldTech) oldTech->Render(state);
}




bool Bvh::IntersectsNearPlane(BvhNode *node) const
{ 
	// note: we have problems with large scale object penetrating the near plane 
	// (e.g., objects in the distance which are always handled to be visible)
	// especially annoying is this problem when using the frustum 
	// fitting on the visible objects for shadow mapping
	// but don't see how to solve this issue without using costly calculations
	
	// we stored the near plane distance => we can use it also here
	float distanceToNearPlane = node->GetDistance();
	//float distanceToNearPlane = node->GetBox().GetMinDistance(sNearPlane);
	
	return (distanceToNearPlane < sNear);
}


void Bvh::CreateRoot()
{
	// create new root
	mRoot = new BvhInterior(NULL);

	// the separation is a purely logical one
	// the bounding boxes of the child nodes are 
	// identical to those of the root node

	mRoot->mBox = mStaticRoot->mBox;
	mRoot->mBox.Include(mDynamicRoot->mBox);

	mRoot->mArea = mRoot->mBox.SurfaceArea();

	mRoot->mFirst = 0;
	mRoot->mLast = (int)mGeometrySize - 1;

	//cout<<"f: " << mRoot->mFirst<< " l: " <<mRoot->mLast << endl;
	// add static root on left subtree
	mRoot->mFront = mStaticRoot;
	mStaticRoot->mParent = mRoot;

	// add dynamic root on left subtree
	mRoot->mBack = mDynamicRoot;
	mDynamicRoot->mParent = mRoot;
}
  



////////////////////////
//-- dynamic hierarchy stuff


void Bvh::UpdateBoundingBoxes(BvhNode *node)
{
	if (node->IsLeaf())
	{
		int numEntities;
		SceneEntity **entities = GetGeometry(node, numEntities);

		node->mBox = SceneEntity::ComputeBoundingBox(entities, numEntities);
		//cout << "box: " << node->mBox << endl;
	}
	else
	{
		BvhNode *f = static_cast<BvhInterior *>(node)->GetFront();
		BvhNode *b = static_cast<BvhInterior *>(node)->GetBack();

		UpdateBoundingBoxes(f);
		UpdateBoundingBoxes(b);

		node->mBox = f->mBox;
		node->mBox.Include(b->mBox);
	}
}


void Bvh::CreateDynamicBranch()
{
	// the bvh has two main branches
	// a static branch (the old root), and a dynamic branch 
	// we create a 'dynamic' leaf which basically is a container
	// for all dynamic objects underneath

	// the bounding boxes of the dynamic tree must be updated
	// once each frame in order to be able to incorporate
	// the movements of the objects within

	
	cout << "\n==============================" << endl;
	cout << "creating dynamic bvh branch"  << endl;

	DEL_PTR(mDynamicRoot);
	-- mNumNodes;

	BvhConstructor bvhConstructor(mGeometry, (int)mStaticGeometrySize, (int)mGeometrySize - 1);

	int numNodes;
	mDynamicRoot = bvhConstructor.Construct(numNodes);

	mNumNodes += numNodes;
}




}