/*
-----------------------------------------------------------------------------
This source file is part of the GameTools Project
http://www.gametools.org

Author: Martin Szydlowski
-----------------------------------------------------------------------------
*/

#include "OgreBiHierarchy.h"
#include "OgreBihRenderable.h"
#include "OgreBiHierarchySceneNode.h"
#include "OgreBiHierarchySceneManager.h"

#include <OgreStringConverter.h>
#include <OgreLogManager.h>
#include <OgreRoot.h>
#include <OgreTimer.h>

#include <OgreMaterialManager.h>

#define PLT_SIZE 101

namespace Ogre
{

enum BihIntersection
{
	OUTSIDE=0,
	INSIDE=1,
	INTERSECT=2
};

static BihIntersection intersect( const Ray &one, const AxisAlignedBox &two )
{
	// Null box?
	if (two.isNull()) return OUTSIDE;

	bool inside = true;
	const Vector3* pCorners = two.getAllCorners();
	Vector3 origin = one.getOrigin();
	Vector3 dir = one.getDirection();

	Vector3 maxT(-1, -1, -1);

	int i = 0;
	for(i=0; i<3; i++ )
	{
		if( origin[i] < pCorners[0][i] )
		{
			inside = false;
			if( dir[i] > 0 )
			{
				maxT[i] = (pCorners[0][i] - origin[i])/ dir[i];
			}
		}
		else if( origin[i] > pCorners[4][i] )
		{
			inside = false;
			if( dir[i] < 0 )
			{
				maxT[i] = (pCorners[4][i] - origin[i]) / dir[i];
			}
		}
	}

	if( inside )
	{
		return INTERSECT;
	}
	int whichPlane = 0;
	if( maxT[1] > maxT[whichPlane])
		whichPlane = 1;
	if( maxT[2] > maxT[whichPlane])
		whichPlane = 2;

	if( ((int)maxT[whichPlane]) & 0x80000000 )
	{
		return OUTSIDE;
	}
	for(i=0; i<3; i++ )
	{
		if( i!= whichPlane )
		{
			float f = origin[i] + maxT[whichPlane] * dir[i];
			if ( f < (pCorners[0][i] - 0.00001f) ||
				f > (pCorners[4][i] +0.00001f ) )
			{
				return OUTSIDE;
			}
		}
	}

	return INTERSECT;

}


/** Checks how the second box intersects with the first.
*/
static BihIntersection intersect( const PlaneBoundedVolume &one, const AxisAlignedBox &two )
{
	// Null box?
	if (two.isNull()) return OUTSIDE;

	// Get corners of the box
	const Vector3* pCorners = two.getAllCorners();

	// For each plane, see if all points are on the negative side
	// If so, object is not visible.
	// If one or more are, it's partial.
	// If all aren't, full
	int corners[ 8 ] = {0, 4, 3, 5, 2, 6, 1, 7};
	bool all_inside = true;
	PlaneList::const_iterator i, iend;
	iend = one.planes.end();
	for (i = one.planes.begin(); i != iend; ++i)
	{
		const Plane& plane = *i;
		bool all_outside = true;

		float distance = 0;

		for ( int corner = 0; corner < 8; ++corner )
		{
			distance = plane.getDistance( pCorners[ corners[ corner ] ] );
			all_outside = all_outside && ( distance < 0 );
			all_inside = all_inside && ( distance >= 0 );

			if ( !all_outside && !all_inside )
				break;
		}

		if ( all_outside )
			return OUTSIDE;
	}

	if ( all_inside )
		return INSIDE;
	else
		return INTERSECT;

}


/** Checks how the second box intersects with the first.
*/
static BihIntersection intersect( const AxisAlignedBox &one, const AxisAlignedBox &two )
{
	// Null box?
	if (one.isNull() || two.isNull()) return OUTSIDE;

	const Vector3 * outside = one.getAllCorners();
	const Vector3 *inside = two.getAllCorners();

	if ( inside[ 4 ].x < outside[ 0 ].x ||
		inside[ 4 ].y < outside[ 0 ].y ||
		inside[ 4 ].z < outside[ 0 ].z ||
		inside[ 0 ].x > outside[ 4 ].x ||
		inside[ 0 ].y > outside[ 4 ].y ||
		inside[ 0 ].z > outside[ 4 ].z )
	{
		return OUTSIDE;
	}

	bool full = ( inside[ 0 ].x > outside[ 0 ].x &&
		inside[ 0 ].y > outside[ 0 ].y &&
		inside[ 0 ].z > outside[ 0 ].z &&
		inside[ 4 ].x < outside[ 4 ].x &&
		inside[ 4 ].y < outside[ 4 ].y &&
		inside[ 4 ].z < outside[ 4 ].z );

	if ( full )
		return INSIDE;
	else
		return INTERSECT;

}

/** Checks how the box intersects with the sphere.
*/
static BihIntersection intersect( const Sphere &one, const AxisAlignedBox &two )
{
	// Null box?
	if (two.isNull()) return OUTSIDE;

	float sradius = one.getRadius();

	sradius *= sradius;

	Vector3 scenter = one.getCenter();

	const Vector3 *corners = two.getAllCorners();

	float s, d = 0;

	Vector3 mndistance = ( corners[ 0 ] - scenter );
	Vector3 mxdistance = ( corners[ 4 ] - scenter );

	if ( mndistance.squaredLength() < sradius &&
		mxdistance.squaredLength() < sradius )
	{
		return INSIDE;
	}

	//find the square of the distance
	//from the sphere to the box
	for ( int i = 0 ; i < 3 ; i++ )
	{
		if ( scenter[ i ] < corners[ 0 ][ i ] )
		{
			s = scenter[ i ] - corners[ 0 ][ i ];
			d += s * s;
		}

		else if ( scenter[ i ] > corners[ 4 ][ i ] )
		{
			s = scenter[ i ] - corners[ 4 ][ i ];
			d += s * s;
		}

	}

	bool partial = ( d <= sradius );

	if ( !partial )
	{
		return OUTSIDE;
	}

	else
	{
		return INTERSECT;
	}
}

Real BihPlaneEvent::KT = 2.0;
Real BihPlaneEvent::KI = 2.0;

//----------------------------------------------------------------------------
// determine if this event is left or right of the reference event
// if the absolute flag is set, we decide by the objectcenter wether the 
// event goes to left or right. 
void BihPlaneEvent::classify(const BihPlaneEvent& e, BihPlaneEvent::Side side, bool absolute)
{
	// only classify events of the same dimension
	if (mDimension == e.mDimension)
	{
		if (!absolute)
		{
			if (mType == PET_END && mPosition[mDimension] <= e.mPosition[e.mDimension])
			{
				mRenderable->setSide(PES_LEFT);
				return;
			}
			else if (mType == PET_START && mPosition[mDimension] >= e.mPosition[e.mDimension])
			{
				mRenderable->setSide(PES_RIGHT);
				return;
			}
			else if (mType == PET_ON)
			{
				if (mPosition[mDimension] < e.mPosition[e.mDimension] ||
					(mPosition[mDimension] == e.mPosition[e.mDimension] && side == PES_LEFT))
				{
					mRenderable->setSide(PES_LEFT);
					return;
				}
				if (mPosition[mDimension] > e.mPosition[e.mDimension] ||
					(mPosition[mDimension] == e.mPosition[e.mDimension] && side == PES_RIGHT))
				{
					mRenderable->setSide(PES_RIGHT);
					return;
				}
			}
		} else
		{
			// The object lies on the splitting plane
			// we decide with the object center if it goes right or left. 
			Vector3 mMid=mRenderable->getBoundingBox().getCenter();
			float mMidDimension=0;
			switch (mDimension)
			{
				case BihPlaneEvent::PED_X:
					mMidDimension=mMid.x;
					break;
				case BihPlaneEvent::PED_Y:
					mMidDimension=mMid.y;
					break;
				case BihPlaneEvent::PED_Z:
					mMidDimension=mMid.z;
					break;
			}
			if (mMidDimension <= e.mPosition[e.mDimension])
			{
				mRenderable->setSide(PES_LEFT);
				return;
			}
			else if (mMidDimension > e.mPosition[e.mDimension])
			{
				mRenderable->setSide(PES_RIGHT);
				return;
			}
		}




	}
}

//----------------------------------------------------------------------------
// clip this event to an aabb (move it so that the plane on one of the box planes)
BihPlaneEvent BihPlaneEvent::clip(AxisAlignedBox& box, BihPlaneEvent::Dimension dim)
{
	Vector3 newpos = mPosition, min = box.getMinimum(), max = box.getMaximum();
	if (newpos[dim] < min[dim])
		newpos[dim] = min[dim];
	else if (newpos[dim] > max[dim])
		newpos[dim] = max[dim];

	return BihPlaneEvent(mRenderable, newpos, mDimension, mType);
}

//----------------------------------------------------------------------------
// the surface area heuristic to determine the cost of splitting the parent aabb
// along the plane represented by this event
void BihPlaneEvent::SAH(const AxisAlignedBox& parent, int nLeft, int nPlane, int nRight, BihSplitInfo& split)
{
	Real mu = splitBox(parent, split.bleft, split.bright);
	Real sav = surfaceArea(parent);
	Real pl = surfaceArea(split.bleft) / sav;
	Real pr = surfaceArea(split.bright) / sav;
	Real costl = splitCost(pl, pr, nLeft + nPlane, nRight, mu);
	Real costr = splitCost(pl, pr, nLeft, nPlane + nRight, mu);

	if (costl < costr)
	{
		split.cost = costl;
		split.side = PES_LEFT;
		split.nleft = nLeft + nPlane;
		split.nright = nRight;
	}
	else
	{
		split.cost = costr;
		split.side = PES_RIGHT;
		split.nleft = nLeft;
		split.nright = nPlane + nRight;

	}
	split.event = *this;
}

//----------------------------------------------------------------------------
// the surface area heuristic to determine the cost of splitting the parent aabb
// along the plane represented by this event modified for BV

void BihPlaneEvent::SAH(const AxisAlignedBox& parent, int nLeft, int nPlane, int nRight, BihSplitInfo& split,AxisAlignedBox left,AxisAlignedBox right)
{
	//Real mu = splitBox(parent, split.bleft, split.bright);
	split.bleft=left;
	split.bright=right;
	Vector3 bmin = parent.getMinimum();
	Vector3 bmax = parent.getMaximum();
	Real mu=lookupPenalty(
		(mPosition[mDimension] - bmin[mDimension]) / 
		(bmax[mDimension] - bmin[mDimension]));
	Real sav = surfaceArea(parent);
	Real pl = surfaceArea(split.bleft) / sav;
	Real pr = surfaceArea(split.bright) / sav;
	Real costl = splitCost(pl, pr, nLeft + nPlane, nRight, mu);
	Real costr = splitCost(pl, pr, nLeft, nPlane + nRight, mu);

	if (costl < costr)
	{
		split.cost = costl;
		split.side = PES_LEFT;
		split.nleft = nLeft + nPlane;
		split.nright = nRight;
	}
	else
	{
		split.cost = costr;
		split.side = PES_RIGHT;
		split.nleft = nLeft;
		split.nright = nPlane + nRight;

	}
	split.event = *this;
}

//----------------------------------------------------------------------------
// split the parent aabb with the plane in this event
Real BihPlaneEvent::splitBox(const AxisAlignedBox& parent, AxisAlignedBox& left, AxisAlignedBox& right)
{
	Vector3 bmin = parent.getMinimum();
	Vector3 bmax = parent.getMaximum();
	// calculate the penalty for spliting the box that way
	Real mu = lookupPenalty(
		(mPosition[mDimension] - bmin[mDimension]) / 
		(bmax[mDimension] - bmin[mDimension]));
	// set corners which are the same as parent AABB
	left.setMinimum(bmin);
	right.setMaximum(bmax);
	// modify "inner" corners
	bmin[mDimension] = mPosition[mDimension];
	bmax[mDimension] = mPosition[mDimension];
	// set the corners on the split plane
	left.setMaximum(bmax);
	right.setMinimum(bmin);

	return mu;
}

//----------------------------------------------------------------------------
// compute surface area of a box ... DUH!
Real BihPlaneEvent::surfaceArea(const AxisAlignedBox& box)
{
	Vector3 sides = box.getMaximum() - box.getMinimum();
	return	2 * sides.x * sides.y +
		2 * sides.y * sides.z +
		2 * sides.z * sides.x;
}

//----------------------------------------------------------------------------
// lookup the penalty for placing the splitting plane near to the edge of the AABB
// 0.0 <= p <= 1.0, p = 0.5 means the plane divides the aabb in half
Real BihPlaneEvent::lookupPenalty(Real p)
{
	// precomputed table of {x^6 + 1|0 <= x <= 1}
	static Real mPenaltyLookupTable[PLT_SIZE];
	static bool init_done = false;

	if (!init_done)
	{
		Real step = 1.0  / (PLT_SIZE - 1);
		Real x = 0.0; 
		//LogManager::getSingleton().logMessage("### Calculating Lookup Table ###");
		for (int i = 0; i < PLT_SIZE; i++)
		{
			mPenaltyLookupTable[i] = Math::Pow(x, 6) + 1.0;
			x += step;
			//LogManager::getSingleton().logMessage("### mPenaltyLookupTable[" + StringConverter::toString(i,3) + "]=" + StringConverter::toString(mPenaltyLookupTable[i]));
		}
		init_done = true;
		//LogManager::getSingleton().logMessage("### Lookup Table Calculated ###");
	}

	// normalize p to [0,1]
	Real x = Math::Abs(p * 2 - 1);
	// compute index
	int i = Math::IFloor(x * (PLT_SIZE - 1));

	return mPenaltyLookupTable[i];
}

//----------------------------------------------------------------------------
// compute cost of the split, reward splitting of empty space (lambda, const),
// penalize splitting off 'thin' slices (mu, const)
Real BihPlaneEvent::splitCost(Real pLeft, Real pRight, int nLeft, int nRight)
{
	// reward splitting off chunks of empty space
	Real lambda = 1.0;
	if (nLeft == 0 || nRight == 0)
	{
		lambda = 0.8;
	}

	// penalize splitting off small chunks (thin slices)
	Real mu = 1.0;
	if (pLeft < 0.1 || pRight < 0.1 || pLeft > 0.9 || pRight > 0.9)
	{
		mu = 1.5;
	}
	return lambda * mu * (KT + (KI * (pLeft*nLeft + pRight*nRight)));
}

//----------------------------------------------------------------------------
// compute cost of the split, reward splitting of empty space (lambda, const),
// penalize splitting off 'thin' slices (mu, parameter)
Real BihPlaneEvent::splitCost(Real pLeft, Real pRight, int nLeft, int nRight, Real mu)
{
	// reward splitting off chunks of empty space
	Real lambda = 1.0;
	if (nLeft == 0 || nRight == 0)
	{
		lambda = 0.8;
	}

	return lambda * mu * (KT + (KI * (pLeft*nLeft + pRight*nRight)));
}

//----------------------------------------------------------------------------
// surface area heuristic modified for the priority queue method of building
// the probabilities (p, pl, pr) are relative to the global (all enclosing) aabb
void BihPlaneEvent::pqSAH(Real globalSA, 
						  Real parentSA, 
						  int nLeft, 
						  int nPlane, 
						  int nRight, 
						  AxisAlignedBox& parentBox, 
						  BihSplitInfo& split)
{
	Real mu = splitBox(parentBox, split.bleft, split.bright);
	Real p = parentSA / globalSA;
	Real pl = surfaceArea(split.bleft) / globalSA;
	Real pr = surfaceArea(split.bright) / globalSA;
	Real costl = pqSplitCost(p, pl, pr, nLeft + nPlane, nRight, mu);
	Real costr = pqSplitCost(p, pl, pr, nLeft, nPlane + nRight, mu);

	if (costl < costr)
	{
		split.cost = costl;
		split.side = PES_LEFT;
		split.nleft = nLeft + nPlane;
		split.nright = nRight;
	}
	else
	{
		split.cost = costr;
		split.side = PES_RIGHT;
		split.nleft = nLeft;
		split.nright = nPlane + nRight;

	}
	split.event = *this;
}

//----------------------------------------------------------------------------
// compute split cost without any penalties
Real BihPlaneEvent::pqSplitCost(Real pParent, Real pLeft, Real pRight, int nLeft, int nRight, Real mu)
{
	return pParent * KT + (KI * (pLeft * nLeft + pRight * nRight));
}

//----------------------------------------------------------------------------
// DEBUG
String BihPlaneEvent::print()
{
	String dim, type;
	if (mDimension == PED_X)
		dim = "X";
	if (mDimension == PED_Y)
		dim = "Y";
	if (mDimension == PED_Z)
		dim = "Z";
	if (mType == PET_END)
		type = "END  ";
	if (mType == PET_ON)
		type = "ON   ";
	if (mType == PET_START)
		type = "START";
	//return StringConverter::toString(mPosition[mDimension]) + " " + dim + " " + type;
	return type + " " + dim + " " + StringConverter::toString(mPosition[mDimension]);
};

//----------------------------------------------------------------------------
String BihSplitInfo::print()
{
	String sside;
	if (side == BihPlaneEvent::PES_BOTH)
		sside = "BOTH ";
	if (side == BihPlaneEvent::PES_LEFT)
		sside = "LEFT ";
	if (side == BihPlaneEvent::PES_RIGHT)
		sside = "RIGHT";
	return "nLeft: " + StringConverter::toString(nleft, 4) + " nRight: " + 
		StringConverter::toString(nright, 4) + " Cost: " + StringConverter::toString(cost, 8, 9) +
		" Side: " + sside + " Event: " + event.print() + " Leftbox: " + 
		StringConverter::toString(bleft.getMinimum()) + ", " + 
		StringConverter::toString(bleft.getMaximum()) + " Rightbox: " +
		StringConverter::toString(bright.getMinimum()) + ", " + 
		StringConverter::toString(bright.getMaximum());
};

//----------------------------------------------------------------------------
String BiHierarchy::BihSubdivisionCandidate::print()
{		
	String sside;
	if (side == BihPlaneEvent::PES_BOTH)
		sside = "BOTH ";
	if (side == BihPlaneEvent::PES_LEFT)
		sside = "LEFT ";
	if (side == BihPlaneEvent::PES_RIGHT)
		sside = "RIGHT";
	return "List: " + StringConverter::toString(events->size(), 4) + " Objects: " +
		StringConverter::toString(nObjects, 4) + " Cost: " + StringConverter::toString(cost, 8, 9) + 
		" Decrease: " + StringConverter::toString(decrease, 8, 9) + " Side: " + sside + " Best: " +
		best->print() +  " Box: " + StringConverter::toString(aabb.getMinimum()) + ", "
		+ StringConverter::toString(aabb.getMaximum());

};

//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------

BiHierarchy::BiHierarchy(int maxdepth, BuildMethod bm): 
mMaxDepth(maxdepth), 
mBuildMethod(bm), 
mHiLiteLevel(0),
mShowAllBoxes(false),
mShowNodes(true),
mBihRoot(0), 
mBuildLog(0),
mAlgorithm(BIHAL_SAH)
{
	init();
}

BiHierarchy::BiHierarchy(int maxdepth, BuildMethod bm, int hilite, bool allboxes, bool shownodes):
mMaxDepth(maxdepth), 
mBuildMethod(bm), 
mHiLiteLevel(hilite),
mShowAllBoxes(allboxes),
mShowNodes(shownodes),
mBihRoot(0), 
mBuildLog(0),
mAlgorithm(BIHAL_SAH)
{
	init();
}

BiHierarchy::~BiHierarchy()
{
	delete mBihRoot;
}

void BiHierarchy::init()
{
	MaterialPtr mat;
	TextureUnitState *tex;

	// init visualization materials (unlit solid green/yellow)
	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxHiLite");
	if (mat.isNull())
	{
		ColourValue green(0, 1, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxHiLite", "General");
		//mat->setAmbient(green);
		//mat->setDiffuse(green);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, green, green);
	}

	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxLeft");
	if (mat.isNull())
	{
		ColourValue red(1, 0, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxLeft", "General");
		//mat->setAmbient(green);
		//mat->setDiffuse(green);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, red, red);
	}

	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxRight");
	if (mat.isNull())
	{
		ColourValue green(0, 1, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxRight", "General");
		//mat->setAmbient(green);
		//mat->setDiffuse(green);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, green, green);
	}
	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxLeftDark");
	if (mat.isNull())
	{
		ColourValue red(0.5, 0, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxLeftDark", "General");
		//mat->setAmbient(green);
		//mat->setDiffuse(green);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, red, red);
	}

	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxRightDark");
	if (mat.isNull())
	{
		ColourValue green(0, 0.5, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxRightDark", "General");
		//mat->setAmbient(green);
		//mat->setDiffuse(green);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, green, green);
	}

	mat = MaterialManager::getSingleton().getByName("BiHierarchy/BoxViz");
	if (mat.isNull())
	{
		ColourValue yellow(1, 1, 0);
		mat = MaterialManager::getSingleton().create("BiHierarchy/BoxViz", "General");
		//mat->setAmbient(yellow);
		//mat->setDiffuse(yellow);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, yellow, yellow);
	}
	mat = MaterialManager::getSingleton().getByName("BaseWhiteNoLighting");
	if (mat.isNull())
	{
		ColourValue white(1, 1, 0);
		mat = MaterialManager::getSingleton().create("BaseWhiteNoLighting", "General");
		//mat->setAmbient(yellow);
		//mat->setDiffuse(yellow);
		mat->setLightingEnabled(false);
		tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
		tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, white, white);
	}

	// retrieve or create build log
	try
	{
		mBuildLog = LogManager::getSingleton().getLog(BiHierarchy_LOGNAME);
	}
	catch (Exception&)
	{		
		mBuildLog = LogManager::getSingleton().createLog(BiHierarchy_LOGNAME);
	}

	setEnhancedVis(false);
}

/************************************************************************/
/* BiHierarchy insert/delete functions                                       */
/************************************************************************/

void BiHierarchy::remove(BihRenderable * rend)
{
	// DEBUG
	//LogManager::getSingleton().logMessage("-- Removing SceneNode: " + (static_cast<BiHierarchySceneNode *>(rend))->getName());
	std::queue<LeafPtr> cleanup;
	LeafPtr leaf;
	LeafSet ls = rend->getLeaves();
	LeafSet::iterator it = ls.begin();
	LeafSet::iterator end = ls.end();
	while (it != end)
	{
		cleanup.push(*it);
		it++;
	}
	while (!cleanup.empty())
	{
		leaf = cleanup.front();
		cleanup.pop();
		leaf->remove(rend);
		bool gone = rend->detachFrom(leaf);
		assert(gone && "Failed to detach leave which is abso-fuckin-lutely impossible!!!");
		//dump();
		recDelete(leaf);
		//dump();
	}
}

void BiHierarchy::recDelete(BiHierarchy::Node * node)
{
	if (node == 0) // DEBUG
	{
		OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
			"SNAFU while inserting BihRenderable into BiHierarchy.",
			"BiHierarchy::recInsert" );
		return;
	}

	if (node->isEmpty())
	{
		if (node == mBihRoot)
		{
			OGRE_DELETE(mBihRoot);
		} 
		else
		{
			BiHierarchy::Branch * parent = BIHBRANCHPTR_CAST(node->getParent());
			if (node == parent->mLeft)
			{
				OGRE_DELETE(parent->mLeft);
			}
			else if (node == parent->mRight)
			{
				OGRE_DELETE(parent->mRight);
			}
			recDelete(parent);
		}
	}
}

void BiHierarchy::insert(BihRenderable * rend)
{
	// make sure the tree exists
	if (mBihRoot)
	{
		// make sure the renderable is inside the world bounding box
		AxisAlignedBox aabb = rend->getBoundingBox();
		AxisAlignedBox isect = mBihRoot->mAABB.intersection(aabb);
		if (isect.getMinimum() == aabb.getMinimum() && isect.getMaximum() == aabb.getMaximum())
		{
			recInsert(mBihRoot, rend);
		}
		else
		{
			//LogManager::getSingleton().logMessage("Inserted node outside of world AABB.");
			BihRenderableList nodelist;
			nodelist.push_back(rend);
			addRendToList(mBihRoot, nodelist);
			OGRE_DELETE(mBihRoot);
			mBihRoot = buildFromList(nodelist, 0, AxisAlignedBox());
		}
	}
	// otherwise build a new one
	else
	{
		build(rend);
	}
}

void BiHierarchy::recInsert(BiHierarchy::Node * node, BihRenderable * rend)
{
	if (node == 0) // DEBUG
	{
		OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
			"SNAFU while inserting BihRenderable into BiHierarchy.",
			"BiHierarchy::recInsert" );
		return;
	}

	// rebuild the leaf/replace with subtree ...
	if (node->isLeaf())
	{
		//LogManager::getSingleton().logMessage("## Replacing leaf.");
		rebuildSubtree(node, rend);
	}
	else
	{
		BiHierarchy::Branch * branch = BIHBRANCHPTR_CAST(node);
		AxisAlignedBox aabb = rend->getBoundingBox();
		Plane::Side smin = branch->mSplitPlane->getSide(aabb.getMinimum());
		Plane::Side smax = branch->mSplitPlane->getSide(aabb.getMaximum());
		if (smin == smax)
		{
			if (smax == Plane::NEGATIVE_SIDE ||
				(smax == Plane::NO_SIDE && branch->mPlaneSide == BihPlaneEvent::PES_LEFT))
			{
				if (branch->mLeft)
					recInsert(branch->mLeft, rend);
				else
					recInsertNew(branch, BihPlaneEvent::PES_LEFT, rend);
			}
			else if (smin == Plane::POSITIVE_SIDE || 
				(smin == Plane::NO_SIDE && branch->mPlaneSide == BihPlaneEvent::PES_RIGHT))
			{
				if (branch->mRight)
					recInsert(branch->mRight, rend);
				else
					recInsertNew(branch, BihPlaneEvent::PES_RIGHT, rend);
			}
		}
		else
		{
			if (smin == Plane::NEGATIVE_SIDE && smax == Plane::NO_SIDE)
			{
				if (branch->mLeft)
					recInsert(branch->mLeft, rend);
				else
					recInsertNew(branch, BihPlaneEvent::PES_LEFT, rend);
			}
			else if (smax == Plane::POSITIVE_SIDE && smin == Plane::NO_SIDE)
			{
				if (branch->mRight)
					recInsert(branch->mRight, rend);
				else
					recInsertNew(branch, BihPlaneEvent::PES_RIGHT, rend);
			}
			else
			{
				// to avoid SNAFUs, rebuild whole subtree
				//LogManager::getSingleton().logMessage("## Rebuilding subtree for insertion");
				rebuildSubtree(node, rend);
			}
		}
	}
}

void BiHierarchy::recInsertNew(BiHierarchy::Branch * parent, BihPlaneEvent::Side side, BihRenderable * rend)
{
	//LogManager::getSingleton().logMessage("## Inserting into new subtree");

	AxisAlignedBox left, rigth, aabb;
	BihPlaneEventList events;
	int nObjects;
	
	rend->computeScene(events, aabb, nObjects, false);

	// override aabb
	splitBox(*parent, left, rigth);
	if (side == BihPlaneEvent::PES_LEFT)
	{
		aabb = left;
		if (mBuildMethod == BIHBM_RECURSIVE)
			parent->mLeft = recBuild(events, nObjects, aabb, parent);
		else if (mBuildMethod == BIHBM_PRIORITYQUEUE)
			parent->mLeft = pqBuild(events, nObjects, aabb, parent);
		else
		{
			OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
				"Invalid build method for the BiHierarchy.",
				"BiHierarchy::buildFromList");
			parent->mLeft = 0;
		}
		
	}
	else if (side == BihPlaneEvent::PES_RIGHT)
	{
		aabb = rigth;
		if (mBuildMethod == BIHBM_RECURSIVE)
			parent->mRight = recBuild(events, nObjects, aabb, parent);
		else if (mBuildMethod == BIHBM_PRIORITYQUEUE)
			parent->mRight = pqBuild(events, nObjects, aabb, parent);
		else
		{
			OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
				"Invalid build method for the BiHierarchy.",
				"BiHierarchy::buildFromList");
			parent->mRight = 0;
		}
	}
}

void BiHierarchy::rebuildSubtree(BiHierarchy::Node * node, BihRenderable * rend)
{
	//LogManager::getSingleton().logMessage("## Rebuilding subtree");

	AxisAlignedBox aabb = node->mAABB;
	
	BihRenderableList nodelist;
	nodelist.push_back(rend);
	addRendToList(node, nodelist);

	if (node == mBihRoot)
	{
		OGRE_DELETE(mBihRoot);
		mBihRoot = buildFromList(nodelist, 0, aabb);
	}
	else
	{
		BiHierarchy::Branch * parent = BIHBRANCHPTR_CAST(node->getParent());

		if (node == parent->mLeft)
		{
			OGRE_DELETE(parent->mLeft);
			parent->mLeft = buildFromList(nodelist, parent, aabb);
		}
		else if (node == parent->mRight)
		{
			OGRE_DELETE(parent->mRight);
			parent->mRight = buildFromList(nodelist, parent, aabb);
		}
		else
		{
			OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
				"SNAFU while inserting BihRenderable into BiHierarchy.",
				"BiHierarchy::recInsert" );
		}
	}
}

// compute both AABB then return the requested one.
void BiHierarchy::splitBox(const BiHierarchy::Branch& parent, AxisAlignedBox& left, AxisAlignedBox& right)
{
	Vector3 bmin = parent.mAABB.getMinimum();
	Vector3 bmax = parent.mAABB.getMaximum();
	Real pos = - parent.mSplitPlane->d;
	int dim = static_cast<int>(parent.mSplitPlane->normal.dotProduct(Vector3(0,1,2)));
	// set corners which are the same as parent AABB
	left.setMinimum(bmin);
	right.setMaximum(bmax);
	// modify "inner" corners
	bmin[dim] = pos;
	bmax[dim] = pos;
	// set the corners on the split plane
	left.setMaximum(bmax);
	right.setMinimum(bmin);
}

void BiHierarchy::addRendToList(BiHierarchy::Node * node, BihRenderableList& nodelist)
{
	if (node->isLeaf())
	{
		BiHierarchy::Leaf * leaf = BIHLEAFPTR_CAST(node);
		BihRenderableList::iterator it  = leaf->mBihRenderables.begin();
		BihRenderableList::iterator end = leaf->mBihRenderables.end();
		while (it != end)
		{
			nodelist.push_back(*it);
			it++;
		}
	}
	else
	{
		BiHierarchy::Branch * branch = BIHBRANCHPTR_CAST(node);
		if (branch->mLeft)
			addRendToList(branch->mLeft, nodelist);
		if (branch->mRight)
			addRendToList(branch->mRight, nodelist);
	}
}

/************************************************************************/
/*                BiHierarchy build functions                           */
/************************************************************************/

void BiHierarchy::build(BihRenderable * sceneRoot)
{
	Timer *timer = Root::getSingleton().getTimer();
	unsigned long t1, t2, t3, t4;

	mTreeStats.clear();
	
	// data we want to collect
	BihPlaneEventList events;
	AxisAlignedBox aabb;
	int nObjects = 0;

	t1 = timer->getMicroseconds(); // DEBUG
	sceneRoot->computeScene(events, aabb, nObjects);
	t2 = timer->getMicroseconds(); // DEBUG
	events.sort();
	t3 = timer->getMicroseconds(); // DEBUG

	mTreeStats.mNumSceneNodes = nObjects;

	assert(! aabb.isNull() && "Teh stubid worldAABB iz NULL ... waht now?");
	// hack to avoid SNAFU with "2-dimensional" scene
	aabb.merge(aabb.getMaximum()+Vector3(1,1,1));
	aabb.merge(aabb.getMinimum()+Vector3(-1,-1,-1));

	if (mBuildMethod == BIHBM_RECURSIVE)
		mBihRoot = recBuild(events, nObjects, aabb, 0);
	else if (mBuildMethod == BIHBM_PRIORITYQUEUE)
		mBihRoot = pqBuild(events, nObjects, aabb, 0);
	else
	{
		OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
			"Invalid build method for the BiHierarchy.",
			"BiHierarchy::build");
		mBihRoot = 0;
	}

	t4 = timer->getMicroseconds(); // DEBUG

	String method = "Invalid";
	if (mBuildMethod == BIHBM_RECURSIVE)
		method = "Recursive";
	else if (mBuildMethod == BIHBM_PRIORITYQUEUE)
		method = "Priority Queue";

	mBuildLog->logMessage("######## SAH Statistics ########");
	mBuildLog->logMessage("Build Method: " + method);
	mBuildLog->logMessage("Time for events build: " + StringConverter::toString(t2 - t1) + "µs");
	mBuildLog->logMessage("Time for events sort: " + StringConverter::toString(t3 - t2) + "µs");
	mBuildLog->logMessage("Time for tree build: " + StringConverter::toString(t4 - t3) + "µs");
	mBuildLog->logMessage("Total time: " + StringConverter::toString(t4 - t1) + "µs");
	mBuildLog->logMessage("Tree Depth: " + StringConverter::toString(mMaxDepth));
	mBuildLog->logMessage("Number of Objects: " + StringConverter::toString(mTreeStats.mNumSceneNodes));
	mBuildLog->logMessage("Number of Leaves: " + StringConverter::toString(mTreeStats.mNumLeaves));
	mBuildLog->logMessage("Number of Nodes: " + StringConverter::toString(mTreeStats.mNumNodes));
	mBuildLog->logMessage("Total cost: " + StringConverter::toString(calcCost()));
	mBuildLog->logMessage("################################");
}

BiHierarchy::Node * BiHierarchy::buildFromList(BihRenderableList& nodelist, BiHierarchy::Branch * parent, AxisAlignedBox& aabb)
{
	// data we want to collect
	BihPlaneEventList events;
	AxisAlignedBox nodeaabb;
	int nObjects = 0;

	BihRenderableList::iterator it = nodelist.begin();
	BihRenderableList::iterator end = nodelist.end();
	while (it != end)
	{
		(*it)->computeScene(events, nodeaabb, nObjects, false);
		it++;
	}

	events.sort();
	
	// HACK
	if (aabb.isNull())
	{
		aabb = nodeaabb;
	}

	if (mBuildMethod == BIHBM_RECURSIVE)
		return recBuild(events, nObjects, aabb, parent);
	else if (mBuildMethod == BIHBM_PRIORITYQUEUE)
		return pqBuild(events, nObjects, aabb, parent);
	else
	{
		OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
			"Invalid build method for the BiHierarchy.",
			"BiHierarchy::buildFromList");
		return 0;
	}
}

BiHierarchy::Node * BiHierarchy::recBuild(BihPlaneEventList& events, int nObjects, AxisAlignedBox& aabb, BiHierarchy::Branch * parent)
{
	// determine the depth we are on
	int level = 0;
	if (parent)
	{
		level = parent->getLevel() + 1;
	}

	/************************************************/
	/**************** BEGIN FINDPLANE ***************/
	/************************************************/
	// find optimal split plane and split node accordingly

	// initialize
	const int dim = 3;
	int pStart, pOn, pEnd;
	int nLeft[dim], nPlane[dim], nRight[dim];
	int nObjectCount[dim];
	float LeftOffset[dim],RightOffset[dim];
	for (int i = 0; i < dim; i++)
	{
		nLeft[i] = 0; nPlane[i] = 0; nRight[i] = nObjects;
		nObjectCount[i]=0;
	}

	BihSplitInfo split, best;
	best.cost = Math::POS_INFINITY;

	BihPlaneEventList::iterator begin = events.begin();
	BihPlaneEventList::iterator end = events.end();
	BihPlaneEventList::iterator it = begin;

	BihPlaneEventList::iterator it_a = begin; // iterator for adjusting the bounding box
    
	AxisAlignedBox minLefts[dim];
	AxisAlignedBox minRights[dim];

	bool LeftMod[dim];
	bool RightMod[dim];

	AxisAlignedBox minLeft;
	AxisAlignedBox minRight;
	

	// try all planes to find the best one for the given set of objects
	
	// Use SAH method
	if (mAlgorithm==BIHAL_SAH)
	{
		while (it != end)
		{
			BihPlaneEventList::iterator p = it;
			pStart = pOn = pEnd = 0;
			
			while (it != end && p->equalsType(*it, BihPlaneEvent::PET_END))
			{
				it++; pEnd++;
			}
			while (it != end && p->equalsType(*it, BihPlaneEvent::PET_ON))
			{
				it++; pOn++;
			}
			while (it != end && p->equalsType(*it, BihPlaneEvent::PET_START))
			{
				it++; pStart++;
			}
			
			
			int d = p->getDimension();
			nPlane[d] = pOn;
			//nRight[d] -= pOn; nRight[d] -= pEnd;

            // find the corrected offsets for the left and right bounding box
			it_a = begin;
			LeftOffset[d]=0;
			RightOffset[d]=0;
			minLefts[d].setNull();
	        minRights[d].setNull();
			nRight[d]=0;
			nLeft[d]=0;
			

			while (it_a != end)
			{
				if (it_a->getDimension()==d)
				{
					it_a->getRenderable()->setSide(BihPlaneEvent::PES_BOTH);
					it_a->getRenderable()->setClassified(false);
					it_a->classify(*p, split.side,true);
					if (it_a->getRenderable()->getSide() == BihPlaneEvent::PES_RIGHT)
					{
						if (!it_a->getRenderable()->isClassified())
						{
							it_a->getRenderable()->setClassified(true);
							nRight[d]++;
						}
						minRights[d].merge(it_a->getRenderable()->getBoundingBox());
						
					}
					// left-only nodes go in left list
					else if (it_a->getRenderable()->getSide() == BihPlaneEvent::PES_LEFT)
					{
						if (!it_a->getRenderable()->isClassified())
						{
							it_a->getRenderable()->setClassified(true);
							nLeft[d]++;
						}
						minLefts[d].merge(it_a->getRenderable()->getBoundingBox());
						
					}
				}


				it_a++;
			}

			
				p->SAH(aabb, nLeft[d], nPlane[d], nRight[d], split,minLefts[d],minRights[d]);
				mBuildLog->logMessage("SAH " + StringConverter::toString(best.cost)+ " " + StringConverter::toString(split.cost) + 
					"dim: " + StringConverter::toString(d));
				if (split.cost < best.cost)
				{
					best = split;
				}
			

			//nLeft[d] += pStart; nLeft[d] += pOn; 
			
		    nPlane[d] = 0;
		}
	} else
	{   
		
		int d;
		while (it != end)
		{
			BihPlaneEventList::iterator p = it;
			d = p->getDimension();
			//if ((p->equalsType(*it, BihPlaneEvent::PET_START)) || (p->equalsType(*it, BihPlaneEvent::PET_ON)))
			  nObjectCount[d]++;
			it++;
		}

		int MaxDimension=1;
		for (int i = 1; i < dim; i++)
	    {
		    if (nObjectCount[i]>nObjectCount[i-1]) MaxDimension=i;
	    }
		it = begin;
		
		nObjectCount[MaxDimension]/=2;
		while ((it != end) && (nObjectCount[MaxDimension]))
		{
			BihPlaneEventList::iterator p = it;
			if  (p->getDimension()== MaxDimension) 
			{
			  nObjectCount[MaxDimension]--;
			  best.event=*it;
			  best.cost=0;
			  
			}
			it++;
		}

		

		
	




	}


	/************************************************/
	/**************** BEGIN TERMINATE ***************/
	/************************************************/
	// check terminating condition
	if (/*best.cost > BihPlaneEvent::KI*nObjects ||*/ level >= mMaxDepth || nObjects<20 )
	{
		// Terminating condition reached, create leaf and add renderables to list
		mBuildLog->logMessage("Terminate " + StringConverter::toString(best.cost)+ " o:" + StringConverter::toString(nObjects) + 
				" ki: " + StringConverter::toString(BihPlaneEvent::KI*nObjects)+ " dim: " + StringConverter::toString(best.event.getDimension()));
		BiHierarchy::Leaf * leaf = new BiHierarchy::Leaf(this, level, aabb, parent);
		if (parent->GetSide()==1) leaf->SetToLeft(); else leaf->SetToRight();

		
		BihRenderable *rend;
		it = begin;
		while (it != end)
		{
			rend = it->getRenderable();
			rend->setClassified(false);
			if (rend->attachTo(leaf, this))
			{
				leaf->insert(rend);
			}
			it++;
		}
		// update stats
		++ mTreeStats.mNumNodes;
		++ mTreeStats.mNumLeaves;
		// update bounding box
		leaf->_updateBounds(false);
		return leaf;
	}

	/************************************************/
	/**************** BEGIN CLASSIFY ****************/
	/************************************************/
	// split the event list in left and right sub-lists
	else
	{
		BihPlaneEventList eventsRight, eventsLeft;
		it = begin;
		// slightly redundant, since we mark each renderable up to 6 times
		while (it != end)
		{
			it->getRenderable()->setSide(BihPlaneEvent::PES_BOTH);
			it->getRenderable()->setClassified(false);
			it++;
		}
		// now classify all renderables. do they belong to the left child, the right child or both?
		it = begin;
		while (it != end)
		{
			it->classify(best.event, best.side,true);
			it++;
		}
		// divide the event lists
		int nLeftS = 0, nRightS = 0, nBothS = 0;
		it = begin;

		minLeft.setNull();
	    minRight.setNull();

		
	
		while (it != end)
		{
			// right-only nodes go in right list
			if (it->getRenderable()->getSide() == BihPlaneEvent::PES_RIGHT)
			{
				if (!it->getRenderable()->isClassified())
				{
					it->getRenderable()->setClassified(true);
					nRightS++;
				}
				minRight.merge(it->getRenderable()->getBoundingBox());
				//mBuildLog->logMessage("merge right " + StringConverter::toString(minRight.volume()));
				eventsRight.push_back(*it);
			}
			// left-only nodes go in left list
			else if (it->getRenderable()->getSide() == BihPlaneEvent::PES_LEFT)
			{
				if (!it->getRenderable()->isClassified())
				{
					it->getRenderable()->setClassified(true);
					nLeftS++;
				}
				minLeft.merge(it->getRenderable()->getBoundingBox());
				//mBuildLog->logMessage("merge left " + StringConverter::toString(minLeft.volume()));
				eventsLeft.push_back(*it);
			}
			// remaining nodes go in both lists, bust must be clipped to prevent
			// events from lying outside the new nodes aabb
			else
			{
				if (!it->getRenderable()->isClassified())
				{
					it->getRenderable()->setClassified(true);
					nBothS++;
				}
				
				// calculate the intersecting box for the right side
                
                 
				  minRight.merge(best.bright.intersection(it->getRenderable()->getBoundingBox()));
				

				//mBuildLog->logMessage("merge Right Overlap " + StringConverter::toString(minRight.volume()));
				eventsRight.push_back(it->clip(best.bright, best.event.getDimension()));
				
				  minLeft.merge(best.bleft.intersection(it->getRenderable()->getBoundingBox()));
				
				//mBuildLog->logMessage("merge Left Overlap " + StringConverter::toString(minLeft.volume()));
				eventsLeft.push_back(it->clip(best.bleft, best.event.getDimension()));
			}
			it++;
		}

		mBuildLog->logMessage("Split R:" + StringConverter::toString(nRightS)+ " L:" + StringConverter::toString(nLeftS) + 
				" O: " + StringConverter::toString(nObjects));


		// calculate minimum of the left bounding box
		/*
		AxisAlignedBox minimum;
		minimum.setNull();
		it=eventsLeft.begin();
		while (it!=eventsLeft.end())
		{
			minimum.merge(it->getRenderable()->getBoundingBox());
			it++;
		}
		*/



		// create a new branch node and continue recursion
		BiHierarchy::Branch * branch = new BiHierarchy::Branch(this, level, aabb, parent, 
			best.event.getSplitPlane(), best.side);

		// now create the child nodes and continue recursion
		/*
		if (best.bleft.volume()>minLeft.volume())
		{
			mBuildLog->logMessage("Volume Smaller Left " + StringConverter::toString(minLeft.volume()) + " " + StringConverter::toString(best.bleft.volume(),3,3));
		}

		if (best.bright.volume()>minRight.volume())
		{
			mBuildLog->logMessage("Volume Smaller Right " + StringConverter::toString(minRight.volume()) + " " + StringConverter::toString(best.bright.volume(),3,3));
		}
		*/

		if (eventsLeft.size() > 0)
		{
			
			branch->mLeft = recBuild(eventsLeft, nLeftS + nBothS, /*best.bleft*/minLeft, branch);
			branch->mLeft->SetToLeft();
			mBuildLog->logMessage("SetToLeft");
			//branch->mLeft = recBuild(eventsLeft, nLeftS + nBothS, best.bleft, branch);
		}
		if (eventsRight.size() > 0)
		{
			
			branch->mRight = recBuild(eventsRight, nBothS + nRightS, /*best.bright*/ minRight, branch);
			branch->mRight->SetToRight();
			mBuildLog->logMessage("SetToRight");


			//branch->mRight = recBuild(eventsRight, nBothS + nRightS, best.bright, branch);
		}

		// update stats
		++ mTreeStats.mNumNodes;

		// update bounding box
		branch->_updateBounds(false);

		return branch;
	}
}

BiHierarchy::Node * BiHierarchy::pqBuild(BihPlaneEventList& events, 
										 int nObjects, AxisAlignedBox& aabb, 
										 BiHierarchy::Branch * parent)
{
	SplitCandidatePQ pqueue;
	Real globalTreeCost;
	Real globalSA;

	BiHierarchy::Node * newNode = 0, * topNode = 0;
	// init global tree cost
	globalTreeCost = BihPlaneEvent::KI * nObjects;
	globalSA = BihPlaneEvent::surfaceArea(aabb);

	BihPlaneEventList * e = new BihPlaneEventList(events);

	// inital split candidate
	BihSplitInfo * best = pqFindPlane(e, nObjects, aabb, globalSA);
	BihSubdivisionCandidate splitcandidate(e, nObjects, aabb, parent, globalTreeCost, 
		globalTreeCost - best->cost, best, BihPlaneEvent::PES_BOTH);
	pqueue.push(splitcandidate);


	/*** BEGIN ITERATIVE BUILD ***/
	while (!pqueue.empty())
	{
		BihSubdivisionCandidate sc = pqueue.top();
		pqueue.pop();

		// determine the depth we are on
		int level = 0;
		if (sc.parent)
		{
			level = sc.parent->getLevel() + 1;
		}

		/************************************************/
		/**************** BEGIN TERMINATE ***************/
		/************************************************/
		// check terminating condition
		if (sc.decrease < 0.0 || level >= mMaxDepth)
		{
			// Terminating condition reached, create leaf and add renderables to list
			BiHierarchy::Leaf * leaf = new BiHierarchy::Leaf(this, level, sc.aabb, sc.parent);
			BihRenderable *rend;
			BihPlaneEventList::iterator begin = sc.events->begin();
			BihPlaneEventList::iterator end = sc.events->end();
			BihPlaneEventList::iterator it = begin;
			while (it != end)
			{
				rend = it->getRenderable();
				rend->setClassified(false);
				if (rend->attachTo(leaf, this))
				{
					leaf->insert(rend);
				}
				it++;
			}
			newNode = leaf;
			if (!topNode)
				topNode = leaf;
			// update stats
			++ mTreeStats.mNumNodes;
			++ mTreeStats.mNumLeaves;
		}

		/************************************************/
		/**************** BEGIN CLASSIFY ****************/
		/************************************************/
		else
		{
			BihPlaneEventList * eventsLeft = new BihPlaneEventList();
			BihPlaneEventList * eventsRight = new BihPlaneEventList();
			BihPlaneEventList::iterator begin = sc.events->begin();
			BihPlaneEventList::iterator end = sc.events->end();
			BihPlaneEventList::iterator it = begin;
			// slightly redundant, since we mark each renderable up to 6 times
			while (it != end)
			{
				it->getRenderable()->setSide(BihPlaneEvent::PES_BOTH);
				it->getRenderable()->setClassified(false);
				it++;
			}

			// now classify all renderables. do they belong to the left child, the right child or both?
			it = begin;
			while (it != end)
			{
				it->classify(sc.best->event, sc.best->side,false);
				it++;
			}

			// divide the event lists
			int nLeftS = 0, nRightS = 0, nBothS = 0;
			it = begin;
			while (it != end)
			{
				// right-only events go in the right list
				if (it->getRenderable()->getSide() == BihPlaneEvent::PES_RIGHT)
				{
					if (!it->getRenderable()->isClassified())
					{
						it->getRenderable()->setClassified(true);
						nRightS++;
					}

					eventsRight->push_back(*it);
				}
				// left-only events go in the left list
				else if (it->getRenderable()->getSide() == BihPlaneEvent::PES_LEFT)
				{
					if (!it->getRenderable()->isClassified())
					{
						it->getRenderable()->setClassified(true);
						nLeftS++;
					}
					eventsLeft->push_back(*it);
				}
				// the rest goes in both lists after being clipped
				else
				{
					if (!it->getRenderable()->isClassified())
					{
						it->getRenderable()->setClassified(true);
						nBothS++;
					}

					eventsRight->push_back(it->clip(sc.best->bright, sc.best->event.getDimension()));
					eventsLeft->push_back(it->clip(sc.best->bleft, sc.best->event.getDimension()));
				}
				it++;
			}

			// create a new branch node
			BiHierarchy::Branch * branch = new BiHierarchy::Branch(this, level, sc.aabb, sc.parent, 
				sc.best->event.getSplitPlane(), sc.best->side);

			globalTreeCost -= sc.decrease;


			// now for each potential child node, compute the split plane and the cost decrease
			// and place them in the queue
			if (eventsLeft->size() > 0)
			{
				best = pqFindPlane(eventsLeft, nLeftS + nBothS, sc.best->bleft, globalSA);
				Real old = BihPlaneEvent::surfaceArea(sc.best->bleft)/globalSA*BihPlaneEvent::KI*(nLeftS + nBothS);
				BihSubdivisionCandidate scleft(eventsLeft, nLeftS + nBothS, sc.best->bleft, 
					branch, old, old - best->cost, best, BihPlaneEvent::PES_LEFT);
				pqueue.push(scleft);
			}
			// cleanup
			else
			{
				delete eventsLeft;
			}

			if (eventsRight->size() > 0)
			{
				best = pqFindPlane(eventsRight, nBothS + nRightS, sc.best->bright, globalSA);
				Real old = BihPlaneEvent::surfaceArea(sc.best->bright)/globalSA*BihPlaneEvent::KI*(nBothS + nRightS);
				BihSubdivisionCandidate scright(eventsRight, nBothS + nRightS, sc.best->bright, 
					branch, old, old - best->cost, best, BihPlaneEvent::PES_RIGHT);
				pqueue.push(scright);
			}
			// cleanup
			else
			{
				delete eventsRight;
			}

			newNode = branch;
			if (!topNode)
				topNode = branch;


			// update stats
			++ mTreeStats.mNumNodes;
		}

		// attach new node to parent
		if (sc.parent)
		{
			if (sc.side == BihPlaneEvent::PES_LEFT)
			{
				sc.parent->mLeft = newNode;
			}
			else if (sc.side == BihPlaneEvent::PES_RIGHT)
			{
				sc.parent->mRight = newNode;
			}
			else
			{
				OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
					"PQBUILD SNAFU - CHILD I AM NOT LEFT NOR RIGHT","BiHierarchy::pqBuild");
			}
		}

		// update bounding boxes when geometry (leaf) was added
		if (newNode->isLeaf())
			newNode->_updateBounds();

		//cleanup
		OGRE_DELETE(sc.events);
		OGRE_DELETE(sc.best);
	}
	mBuildLog->logMessage("Cost after PQBUILD €" + StringConverter::toString(globalTreeCost));
	return topNode;
}

//-------------------------------------------------------------------------
BihSplitInfo * BiHierarchy::pqFindPlane(BihPlaneEventList * events, int nObjects, AxisAlignedBox& aabb, Real globalSA)
{
	static const int dim = 3;
	int pStart, pOn, pEnd;
	int nLeft[dim], nPlane[dim], nRight[dim];
	for (int i = 0; i < dim; i++)
	{
		nLeft[i] = 0; nPlane[i] = 0; nRight[i] = nObjects;
	}

	BihSplitInfo split, best;
	best.cost = Math::POS_INFINITY;

	Real parentSA = BihPlaneEvent::surfaceArea(aabb);

	BihPlaneEventList::iterator begin = events->begin();
	BihPlaneEventList::iterator end = events->end();
	BihPlaneEventList::iterator it = begin;
	// try all planes to find the best one for the given set of objects
	while (it != end)
	{
		BihPlaneEventList::iterator p = it;
		pStart = pOn = pEnd = 0;
		while (it != end && p->equalsType(*it, BihPlaneEvent::PET_END))
		{
			it++; pEnd++;
		}
		while (it != end && p->equalsType(*it, BihPlaneEvent::PET_ON))
		{
			it++; pOn++;
		}
		while (it != end && p->equalsType(*it, BihPlaneEvent::PET_START))
		{
			it++; pStart++;
		}
		int d = p->getDimension();
		nPlane[d] = pOn; nRight[d] -= pOn; nRight[d] -= pEnd;
		p->pqSAH(globalSA, parentSA, nLeft[d], nPlane[d], nRight[d], aabb, split);
		if (split.cost < best.cost)
		{
			best = split;
		}
		nLeft[d] += pStart; nLeft[d] += pOn; nPlane[d] = 0;
	}
	return new BihSplitInfo(best);
}



/************************************************************************/
/*               BiHierarchy rendering functions                        */
/************************************************************************/


//-------------------------------------------------------------------------
void BiHierarchy::queueVisibleObjects(BiHierarchyCamera* cam, RenderQueue* queue, bool onlyShadowCasters, 
	bool showBoxes, BiHierarchy::NodeList& visibleNodes)
{
	mFrameStats.clear();
	cam->mNumVisQueries = 0;

	if (mBihRoot)
		recQueueVisibleObjects(mBihRoot, Root::getSingleton().getCurrentFrameNumber(),
			cam, queue, onlyShadowCasters, showBoxes, visibleNodes);

	mFrameStats.mTraversedNodes = cam->mNumVisQueries;
}

//-------------------------------------------------------------------------
void BiHierarchy::recQueueVisibleObjects(BiHierarchy::Node * node, unsigned long currentFrame, 
	BiHierarchyCamera* cam, RenderQueue* queue, bool onlyShadowCasters, bool showBoxes, 
	BiHierarchy::NodeList& visibleNodes, bool fullVis)
{
	BiHierarchyCamera::NodeVisibility vis = BiHierarchyCamera::BIHNV_PART;
	// test visibility
	//if (cam->isVisible(node->mAABB))
	if (fullVis || 
		((vis = (cam->*getVisibility)(node->mAABB)) != BiHierarchyCamera::BIHNV_NONE))
	{
		visibleNodes.push_back(node);

		bool v = (fullVis || vis == BiHierarchyCamera::BIHNV_FULL);

		node->queueVisibleObjects(currentFrame, cam, queue, onlyShadowCasters, showBoxes, v);

		if (v || node->isLeaf())
			++ mFrameStats.mRenderedNodes;

		if (!v)
		{

			if (node->getLeftChild())
				recQueueVisibleObjects(node->getLeftChild(), currentFrame, 
				cam, queue, onlyShadowCasters, showBoxes, visibleNodes, v);
			if (node->getRightChild())
				recQueueVisibleObjects(node->getRightChild(), currentFrame, 
				cam, queue, onlyShadowCasters, showBoxes, visibleNodes, v);
		}
	}
	else
	{
		++ mFrameStats.mFrustumCulledNodes;
	}
}

//-------------------------------------------------------------------------
void BiHierarchy::setEnhancedVis(bool enh)
{
	if (enh)
		getVisibility = &BiHierarchyCamera::getVisibilityEnhanced;
	else
		getVisibility = &BiHierarchyCamera::getVisibilitySimple;
}

//-------------------------------------------------------------------------
bool BiHierarchy::getEnhancedVis()
{
	return getVisibility == &BiHierarchyCamera::getVisibilityEnhanced;
}

//-------------------------------------------------------------------------
//void BiHierarchy::findVisibleNodes(NodeList& visibleNodes, Camera * cam)
//{
//	if (mBihRoot)
//		recFindVisibleNodes(mBihRoot, visibleNodes, cam);
//}

////-------------------------------------------------------------------------
//void BiHierarchy::recFindVisibleNodes(BiHierarchy::Node * node, NodeList& visibleNodes, Camera * cam)
//{
//	// test visibility
//	if (cam->isVisible(node->mAABB))
//	{
//		visibleNodes.push_back(node);

//		if (node->getLeftChild())
//			recFindVisibleNodes(node->getLeftChild(), visibleNodes, cam);
//		if (node->getRightChild())
//			recFindVisibleNodes(node->getRightChild(), visibleNodes, cam);
//	}
//}

void BiHierarchy::findNodesIn(const AxisAlignedBox &box, std::list<SceneNode *> &list, SceneNode *exclude)
{
	if (mBihRoot)
		recFindNodesIn(box, list, exclude, mBihRoot);
}

void BiHierarchy::findNodesIn(const Sphere &sphere, std::list<SceneNode *> &list, SceneNode *exclude)
{
	if (mBihRoot)
		recFindNodesIn(sphere, list, exclude, mBihRoot);
}

void BiHierarchy::findNodesIn(const PlaneBoundedVolume &volume, std::list<SceneNode *> &list, SceneNode *exclude)
{
	if (mBihRoot)
		recFindNodesIn(volume, list, exclude, mBihRoot);
}

void BiHierarchy::findNodesIn(const Ray &ray, std::list<SceneNode *> &list, SceneNode *exclude)
{
	if (mBihRoot)
		recFindNodesIn(ray, list, exclude, mBihRoot);
}

void BiHierarchy::recFindNodesIn(const AxisAlignedBox &box, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
	// check intersection
	if ( !full )
	{
		BihIntersection isect = intersect(box, node->_getWorldAABB());

		if ( isect == OUTSIDE )
			return ;

		full = ( isect == INSIDE );
	}

	if (node->isLeaf())
	{
		LeafPtr leaf = BIHLEAFPTR_CAST(node);
		for (BihRenderableList::iterator it = leaf->mBihRenderables.begin();
			it != leaf->mBihRenderables.end(); it ++)
		{
			SceneNode *sn = static_cast<BiHierarchySceneNode *>(*it);

			if (sn != exclude)
			{
				if (full)
				{
					list.push_back(sn);
				}
				else
				{
					BihIntersection nsect = intersect(box, sn->_getWorldAABB());

					if ( nsect != OUTSIDE )
					{
						list.push_back(sn);
					}
				}
			}
		}
	}
	else
	{
		if (node->getLeftChild())
			recFindNodesIn(box, list, exclude, node->getLeftChild(), full);
		if (node->getRightChild())
			recFindNodesIn(box, list, exclude, node->getRightChild(), full);
	}
}


void BiHierarchy::recFindNodesIn(const Sphere &sphere, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
	// TODO
}


void BiHierarchy::recFindNodesIn(const PlaneBoundedVolume &volume, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
	// TODO
}


void BiHierarchy::recFindNodesIn(const Ray &ray, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
	// TODO
}


/************************************************************************/
/*              BiHierarchy debug & helper functions                    */
/************************************************************************/

//-------------------------------------------------------------------------
void BiHierarchy::dump()
{
	//LogManager::getSingleton().logMessage("#@#@#@#@ Dumping BiHierarchy #@#@#@#@");
	mBuildLog->logMessage("#@#@#@#@ Dumping BiHierarchy #@#@#@#@");
	if (mBihRoot)
		dump(mBihRoot);
}

//-------------------------------------------------------------------------
void BiHierarchy::dump(BiHierarchy::Node * node)
{
	//LogManager * log = LogManager::getSingletonPtr();
	BiHierarchySceneNode * scenenode;
	String pad;
	int p;
	
	pad = "";
	for (p = 0; p < node->getLevel(); p++)
	{
		pad += " ";
	}

	if (node->isLeaf())
	{
		BiHierarchy::Leaf * leaf = BIHLEAFPTR_CAST(node);
		BihRenderableList::iterator it  = leaf->mBihRenderables.begin();
		BihRenderableList::iterator end = leaf->mBihRenderables.end();
		while (it != end)
		{
			scenenode = static_cast<BiHierarchySceneNode *>(*it);
			mBuildLog->logMessage(pad + "# Leaf   level " + 
				StringConverter::toString(node->getLevel()) + 
				" SceneNode " + scenenode->getName());
			mBuildLog->logMessage(pad + "## Objects: " + scenenode->dumpToString());
			it++;
		}
	}
	else
	{
		BiHierarchy::Branch * branch = BIHBRANCHPTR_CAST(node);
		if (branch->mLeft)
		{
			mBuildLog->logMessage(pad + "# Branch level " + 
				StringConverter::toString(node->getLevel()) + " Left Child");
			dump(branch->mLeft);
		}
		if (branch->mRight)
		{
			mBuildLog->logMessage(pad + "# Branch level " + 
				StringConverter::toString(node->getLevel()) + " Right Child");
			dump(branch->mRight);
		}
	}
}

//-------------------------------------------------------------------------
Real BiHierarchy::calcCost()
{
	if (mBihRoot)
		return calcCost(mBihRoot, BihPlaneEvent::surfaceArea(mBihRoot->mAABB));
	else
		return 0;
}

//-------------------------------------------------------------------------
Real BiHierarchy::calcCost(BiHierarchy::Node * node, Real vs)
{
	if (node == 0)
		return 0.0;

	if (node->isLeaf())
	{
		BiHierarchy::Leaf * leaf = BIHLEAFPTR_CAST(node);
		return (BihPlaneEvent::surfaceArea(node->mAABB)/vs) *
			BihPlaneEvent::KI * leaf->mBihRenderables.size();
	}
	else
	{
		BiHierarchy::Branch * branch = BIHBRANCHPTR_CAST(node);
		return (BihPlaneEvent::surfaceArea(node->mAABB)/vs) * BihPlaneEvent::KT + 
			calcCost(branch->mLeft, vs) + calcCost(branch->mRight, vs);
	}
}

/************************************************************************/
/* BiHierarchy::Node/Branch/Leaf functions                                   */
/************************************************************************/

//-------------------------------------------------------------------------
BiHierarchy::Leaf::~Leaf()
{
	// detach all scene nodes in the case that we are rebuilding
	// the tree but not the scene
	BihRenderableList::iterator it = mBihRenderables.begin();
	BihRenderableList::iterator end = mBihRenderables.end();
	while (it != end)
	{
		(*it)->detachFrom(this);
		it++;
	}
	mBihRenderables.clear();
}

//-------------------------------------------------------------------------
void BiHierarchy::Leaf::queueVisibleObjects(unsigned long currentFrame, 
	Camera* cam, RenderQueue* queue, bool onlyShadowCasters, bool showBoxes, bool fullVis)
{
	BihRenderableList::iterator it  = mBihRenderables.begin();
	BihRenderableList::iterator end = mBihRenderables.end();
	while (it != end)
	{			
		if (!(*it)->isQueued(currentFrame, cam))
		{
			(*it)->queueObjects(cam, queue, onlyShadowCasters);
		}
		it++;
	}

	if (showBoxes)
	{
        if (mLevel == mOwner->getHiLiteLevel()|| (mLevel-1) == mOwner->getHiLiteLevel() || mOwner->getShowAllBoxes())
		queue->addRenderable(getWireBoundingBox());
		/*
		if (mLevel == mOwner->getHiLiteLevel() || mOwner->getShowAllBoxes())
			queue->addRenderable(getWireBoundingBox());
		*/
	}

}

//-------------------------------------------------------------------------
void BiHierarchy::Leaf::getGeometryList(GeometryVector *geometryList)
{
	BihRenderableList::iterator it = mBihRenderables.begin();
	BihRenderableList::iterator end = mBihRenderables.end();
	while (it != end)
	{
		(*it)->getGeometryList(geometryList);
		it++;
	}
}

//-------------------------------------------------------------------------
// update the world aabb based on the contained geometry
void BiHierarchy::Leaf::_updateBounds(bool recurse)
{
	// reset box
	mWorldAABB.setNull();

	// merge boxes from attached geometry
	BihRenderableList::iterator it = mBihRenderables.begin();
	BihRenderableList::iterator end = mBihRenderables.end();
	while (it != end)
	{
		//(*it)->_updateBounds();
		mWorldAABB.merge((*it)->getBoundingBox());
		it++;
	}

	// update parent recursively
	if (recurse && mParent)
		mParent->_updateBounds(recurse);
}

} // namespace Ogre