// ================================================================
// $Id: lsds_kdtree.cpp,v 1.18 2005/04/16 09:34:21 bittner Exp $
// ****************************************************************
/**
   The KD tree based LSDS
*/
// Initial coding by
/**
   @author Jiri Bittner
*/

// Standard headers
#include <stack>
#include <queue>
#include <algorithm>
#include <fstream>
#include <string>

#include "VspKdTree.h"

#include "Environment.h"
#include "VssRay.h"
#include "Intersectable.h"
#include "Ray.h"
#include "RayInfo.h"
#include "ViewCellsManager.h"
#include "ViewCellBsp.h"

namespace GtpVisibilityPreprocessor {

// Static variables
int VspKdLeaf::sMailId = 0;
int VspKdMergeCandidate::sMaxPvsSize = 150;
float VspKdMergeCandidate::sOverallCost = Limits::Small;
#define USE_FIXEDPOINT_T 0

/// Adds object to the pvs of the front and back node
inline void AddObject2Pvs(Intersectable *object,
						  const int side,
						  int &pvsBack,
						  int &pvsFront)
{
	if (!object)
		return;

	if (side <= 0)
	{
		if (!object->Mailed() && !object->Mailed(2))
		{
			++ pvsBack;

			if (object->Mailed(1))
				object->Mail(2);
			else
				object->Mail();
		}
	}

	if (side >= 0)
	{
		if (!object->Mailed(1) && !object->Mailed(2))
		{
			++ pvsFront;

			if (object->Mailed())
				object->Mail(2);
			else
				object->Mail(1);
		}
	}
}

/**************************************************************/
/*                class VspKdNode implementation              */
/**************************************************************/

// Inline constructor
VspKdNode::VspKdNode(VspKdNode *p):
mParent(p), mAxis(-1), mDepth(p ? p->mDepth + 1 : 0)
{}

VspKdNode::~VspKdNode()
{
};

inline VspKdNode *VspKdNode::GetParent() const
{
	return mParent;
}

inline void VspKdNode::SetParent(VspKdNode *p)
{
	mParent = p;
}

bool VspKdNode::IsLeaf() const
{
	return mAxis == -1;
}

int VspKdNode::GetAccessTime()
{
	return 0x7FFFFFF;
}

/**************************************************************/
/*                VspKdInterior implementation            */
/**************************************************************/

VspKdInterior::VspKdInterior(VspKdInterior *p):
VspKdNode(p), mBack(NULL), mFront(NULL), mAccesses(0), mLastAccessTime(-1)
{
}

int VspKdInterior::GetAccessTime()
{
	return mLastAccessTime;
}

void VspKdInterior::SetupChildLinks(VspKdNode *b, VspKdNode *f)
{
	mBack = b;
	mFront = f;
	b->SetParent(this);
	f->SetParent(this);
}

void VspKdInterior::ReplaceChildLink(VspKdNode *oldChild,
									 VspKdNode *newChild)
{
	if (mBack == oldChild)
		mBack = newChild;
	else
		mFront = newChild;
}

int VspKdInterior::Type() const
{
	return EInterior;
}

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


void VspKdInterior::Print(ostream &s) const
{
	switch (mAxis)
	{
	case 0:
		s << "x "; break;
	case 1:
		s << "y "; break;
	case 2:
		s << "z "; break;
	}

	s << mPosition << " ";

	mBack->Print(s);
	mFront->Print(s);
}


int VspKdInterior::ComputeRayIntersection(const RayInfo &rayData, float &t)
{
	return rayData.ComputeRayIntersection(mAxis, mPosition, t);
}


VspKdNode *VspKdInterior::GetBack() const
{
	return mBack;
}


VspKdNode *VspKdInterior::GetFront() const
{
	return mFront;
}


/*******************************************************************/
/*                   class VspKdLeaf implementation                */
/*******************************************************************/


VspKdLeaf::VspKdLeaf(VspKdNode *p, 
					 const int nRays, 
					 const int maxCostMisses):
VspKdNode(p), mRays(), mPvsSize(0), mValidPvs(false), mViewCell(NULL),
mMaxCostMisses(maxCostMisses), mPvs(NULL), mVolume(0)
{
	mRays.reserve(nRays);
}

VspKdLeaf::~VspKdLeaf()
{
	DEL_PTR(mPvs);
}


int VspKdLeaf::GetMaxCostMisses()
{
	return mMaxCostMisses;
}


int VspKdLeaf::Type() const
{
	return ELeaf;
}

void VspKdLeaf::Print(ostream &s) const
{
	s << endl << "L: r = " << (int)mRays.size() << endl;
};

void VspKdLeaf::AddRay(const RayInfo &data)
{
	mValidPvs = false;
	mRays.push_back(data);
	data.mRay->Ref();
}

int VspKdLeaf::GetPvsSize() const
{
	return mPvsSize;
}

void VspKdLeaf::SetPvsSize(const int s)
{
	mPvsSize = s;
}

void VspKdLeaf::Mail()
{
	mMailbox = sMailId;
}

void VspKdLeaf::NewMail()
{
	++ sMailId;
}

bool VspKdLeaf::Mailed() const
{
	return mMailbox == sMailId;
}

bool VspKdLeaf::Mailed(const int mail) const
{
	return mMailbox == mail;
}

int VspKdLeaf::GetMailbox() const
{
	return mMailbox;
}

float VspKdLeaf::GetAvgRayContribution() const
{
	return GetPvsSize() / ((float)mRays.size() + Limits::Small);
}

float VspKdLeaf::GetSqrRayContribution() const
{
	return sqr(GetAvgRayContribution());
}

RayInfoContainer &VspKdLeaf::GetRays()
{
	return mRays;
}

void VspKdLeaf::ExtractPvs(ObjectContainer &objects) const
{
	RayInfoContainer::const_iterator it, it_end = mRays.end();

	for (it = mRays.begin(); it != it_end; ++ it)
	{
		if ((*it).mRay->mTerminationObject)
			objects.push_back((*it).mRay->mTerminationObject);
		if ((*it).mRay->mOriginObject)
			objects.push_back((*it).mRay->mOriginObject);
	}
}

void VspKdLeaf::GetRays(VssRayContainer &rays)
{
	RayInfoContainer::const_iterator it, it_end = mRays.end();

	for (it = mRays.begin(); it != mRays.end(); ++ it)
		rays.push_back((*it).mRay);
}


void VspKdLeaf::SetViewCell(VspKdViewCell *viewCell)
{
	mViewCell = viewCell;
}


void VspKdLeaf::UpdatePvsSize()
{
	if (!mValidPvs)
	{
		Intersectable::NewMail();
		int pvsSize = 0;
		for(RayInfoContainer::iterator ri = mRays.begin();
			ri != mRays.end(); ++ ri)
		{
			if ((*ri).mRay->IsActive())
			{
				Intersectable *object;
#if BIDIRECTIONAL_RAY
				object = (*ri).mRay->mOriginObject;

				if (object && !object->Mailed())
				{
					++ pvsSize;
					object->Mail();
				}
#endif
				object = (*ri).mRay->mTerminationObject;
				if (object && !object->Mailed())
				{
					++ pvsSize;
					object->Mail();
				}
			}
		}

		mPvsSize = pvsSize;
		mValidPvs = true;
	}
}


/***************************************************************/
/*          class VspKdIntermediate implementation             */
/***************************************************************/


VspKdIntermediate::VspKdIntermediate(VspKdInterior *p):
VspKdNode(p), mBack(NULL), mFront(NULL), mAccesses(0), mLastAccessTime(-1)
{
}


VspKdIntermediate::~VspKdIntermediate()
{
	DEL_PTR(mFront);
	DEL_PTR(mBack);
}
    
	
int VspKdIntermediate::Type() const
{
	return EIntermediate;
}


VspKdLeaf *VspKdIntermediate::GetBack() const
{
	return mBack;
}


VspKdLeaf *VspKdIntermediate::GetFront() const
{
	return mFront;
}


void VspKdIntermediate::Print(ostream &s) const
{
	s << endl << mSplitPlane << endl;
}


void VspKdIntermediate::SetupChildLinks(VspKdLeaf *b, VspKdLeaf *f)
{
	mBack = b;
	mFront = f;

	b->SetParent(this);
	f->SetParent(this);
}


int VspKdIntermediate::ComputeRayIntersection(const RayInfo &rayData, float &t)
{
	return rayData.ComputeRayIntersection(mSplitPlane, t);
}


/*************************************************************/
/*              class VspKdTree implementation               */
/*************************************************************/


VspKdTree::VspKdTree(): mOnlyDrivingAxis(false)
{
	Environment::GetSingleton()->GetIntValue("VspKdTree.Termination.maxDepth", mTermMaxDepth);
	Environment::GetSingleton()->GetIntValue("VspKdTree.Termination.minPvs", mTermMinPvs);
	Environment::GetSingleton()->GetIntValue("VspKdTree.Termination.minRays", mTermMinRays);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.Termination.maxRayContribution", mTermMaxRayContribution);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.Termination.maxCostRatio", mTermMaxCostRatio);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.Termination.minSize", mTermMinSize);

	Environment::GetSingleton()->GetIntValue("VspKdTree.Termination.missTolerance", mTermMissTolerance);

	mTermMinSize = sqr(mTermMinSize);

	Environment::GetSingleton()->GetFloatValue("VspKdTree.epsilon", mEpsilon);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.ct_div_ci", mCtDivCi);

	Environment::GetSingleton()->GetFloatValue("VspKdTree.maxTotalMemory", mMaxTotalMemory);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.maxStaticMemory", mMaxStaticMemory);

	Environment::GetSingleton()->GetIntValue("VspKdTree.accessTimeThreshold", mAccessTimeThreshold);
	Environment::GetSingleton()->GetIntValue("VspKdTree.minCollapseDepth", mMinCollapseDepth);

	//Environment::GetSingleton()->GetIntValue("ViewCells.maxViewCells", mMaxViewCells);

	Environment::GetSingleton()->GetIntValue("VspKdTree.PostProcess.minViewCells", mMergeMinViewCells);
	Environment::GetSingleton()->GetFloatValue("VspKdTree.PostProcess.maxCostRatio", mMergeMaxCostRatio);
	Environment::GetSingleton()->GetIntValue("VspKdTree.PostProcess.maxPvsSize",
							 VspKdMergeCandidate::sMaxPvsSize);

	Environment::GetSingleton()->GetBoolValue("VspKdTree.splitUseOnlyDrivingAxis", mOnlyDrivingAxis);
	Environment::GetSingleton()->GetIntValue("VspKdTree.Termination.maxViewCells", mMaxViewCells);

	//-- output
	Debug << "======= vsp kd tree options ========" << endl;
	Debug << "max depth: "<< mTermMaxDepth << endl;
	Debug << "min pvs: "<< mTermMinPvs << endl;
	Debug << "min rays: "<< mTermMinRays << endl;
	Debug << "max ray contribution: "<< mTermMaxRayContribution << endl;
	Debug << "max cost ratio: " << mTermMaxCostRatio << endl;
	Debug << "min size: " << mTermMinSize << endl;

	Debug << "split type: ";

	//-- split type
	char sname[128];
	Environment::GetSingleton()->GetStringValue("VspKdTree.splitType", sname);
	string name(sname);

	if (name.compare("regular") == 0)
	{
		Debug << "regular split" << endl;
		splitType = ESplitRegular;
	}
	else
	{
		if (name.compare("heuristic") == 0)
		{
			Debug << "heuristic split" << endl;
			splitType = ESplitHeuristic;
		}
		else
		{
			cerr << "Invalid VspKdTree split type " << name << endl;
			exit(1);
		}
	}

	mRoot = NULL;
	mSplitCandidates = new vector<SortableEntry>;
}


VspKdTree::~VspKdTree()
{
	DEL_PTR(mRoot);
	DEL_PTR(mSplitCandidates);
}

void VspKdStatistics::Print(ostream &app) const
{
	app << "===== VspKdTree statistics ===============\n";

	app << "#N_RAYS ( Number of rays )\n"
		<< rays << endl;

	app << "#N_INITPVS ( Initial PVS size )\n"
		<< initialPvsSize << endl;

	app << "#N_NODES ( Number of nodes )\n" << nodes << "\n";

	app << "#N_LEAVES ( Number of leaves )\n" << Leaves() << "\n";

	app << "#N_SPLITS ( Number of splits in axes x y z)\n";

	for (int i = 0; i < 3; ++ i)
		app << splits[i] << " ";
	app << endl;

	app << "#N_RAYREFS ( Number of rayRefs )\n" <<
		rayRefs << "\n";

	app << "#N_RAYRAYREFS  ( Number of rayRefs / ray )\n" <<
		rayRefs / (double)rays << "\n";

	app << "#N_LEAFRAYREFS  ( Number of rayRefs / leaf )\n" <<
		rayRefs / (double)Leaves() << "\n";

	app << "#N_MAXRAYREFS  ( Max number of rayRefs / leaf )\n" <<
		maxRayRefs << "\n";

  //  app << setprecision(4);

	app << "#N_PMAXDEPTHLEAVES ( Percentage of leaves at maxdepth )\n" <<
		maxDepthNodes * 100 / (double)Leaves() << endl;

	app << "#N_PMINPVSLEAVES  ( Percentage of leaves with mininimal PVS )\n" <<
		minPvsNodes * 100 / (double)Leaves() << endl;

	app << "#N_PMINRAYSLEAVES  ( Percentage of leaves with minimal number of rays)\n" <<
		minRaysNodes * 100 / (double)Leaves() << endl;

	app << "#N_PMINSIZELEAVES  ( Percentage of leaves with minSize )\n"<<
		minSizeNodes * 100 / (double)Leaves() << endl;

	app << "#N_PMAXRAYCONTRIBLEAVES  ( Percentage of leaves with maximal ray contribution )\n" <<
		maxRayContribNodes * 100 / (double)Leaves() << endl;

	app << "#N_PMAXRCOSTLEAVES  ( Percentage of leaves with maximal cost ratio )\n" <<
		maxCostNodes * 100 / (double)Leaves() << endl;

	app << "#N_ADDED_RAYREFS  ( Number of dynamically added ray references )\n"<<
		addedRayRefs << endl;

	app << "#N_REMOVED_RAYREFS  ( Number of dynamically removed ray references )\n"<<
		removedRayRefs << endl;

	//  app << setprecision(4);

	app << "#N_( Maximal PVS size / leaf)\n"
		<< maxPvsSize << endl;

	app << "#N_( Average PVS size / leaf)\n"
		<< (double) accPvsSize / (double)Leaves() << endl;

	app << "#N_CTIME  ( Construction time [s] )\n"
		<< Time() << " \n";

	app << "===== END OF VspKdTree statistics ==========\n";
}


void VspKdTree::CollectViewCells(ViewCellContainer &viewCells) const
{
	stack<VspKdNode *> nodeStack;
	nodeStack.push(mRoot);

	ViewCell::NewMail();

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

		if (node->IsLeaf())
		{
			ViewCell *viewCell = dynamic_cast<VspKdLeaf *>(node)->mViewCell;

			if (!viewCell->Mailed())
			{
				viewCell->Mail();
				viewCells.push_back(viewCell);
			}
		}
		else
		{
			VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

			nodeStack.push(interior->GetFront());
			nodeStack.push(interior->GetBack());
		}
	}
}

void VspKdTree::Construct(const VssRayContainer &rays,
						  AxisAlignedBox3 *forcedBoundingBox)
{
	mStat.Start();

	mMaxMemory = mMaxStaticMemory;
	DEL_PTR(mRoot);

	mRoot = new VspKdLeaf(NULL, (int)rays.size());

	// first construct a leaf that will get subdivided
	VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(mRoot);

	mStat.nodes = 1;
  	mBox.Initialize();

	//-- compute bounding box
	if (forcedBoundingBox)
		mBox = *forcedBoundingBox;
	else
		for (VssRayContainer::const_iterator ri = rays.begin(); ri != rays.end(); ++ ri)
		{
			if ((*ri)->mOriginObject)
            	mBox.Include((*ri)->GetOrigin());
			if ((*ri)->mTerminationObject)
				mBox.Include((*ri)->GetTermination());
		}

	Debug << "bbox = " << mBox << endl;

	for (VssRayContainer::const_iterator ri = rays.begin(); ri != rays.end(); ++ ri)
	{
		//leaf->AddRay(RayInfo(*ri));
		VssRay *ray = *ri;
		float minT, maxT;
		
		// TODO: not very efficient to implictly cast between rays types!
		if (mBox.GetRaySegment(*ray, minT, maxT))
		{
			float len = ray->Length();

			if (!len)
				len = Limits::Small;

			leaf->AddRay(RayInfo(ray, minT / len, maxT / len));
		}
	}

	mStat.rays = (int)leaf->mRays.size();
	leaf->UpdatePvsSize();

	mStat.initialPvsSize = leaf->GetPvsSize();

	// Subdivide();
	mRoot = Subdivide(TraversalData(leaf, mBox, 0));

	if (mSplitCandidates)
	{
		// force release of this vector
		delete mSplitCandidates;
		mSplitCandidates = new vector<SortableEntry>;
	}

	mStat.Stop();

	mStat.Print(cout);
	Debug << "#Total memory=" << GetMemUsage() << endl;
}



VspKdNode *VspKdTree::Subdivide(const TraversalData &tdata)
{
	VspKdNode *result = NULL;

	priority_queue<TraversalData> tStack;
	//stack<TraversalData> tStack;

	tStack.push(tdata);

	AxisAlignedBox3 backBox;
	AxisAlignedBox3 frontBox;

	int lastMem = 0;

	while (!tStack.empty())
	{
		float mem = GetMemUsage();

		if (lastMem / 10 != ((int)mem) / 10)
		{
			Debug << mem << " MB" << endl;
		}
		lastMem = (int)mem;

		if (1 && mem > mMaxMemory)
		{
			Debug << "memory limit reached: " << mem << endl;
			// count statistics on unprocessed leafs
			while (!tStack.empty())
			{
				EvaluateLeafStats(tStack.top());
				tStack.pop();
			}
			break;
		}

		TraversalData data = tStack.top();
		tStack.pop();

		VspKdNode *node = SubdivideNode(dynamic_cast<VspKdLeaf *>(data.mNode),
										data.mBox, backBox,	frontBox);
		if (result == NULL)
			result = node;

		if (!node->IsLeaf())
		{
			VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

			// push the children on the stack
			tStack.push(TraversalData(interior->GetBack(), backBox, data.mDepth + 1));
			tStack.push(TraversalData(interior->GetFront(), frontBox, data.mDepth + 1));
		}
		else
		{
			EvaluateLeafStats(data);
		}
	}

	return result;
}


// returns selected plane for subdivision
int VspKdTree::SelectPlane(VspKdLeaf *leaf,
						   const AxisAlignedBox3 &box,
						   float &position,
						   int &raysBack,
						   int &raysFront,
						   int &pvsBack,
						   int &pvsFront)
{
	int axis = 0;
	float costRatio = 0;

	if (splitType == ESplitRegular)
	{
		costRatio = BestCostRatioRegular(leaf,
										 box,
										 axis,
										 position,
										 raysBack,
										 raysFront,
										 pvsBack,
										 pvsFront);
	}
	else if (splitType == ESplitHeuristic)
	{
		costRatio = BestCostRatioHeuristic(leaf,
										   box,
										   axis,
										   position,
										   raysBack,
										   raysFront,
										   pvsBack,
										   pvsFront);
	}
	else
	{
		cerr << "VspKdTree: Unknown split heuristics\n";
		exit(1);
	}

	if (costRatio > mTermMaxCostRatio)
	{
		//Debug << "Too big cost ratio " << costRatio << endl;
		++ leaf->mMaxCostMisses;
		if (leaf->mMaxCostMisses > mTermMissTolerance)
		{
			++ mStat.maxCostNodes;
			return -1;
		}
	}

	if (0)
		Debug << "pvs=" << leaf->mPvsSize
			  << " rays=" << (int)leaf->mRays.size()
			  << " rc=" << leaf->GetAvgRayContribution()
			  << " axis=" << axis << endl;

	return axis;
}



float VspKdTree::EvalCostRatio(VspKdLeaf *leaf,
							   const AxisAlignedBox3 &box,
							   const int axis,
							   const float position,
							   int &raysBack,
							   int &raysFront,
							   int &pvsBack,
							   int &pvsFront)
{
	raysBack = 0;
	raysFront = 0;
	pvsFront = 0;
	pvsBack = 0;

	Intersectable::NewMail(3);

	// eval pvs size
	const int pvsSize = leaf->GetPvsSize();

	// this is the main ray classification loop!
	for(RayInfoContainer::iterator ri = leaf->mRays.begin();
			ri != leaf->mRays.end(); ++ ri)
	{
		if (!(*ri).mRay->IsActive())
			continue;

		// determine the side of this ray with respect to the plane
		float t;
		int side = (*ri).ComputeRayIntersection(axis, position, t);
			
		if (side <= 0)
			++ raysBack;

		if (side >= 0)
			++ raysFront;

		AddObject2Pvs((*ri).mRay->mTerminationObject, side, pvsBack, pvsFront);
	}

	//-- only one of these options should be one

	if (0) //-- only pvs
	{
		const float sum = float(pvsBack + pvsFront);
		const float oldCost = (float)pvsSize;

		return sum / oldCost;
	}

	//-- pvs + probability heuristics
	float pBack, pFront, pOverall;

	if (1)
	{
		// box length substitute for probability
		const float minBox = box.Min(axis);
		const float maxBox = box.Max(axis);

		pBack = position - minBox;
		pFront = maxBox - position;
		pOverall = maxBox - minBox;
	}

	if (0) //-- area substitute for probability
	{
		pOverall = box.SurfaceArea();

		const bool useMidSplit = true;
		//const bool useMidSplit = false;	
					
		if (!useMidSplit)
		{
			Vector3 pos = box.Max(); pos[axis] = position;
			pBack = AxisAlignedBox3(box.Min(), pos).SurfaceArea();

			pos = box.Min(); pos[axis] = position;
			pFront = AxisAlignedBox3(pos, box.Max()).SurfaceArea();
		}
		else
		{
			//-- simplified computation for mid split
			const int axis2 = (axis + 1) % 3;
			const int axis3 = (axis + 2) % 3;

			const float faceArea = 
				(box.Max(axis2) - box.Min(axis2)) *
				(box.Max(axis3) - box.Min(axis3));

			pBack = pFront = pOverall * 0.5f + faceArea;
		}
	}

	//-- ray density substitute for probability
	if (0)
	{
		pBack = (float)raysBack;
		pFront = (float)raysFront;
		pOverall = (float)leaf->mRays.size();
	}

	//Debug << axis << " " << pvsSize << " " << pvsBack << " " << pvsFront << endl;
	//Debug << pFront << " " << pBack << " " << pOverall << endl;

	// float sum = raysBack*(position - minBox) + raysFront*(maxBox - position);
	const float newCost = pvsBack * pBack + pvsFront * pFront;
	//  float oldCost = leaf->mRays.size();
	const float oldCost = (float)pvsSize * pOverall;

	return  (mCtDivCi + newCost) / oldCost;
}


float VspKdTree::BestCostRatioRegular(VspKdLeaf *leaf,
									  const AxisAlignedBox3 &box,
									  int &axis,
									  float &position,
									  int &raysBack,
									  int &raysFront,
									  int &pvsBack,
									  int &pvsFront)
{
	int nRaysBack[3], nRaysFront[3];
	int nPvsBack[3], nPvsFront[3];

	float nPosition[3];
	float nCostRatio[3];
	int bestAxis = -1;

	//AxisAlignedBox3 sBox = GetBBox(leaf);
	const int sAxis = box.Size().DrivingAxis();

	for (axis = 0; axis < 3; ++ axis)
	{
		if (!mOnlyDrivingAxis || axis == sAxis)
		{

			nPosition[axis] = (box.Min()[axis] + box.Max()[axis]) * 0.5f;

			nCostRatio[axis] = EvalCostRatio(leaf,
											 box,
											 axis,
											 nPosition[axis],
											 nRaysBack[axis],
											 nRaysFront[axis],
											 nPvsBack[axis],
											 nPvsFront[axis]);
			if (bestAxis == -1)
			{
				bestAxis = axis;
			}
			else if (nCostRatio[axis] < nCostRatio[bestAxis])
			{
				/*Debug << "pvs front " << nPvsBack[axis]
					  << " pvs back " << nPvsFront[axis]
					  << " overall pvs " << leaf->GetPvsSize() << endl;*/
				bestAxis = axis;
			}

		}
	}

	//-- assign best axis
	axis = bestAxis;
	position = nPosition[bestAxis];

	raysBack = nRaysBack[bestAxis];
	raysFront = nRaysFront[bestAxis];

	pvsBack = nPvsBack[bestAxis];
	pvsFront = nPvsFront[bestAxis];

	return nCostRatio[bestAxis];
}

float VspKdTree::BestCostRatioHeuristic(VspKdLeaf *leaf,
										const AxisAlignedBox3 &box,
										int &axis,
										float &position,
										int &raysBack,
										int &raysFront,
										int &pvsBack,
										int &pvsFront)
{
	//AxisAlignedBox3 box = GetBBox(leaf);
	//	AxisAlignedBox3 dirBox = GetDirBBox(node);

	axis = box.Size().DrivingAxis();

	SortSplitCandidates(leaf, axis);

	// go through the lists, count the number of objects left and right
	// and evaluate the following cost funcion:
	// C = ct_div_ci  + (ql*rl + qr*rr)/queries

	int rl = 0, rr = (int)leaf->mRays.size();
	int pl = 0, pr = leaf->GetPvsSize();

	float minBox = box.Min(axis);
	float maxBox = box.Max(axis);
	float sizeBox = maxBox - minBox;

	float minBand = minBox + 0.1f*(maxBox - minBox);
	float maxBand = minBox + 0.9f*(maxBox - minBox);

	float sum = rr*sizeBox;
	float minSum = 1e20f;

	Intersectable::NewMail();

	// set all object as belonging to the fron pvs
	for(RayInfoContainer::iterator ri = leaf->mRays.begin();
		ri != leaf->mRays.end(); ++ ri)
	{
		if ((*ri).mRay->IsActive())
		{
			Intersectable *object = (*ri).mRay->mTerminationObject;

			if (object)
				if (!object->Mailed())
				{
					object->Mail();
					object->mCounter = 1;
				}
				else
					++ object->mCounter;
		}
	}

	Intersectable::NewMail();

	for (vector<SortableEntry>::const_iterator ci = mSplitCandidates->begin();
		ci < mSplitCandidates->end(); ++ ci)
	{
		VssRay *ray;

		switch ((*ci).type)
		{
			case SortableEntry::ERayMin:
				{
					++ rl;
					ray = (*ci).ray;
					Intersectable *object = ray->mTerminationObject;
					if (object && !object->Mailed())
					{
						object->Mail();
						++ pl;
					}
					break;
				}
			case SortableEntry::ERayMax:
				{
					-- rr;

					ray = (*ci).ray;
					Intersectable *object = ray->mTerminationObject;

					if (object)
					{
						if (-- object->mCounter == 0)
						-- pr;
					}

					break;
				}
		}

		if ((*ci).value > minBand && (*ci).value < maxBand)
		{
			sum = pl*((*ci).value - minBox) + pr*(maxBox - (*ci).value);

			//  cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
			// cout<<"cost= "<<sum<<endl;

			if (sum < minSum)
			{
				minSum = sum;
				position = (*ci).value;

				raysBack = rl;
				raysFront = rr;

				pvsBack = pl;
				pvsFront = pr;

			}
		}
	}

	float oldCost = (float)leaf->GetPvsSize();
	float newCost = mCtDivCi + minSum / sizeBox;
	float ratio = newCost / oldCost;

	//Debug << "costRatio=" << ratio << " pos=" << position << " t=" << (position - minBox) / (maxBox - minBox)
	//     <<"\t q=(" << queriesBack << "," << queriesFront << ")\t r=(" << raysBack << "," << raysFront << ")" << endl;

	return ratio;
}

void VspKdTree::SortSplitCandidates(VspKdLeaf *node,
									const int axis)
{
	mSplitCandidates->clear();

	int requestedSize = 2 * (int)(node->mRays.size());
	// creates a sorted split candidates array
	if (mSplitCandidates->capacity() > 500000 &&
		requestedSize < (int)(mSplitCandidates->capacity()/10) )
	{
    	DEL_PTR(mSplitCandidates);
   		mSplitCandidates = new vector<SortableEntry>;
	}

	mSplitCandidates->reserve(requestedSize);

	// insert all queries
	for(RayInfoContainer::const_iterator ri = node->mRays.begin();
		ri < node->mRays.end(); ++ ri)
	{
		bool positive = (*ri).mRay->HasPosDir(axis);
		mSplitCandidates->push_back(SortableEntry(positive ? SortableEntry::ERayMin : SortableEntry::ERayMax,
												  (*ri).ExtrapOrigin(axis), (*ri).mRay));

		mSplitCandidates->push_back(SortableEntry(positive ? SortableEntry::ERayMax : SortableEntry::ERayMin,
												  (*ri).ExtrapTermination(axis), (*ri).mRay));
	}

	stable_sort(mSplitCandidates->begin(), mSplitCandidates->end());
}


void VspKdTree::EvaluateLeafStats(const TraversalData &data)
{
	// the node became a leaf -> evaluate stats for leafs
	VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(data.mNode);

	if (leaf->GetPvsSize() > mStat.maxPvsSize)
		mStat.maxPvsSize = leaf->GetPvsSize();

	if ((int)(leaf->mRays.size()) > mStat.maxRayRefs)
		mStat.maxRayRefs = (int)leaf->mRays.size();

	mStat.rays += (int)leaf->mRays.size();

	if (data.mDepth >= mTermMaxDepth)
	{
		++ mStat.maxDepthNodes;
	}

	if (leaf->GetPvsSize() < mTermMinPvs)
		++ mStat.minPvsNodes;

	mStat.accPvsSize += leaf->GetPvsSize();

	if ((int)leaf->GetRays().size() < mTermMinRays)
		++ mStat.minRaysNodes;

	if (leaf->GetAvgRayContribution() > mTermMaxRayContribution)
		++ mStat.maxRayContribNodes;

	if (SqrMagnitude(data.mBox.Size()) <= mTermMinSize)
		++ mStat.minSizeNodes;
}


inline bool VspKdTree::TerminationCriteriaMet(const VspKdLeaf *leaf,
											  const AxisAlignedBox3 &box) const
{
	return ((leaf->GetPvsSize() < mTermMinPvs) ||
		    (leaf->mRays.size() < mTermMinRays) ||
			(leaf->GetAvgRayContribution() > mTermMaxRayContribution ) ||
			(leaf->mDepth >= mTermMaxDepth) ||
			(mStat.Leaves() >= mMaxViewCells) ||
			(SqrMagnitude(box.Size()) <= mTermMinSize));
}


VspKdNode *VspKdTree::SubdivideNode(VspKdLeaf *leaf,
									const AxisAlignedBox3 &box,
									AxisAlignedBox3 &backBBox,
									AxisAlignedBox3 &frontBBox)
{
	if (TerminationCriteriaMet(leaf, box))
	{
		if (1 && leaf->mDepth >= mTermMaxDepth)
		{
			Debug << "Warning: max depth reached depth=" << (int)leaf->mDepth<<" rays=" << (int)leaf->mRays.size() << endl;
			Debug << "Bbox: " << GetBBox(leaf) << endl;
		}
		//Debug << "depth: " << (int)leaf->mDepth << " pvs: " << leaf->GetPvsSize() << " rays: " << leaf->mRays.size() << endl;

		return leaf;
	}

	float position;
	// first count ray sides
	int raysBack;
	int raysFront;
	int pvsBack;
	int pvsFront;

	// select subdivision axis
	const int axis = SelectPlane(leaf, box, position, raysBack, raysFront, pvsBack, pvsFront);
	//Debug << "rays back=" << raysBack << " rays front=" << raysFront << " pvs back=" << pvsBack << " pvs front=" <<	pvsFront << endl;

	if (axis == -1)
		return leaf;
	
	mStat.nodes += 2;
	++ mStat.splits[axis];

	// add the new nodes to the tree
	VspKdInterior *node = 
		new VspKdInterior(dynamic_cast<VspKdInterior *>(leaf->mParent));

	node->mAxis = axis;
	node->mPosition = position;
	node->mBox = box;

	backBBox = box;
	frontBBox = box;

	VspKdLeaf *back = new VspKdLeaf(node, raysBack, leaf->GetMaxCostMisses());
	back->SetPvsSize(pvsBack);
	VspKdLeaf *front = new VspKdLeaf(node, raysFront, leaf->GetMaxCostMisses());
	front->SetPvsSize(pvsFront);

	// replace a link from node's parent
	if (leaf->mParent)
	{
		VspKdInterior *parent = dynamic_cast<VspKdInterior *>(leaf->mParent);
		parent->ReplaceChildLink(leaf, node);
	}

	// and setup child links
	node->SetupChildLinks(back, front);

	if (axis <= VspKdNode::SPLIT_Z)
	{
		backBBox.SetMax(axis, position);
		frontBBox.SetMin(axis, position);

		for(RayInfoContainer::iterator ri = leaf->mRays.begin();
				ri != leaf->mRays.end(); ++ ri)
		{
			if ((*ri).mRay->IsActive())
			{
				// first unref ray from the former leaf
				(*ri).mRay->Unref();
				  float t;
				// determine the side of this ray with respect to the plane
				int side = node->ComputeRayIntersection(*ri, t);

				if (side == 0)
				{
					if ((*ri).mRay->HasPosDir(axis))
					{
						back->AddRay(RayInfo((*ri).mRay,
											 (*ri).mMinT,
											 t));
						front->AddRay(RayInfo((*ri).mRay,
									 		  t,
											  (*ri).mMaxT));
					}
					else
					{
						back->AddRay(RayInfo((*ri).mRay,
											 t,
											 (*ri).mMaxT));
						front->AddRay(RayInfo((*ri).mRay,
									 		  (*ri).mMinT,
											  t));
					}
				}
				else
					if (side == 1)
						front->AddRay(*ri);
					else
						back->AddRay(*ri);
			} else
				(*ri).mRay->Unref();
		}
	}
	else
	{
		// rays front/back

        for (RayInfoContainer::iterator ri = leaf->mRays.begin();
			ri != leaf->mRays.end(); ++ ri)
		{
			if ((*ri).mRay->IsActive())
			{
				// first unref ray from the former leaf
				(*ri).mRay->Unref();

				int side;
				if ((*ri).mRay->GetDirParametrization(axis - 3) > position)
					side = 1;
				else
					side = -1;

				if (side == 1)
					front->AddRay(*ri);
				else
					back->AddRay(*ri);
			}
			else
				(*ri).mRay->Unref();
		}
	}

	// update stats
	mStat.rayRefs -= (int)leaf->mRays.size();
	mStat.rayRefs += raysBack + raysFront;

	DEL_PTR(leaf);

	return node;
}

int VspKdTree::ReleaseMemory(const int time)
{
	stack<VspKdNode *> tstack;

	// find a node in the tree which subtree will be collapsed
	int maxAccessTime = time - mAccessTimeThreshold;
	int released = 0;

	tstack.push(mRoot);

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();

		if (!node->IsLeaf())
		{
			VspKdInterior *in = dynamic_cast<VspKdInterior *>(node);
			// cout<<"depth="<<(int)in->depth<<" time="<<in->lastAccessTime<<endl;
			if (in->mDepth >= mMinCollapseDepth && in->mLastAccessTime <= maxAccessTime)
			{
				released = CollapseSubtree(node, time);
				break;
			}
			if (in->GetBack()->GetAccessTime() < in->GetFront()->GetAccessTime())
			{
				tstack.push(in->GetFront());
				tstack.push(in->GetBack());
			}
			else
			{
				tstack.push(in->GetBack());
				tstack.push(in->GetFront());
			}
		}
	}

	while (tstack.empty())
	{
		// could find node to collaps...
		// cout<<"Could not find a node to release "<<endl;
		break;
	}

	return released;
}


VspKdNode *VspKdTree::SubdivideLeaf(VspKdLeaf *leaf,
										const float sizeThreshold)
{
	VspKdNode *node = leaf;

	AxisAlignedBox3 leafBBox = GetBBox(leaf);

	static int pass = 0;
	++ pass;

	// check if we should perform a dynamic subdivision of the leaf
	if (// leaf->mRays.size() > (unsigned)termMinCost &&
		(leaf->GetPvsSize() >= mTermMinPvs) &&
		(SqrMagnitude(leafBBox.Size()) > sizeThreshold) )
	{
    	// memory check and release
		if (GetMemUsage() > mMaxTotalMemory)
			ReleaseMemory(pass);

		AxisAlignedBox3 backBBox, frontBBox;

		// subdivide the node
		node = SubdivideNode(leaf, leafBBox, backBBox, frontBBox);
	}
	return node;
}



void VspKdTree::UpdateRays(VssRayContainer &remove,
						   VssRayContainer &add)
{
	VspKdLeaf::NewMail();

	// schedule rays for removal
	for(VssRayContainer::const_iterator ri = remove.begin();
		ri != remove.end(); ++ ri)
	{
		(*ri)->ScheduleForRemoval();
	}

	int inactive = 0;

	for (VssRayContainer::const_iterator ri = remove.begin(); ri != remove.end(); ++ ri)
	{
		if ((*ri)->ScheduledForRemoval())
			//      RemoveRay(*ri, NULL, false);
			// !!! BUG - with true it does not work correctly - aggreated delete
			RemoveRay(*ri, NULL, true);
		else
			++ inactive;
	}

	//  cout<<"all/inactive"<<remove.size()<<"/"<<inactive<<endl;
	for (VssRayContainer::const_iterator ri = add.begin(); ri != add.end(); ++ ri)
	{
		AddRay(*ri);
	}
}


void VspKdTree::RemoveRay(VssRay *ray,
						  vector<VspKdLeaf *> *affectedLeaves,
						  const bool removeAllScheduledRays)
{
	stack<RayTraversalData> tstack;

	tstack.push(RayTraversalData(mRoot, RayInfo(ray)));

	RayTraversalData data;

	// cout<<"Number of ray refs = "<<ray->RefCount()<<endl;
	while (!tstack.empty())
	{
		data = tstack.top();
		tstack.pop();

		if (!data.mNode->IsLeaf())
		{
			// split the set of rays in two groups intersecting the
			// two subtrees
			TraverseInternalNode(data, tstack);
      	}
		else
		{
			// remove the ray from the leaf
			// find the ray in the leaf and swap it with the last ray...
			VspKdLeaf *leaf = (VspKdLeaf *)data.mNode;

			if (!leaf->Mailed())
			{
				leaf->Mail();

				if (affectedLeaves)
					affectedLeaves->push_back(leaf);

				if (removeAllScheduledRays)
				{
					int tail = (int)leaf->mRays.size() - 1;

					for (int i=0; i < (int)(leaf->mRays.size()); ++ i)
					{
						if (leaf->mRays[i].mRay->ScheduledForRemoval())
						{
							// find a ray to replace it with
							while (tail >= i && leaf->mRays[tail].mRay->ScheduledForRemoval())
							{
								++ mStat.removedRayRefs;
								leaf->mRays[tail].mRay->Unref();
								leaf->mRays.pop_back();

								-- tail;
							}

							if (tail < i)
								break;

							++ mStat.removedRayRefs;

							leaf->mRays[i].mRay->Unref();
							leaf->mRays[i] = leaf->mRays[tail];
							leaf->mRays.pop_back();
							-- tail;
						}
					}
				}
			}

			if (!removeAllScheduledRays)
				for (int i=0; i < (int)leaf->mRays.size(); i++)
				{
					if (leaf->mRays[i].mRay == ray)
					{
						++ mStat.removedRayRefs;
						ray->Unref();
						leaf->mRays[i] = leaf->mRays[leaf->mRays.size() - 1];
						leaf->mRays.pop_back();
					    // check this ray again
						break;
					}
				}
		}
	}

	if (ray->RefCount() != 0)
	{
		cerr << "Error: Number of remaining refs = " << ray->RefCount() << endl;
		exit(1);
	}

}

void VspKdTree::AddRay(VssRay *ray)
{
	stack<RayTraversalData> tstack;

	tstack.push(RayTraversalData(mRoot, RayInfo(ray)));

	RayTraversalData data;

	while (!tstack.empty())
	{
		data = tstack.top();
		tstack.pop();

		if (!data.mNode->IsLeaf())
		{
			TraverseInternalNode(data, tstack);
		}
		else
		{
			// remove the ray from the leaf
			// find the ray in the leaf and swap it with the last ray
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(data.mNode);

			leaf->AddRay(data.mRayData);
			++ mStat.addedRayRefs;
		}
	}
}

void VspKdTree::TraverseInternalNode(RayTraversalData &data,
									 stack<RayTraversalData> &tstack)
{
	VspKdInterior *in = dynamic_cast<VspKdInterior *>(data.mNode);

	if (in->mAxis <= VspKdNode::SPLIT_Z)
	{
	    // determine the side of this ray with respect to the plane
		float t;
		const int side = in->ComputeRayIntersection(data.mRayData, t);

		if (side == 0)
		{
			if (data.mRayData.mRay->HasPosDir(in->mAxis))
			{
				tstack.push(RayTraversalData(in->GetBack(),
							RayInfo(data.mRayData.mRay, data.mRayData.mMinT, t)));

				tstack.push(RayTraversalData(in->GetFront(),
							RayInfo(data.mRayData.mRay, t, data.mRayData.mMaxT)));

			}
			else
			{
				tstack.push(RayTraversalData(in->GetBack(),
							RayInfo(data.mRayData.mRay,
							t,
							data.mRayData.mMaxT)));
				tstack.push(RayTraversalData(in->GetFront(),
							RayInfo(data.mRayData.mRay,
							data.mRayData.mMinT,
							t)));
			}
		}
		else
			if (side == 1)
				tstack.push(RayTraversalData(in->GetFront(), data.mRayData));
			else
				tstack.push(RayTraversalData(in->GetBack(), data.mRayData));
	}
	else
	{
		// directional split
		if (data.mRayData.mRay->GetDirParametrization(in->mAxis - 3) > in->mPosition)
			tstack.push(RayTraversalData(in->GetFront(), data.mRayData));
		else
			tstack.push(RayTraversalData(in->GetBack(), data.mRayData));
	}
}


int VspKdTree::CollapseSubtree(VspKdNode *sroot, const int time)
{
	// first count all rays in the subtree
	// use mail 1 for this purpose
	stack<VspKdNode *> tstack;

	int rayCount = 0;
	int totalRayCount = 0;
	int collapsedNodes = 0;

#if DEBUG_COLLAPSE
	cout << "Collapsing subtree" << endl;
	cout << "accessTime=" << sroot->GetAccessTime() << endl;
	cout << "depth=" << (int)sroot->depth << endl;
#endif

	// tstat.collapsedSubtrees++;
	// tstat.collapseDepths += (int)sroot->depth;
	// tstat.collapseAccessTimes += time - sroot->GetAccessTime();
	tstack.push(sroot);
	VssRay::NewMail();

	while (!tstack.empty())
	{
		++ collapsedNodes;

		VspKdNode *node = tstack.top();
		tstack.pop();
		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = (VspKdLeaf *) node;

			for(RayInfoContainer::iterator ri = leaf->mRays.begin();
					ri != leaf->mRays.end(); ++ ri)
			{
				++ totalRayCount;

				if ((*ri).mRay->IsActive() && !(*ri).mRay->Mailed())
				{
					(*ri).mRay->Mail();
					++ rayCount;
				}
			}
		}
		else
		{
			tstack.push(dynamic_cast<VspKdInterior *>(node)->GetFront());
			tstack.push(dynamic_cast<VspKdInterior *>(node)->GetBack());
		}
	}

	VssRay::NewMail();

	// create a new node that will hold the rays
	VspKdLeaf *newLeaf = new VspKdLeaf(sroot->mParent, rayCount);

	VspKdInterior *parent = 
		dynamic_cast<VspKdInterior *>(newLeaf->mParent);

	if (parent)
	{
		parent->ReplaceChildLink(sroot, newLeaf);
	}
	tstack.push(sroot);

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(node);

			for(RayInfoContainer::iterator ri = leaf->mRays.begin();
					ri != leaf->mRays.end(); ++ ri)
			{
				// unref this ray from the old node
				if ((*ri).mRay->IsActive())
				{
					(*ri).mRay->Unref();
					if (!(*ri).mRay->Mailed())
					{
						(*ri).mRay->Mail();
						newLeaf->AddRay(*ri);
					}
				}
				else
					(*ri).mRay->Unref();
			}
		}
		else
		{
			VspKdInterior *interior =
				dynamic_cast<VspKdInterior *>(node);
			tstack.push(interior->GetBack());
			tstack.push(interior->GetFront());
		}
	}

	// delete the node and all its children
	DEL_PTR(sroot);

	// for(VspKdNode::SRayContainer::iterator ri = newleaf->mRays.begin();
    //      ri != newleaf->mRays.end(); ++ ri)
	//     (*ri).ray->UnMail(2);

#if DEBUG_COLLAPSE
	cout << "Total memory before=" << GetMemUsage() << endl;
#endif

	mStat.nodes -= collapsedNodes - 1;
	mStat.rayRefs -= totalRayCount - rayCount;

#if DEBUG_COLLAPSE
	cout << "collapsed nodes" << collapsedNodes << endl;
	cout << "collapsed rays" << totalRayCount - rayCount << endl;
	cout << "Total memory after=" << GetMemUsage() << endl;
	cout << "================================" << endl;
#endif

	//  tstat.collapsedNodes += collapsedNodes;
	//  tstat.collapsedRays += totalRayCount - rayCount;
    return totalRayCount - rayCount;
}


int VspKdTree::GetPvsSize(VspKdNode *node, const AxisAlignedBox3 &box) const
{
	stack<VspKdNode *> tstack;
	tstack.push(mRoot);

	Intersectable::NewMail();
	int pvsSize = 0;

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();

	  if (node->IsLeaf())
		{
			VspKdLeaf *leaf = (VspKdLeaf *)node;
			for (RayInfoContainer::iterator ri = leaf->mRays.begin();
				 ri != leaf->mRays.end(); ++ ri)
			{
				if ((*ri).mRay->IsActive())
				{
					Intersectable *object;
#if BIDIRECTIONAL_RAY
					object = (*ri).mRay->mOriginObject;

					if (object && !object->Mailed())
					{
						++ pvsSize;
						object->Mail();
					}
#endif
					object = (*ri).mRay->mTerminationObject;
					if (object && !object->Mailed())
					{
						++ pvsSize;
						object->Mail();
					}
				}
			}
		}
		else
		{
			VspKdInterior *in = dynamic_cast<VspKdInterior *>(node);

			if (in->mAxis < 3)
			{
				if (box.Max(in->mAxis) >= in->mPosition)
					tstack.push(in->GetFront());

				if (box.Min(in->mAxis) <= in->mPosition)
					tstack.push(in->GetBack());
			}
			else
			{
				// both nodes for directional splits
				tstack.push(in->GetFront());
				tstack.push(in->GetBack());
			}
		}
	}

	return pvsSize;
}

void VspKdTree::GetRayContributionStatistics(float &minRayContribution,
											 float &maxRayContribution,
											 float &avgRayContribution)
{
	stack<VspKdNode *> tstack;
	tstack.push(mRoot);

	minRayContribution = 1.0f;
	maxRayContribution = 0.0f;

	float sumRayContribution = 0.0f;
	int leaves = 0;

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();
		if (node->IsLeaf())
		{
			leaves++;
			VspKdLeaf *leaf = (VspKdLeaf *)node;
			float c = leaf->GetAvgRayContribution();

			if (c > maxRayContribution)
				maxRayContribution = c;
			if (c < minRayContribution)
				minRayContribution = c;
			sumRayContribution += c;

		}
		else
		{
			VspKdInterior *in = (VspKdInterior *)node;
			
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}

	Debug << "sum=" << sumRayContribution << endl;
	Debug << "leaves=" << leaves << endl;
	avgRayContribution = sumRayContribution / (float)leaves;
}


int VspKdTree::GenerateRays(const float ratioPerLeaf,
							SimpleRayContainer &rays)
{
	stack<VspKdNode *> tstack;
	tstack.push(mRoot);

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(node);

			float c = leaf->GetAvgRayContribution();
			int num = (int)(c*ratioPerLeaf + 0.5);
			//			cout<<num<<" ";

			for (int i=0; i < num; i++)
			{
				Vector3 origin = GetBBox(leaf).GetRandomPoint();
				/*Vector3 dirVector = GetDirBBox(leaf).GetRandomPoint();
				Vector3 direction = Vector3(sin(dirVector.x), sin(dirVector.y), cos(dirVector.x));
				//cout<<"dir vector.x="<<dirVector.x<<"direction'.x="<<atan2(direction.x, direction.y)<<endl;
				rays.push_back(SimpleRay(origin, direction));*/
			}

		}
		else
		{
			VspKdInterior *in =
				dynamic_cast<VspKdInterior *>(node);
		
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}

	return (int)rays.size();
}


float VspKdTree::GetAvgPvsSize()
{
	stack<VspKdNode *> tstack;
	tstack.push(mRoot);

	int sumPvs = 0;
	int leaves = 0;

	while (!tstack.empty())
	{
		VspKdNode *node = tstack.top();
		tstack.pop();

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = (VspKdLeaf *)node;
			// update pvs size
			leaf->UpdatePvsSize();
			sumPvs += leaf->GetPvsSize();
			leaves++;
		}
		else
		{
			VspKdInterior *in = (VspKdInterior *)node;
			
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}

	return sumPvs / (float)leaves;
}

VspKdNode *VspKdTree::GetRoot() const
{
	return mRoot;
}


AxisAlignedBox3 VspKdTree::GetBBox(VspKdNode *node) const
{
	if (node->mParent == NULL)
		return mBox;

	if (!node->IsLeaf())
	{
		if (node->Type() == VspKdNode::EIntermediate)
            return (dynamic_cast<VspKdInterior *>(node))->mBox;
		else
			return (dynamic_cast<VspKdIntermediate *>(node))->mBox;
	}

	VspKdInterior *parent = dynamic_cast<VspKdInterior *>(node->mParent);

	if (parent->mAxis >= 3)
		return parent->mBox;

	AxisAlignedBox3 box(parent->mBox);
	if (parent->GetFront() == node)
		box.SetMin(parent->mAxis, parent->mPosition);
	else
		box.SetMax(parent->mAxis, parent->mPosition);

	return box;
}


int	VspKdTree::GetRootPvsSize() const
{
	return GetPvsSize(mRoot, mBox);
}


const VspKdStatistics &VspKdTree::GetStatistics() const
{
	return mStat;
}


void VspKdTree::AddRays(VssRayContainer &add)
{
	VssRayContainer remove;
	UpdateRays(remove, add);
}

// return memory usage in MB
float VspKdTree::GetMemUsage() const
{
	return
		(sizeof(VspKdTree) +
		 mStat.Leaves() * sizeof(VspKdLeaf) +
		 mStat.Interior() * sizeof(VspKdInterior) +
		 mStat.rayRefs * sizeof(RayInfo)) / (1024.0f * 1024.0f);
}


float VspKdTree::GetRayMemUsage() const
{
	return mStat.rays * (sizeof(VssRay))/(1024.0f * 1024.0f);
}


int VspKdTree::ComputePvsSize(VspKdNode *node,
							  const RayInfoContainer &globalRays) const
{
	int pvsSize = 0;

	const AxisAlignedBox3 box = GetBBox(node);

	RayInfoContainer::const_iterator it, it_end = globalRays.end();

	Intersectable::NewMail();

	// warning: implicit conversion from VssRay to Ray
	for (it = globalRays.begin(); it != globalRays.end(); ++ it)
	{
		VssRay *ray = (*it).mRay;

		// ray intersects node bounding box
		if (box.GetMinMaxT(*ray, NULL, NULL))
		{
			if (ray->mTerminationObject && !ray->mTerminationObject->Mailed())
			{
				ray->mTerminationObject->Mail();
				++ pvsSize;
			}
		}
	}
	return pvsSize;
}


void VspKdTree::CollectLeaves(vector<VspKdLeaf *> &leaves) const
{
	stack<VspKdNode *> nodeStack;
	nodeStack.push(mRoot);

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

		nodeStack.pop();

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(node);
			leaves.push_back(leaf);
		}
		else
		{
			VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

			nodeStack.push(interior->GetBack());
			nodeStack.push(interior->GetFront());
		}
	}
}


int VspKdTree::FindNeighbors(VspKdLeaf *n,
							 vector<VspKdLeaf *> &neighbors,
							 bool onlyUnmailed)
{
	stack<VspKdNode *> nodeStack;

	nodeStack.push(mRoot);

	AxisAlignedBox3 box = GetBBox(n);

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

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(node);

			if (leaf != n && (!onlyUnmailed || !leaf->Mailed()))
				neighbors.push_back(leaf);
		}
		else
		{
			VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

			if (interior->mPosition > box.Max(interior->mAxis))
				nodeStack.push(interior->mBack);
			else
			{
				if (interior->mPosition < box.Min(interior->mAxis))
					nodeStack.push(interior->mFront);
				else
				{
					// random decision
					nodeStack.push(interior->mBack);
					nodeStack.push(interior->mFront);
				}
			}
		}
	}

	return (int)neighbors.size();
}


void VspKdTree::SetViewCellsManager(ViewCellsManager *vcm)
{
	mViewCellsManager = vcm;
}


int VspKdTree::CastLineSegment(const Vector3 &origin,
							   const Vector3 &termination,
							   ViewCellContainer &viewcells)
{
	int hits = 0;

	float mint = 0.0f, maxt = 1.0f;
	const Vector3 dir = termination - origin;

	stack<LineTraversalData> tStack;

	Intersectable::NewMail();

	Vector3 entp = origin;
	Vector3 extp = termination;

	VspKdNode *node = mRoot;
	VspKdNode *farChild;

	float position;
	int axis;

	cerr<<"HERE"<<endl;
	while (1)
	{
		if (!node->IsLeaf())
		{
			VspKdInterior *in = (VspKdInterior *)node;
			position = in->mPosition;
			axis = in->mAxis;

			if (entp[axis] <= position)
			{
				if (extp[axis] <= position)
				{
					node = in->mBack;
					// cases N1,N2,N3,P5,Z2,Z3
					continue;
				} else
				{
					// case N4
					node = in->mBack;
					farChild = in->mFront;
				}
			}
			else
			{
				if (position <= extp[axis])
				{
					node = in->mFront;
					// cases P1,P2,P3,N5,Z1
					continue;
				}
				else
				{
					node = in->mFront;
					farChild = in->mBack;
					// case P4
				}
			}

 			// $$ modification 3.5.2004 - hints from Kamil Ghais
			// case N4 or P4
			float tdist = (position - origin[axis]) / dir[axis];
			tStack.push(LineTraversalData(farChild, extp, maxt)); //TODO
			extp = origin + dir * tdist;
			maxt = tdist;
		}
		else
		{
			// compute intersection with all objects in this leaf
			VspKdLeaf *leaf = (VspKdLeaf *)node;
			ViewCell *vc = leaf->GetViewCell();

			if (!vc->Mailed())
			{
				vc->Mail();
				viewcells.push_back(vc);
				++ hits;
			}

#if 0			
			if (0) //matt TODO: REMOVE LATER
			  leaf->mRays.push_back(RayInfo(new VssRay(origin, termination, NULL, NULL, 0)));
#endif
			
			// get the next node from the stack
			if (tStack.empty())
				break;

			entp = extp;
			mint = maxt;
			
			LineTraversalData &s  = tStack.top();
			node = s.mNode;
			extp = s.mExitPoint;
			maxt = s.mMaxT;
			tStack.pop();
		}
	}

	return hits;
}


void VspKdTree::GenerateViewCell(VspKdLeaf *leaf)
{
	RayInfoContainer::const_iterator it, it_end = leaf->GetRays().end();

	VspKdViewCell *vc = dynamic_cast<VspKdViewCell *>(mViewCellsManager->GenerateViewCell());
	leaf->SetViewCell(vc);

	vc->SetVolume(GetBBox(leaf).GetVolume());
	vc->SetArea(GetBBox(leaf).SurfaceArea());

	vc->mLeaf = leaf;

	for (it = leaf->GetRays().begin(); it != it_end; ++ it)
	{
		VssRay *ray = (*it).mRay;

		if (ray->mTerminationObject)
		  vc->GetPvs().AddSample(ray->mTerminationObject, ray->mPdf);

		if (ray->mOriginObject)
		  vc->GetPvs().AddSample(ray->mOriginObject, ray->mPdf);
	}
}


bool VspKdTree::MergeViewCells(VspKdLeaf *l1, VspKdLeaf *l2)
{
	//-- change pointer to view cells of all leaves associated 
	//-- with the previous view cells
	VspKdViewCell *fVc = l1->GetViewCell();
	VspKdViewCell *bVc = l2->GetViewCell();

	VspKdViewCell *vc = dynamic_cast<VspKdViewCell *>(
		mViewCellsManager->MergeViewCells(fVc, bVc));

	// if merge was unsuccessful
	if (!vc) return false;
// TODO
#if HAS_TO_BE_REDONE
	// set new size of view cell
	vc->SetVolume(fVc->GetVolume() + bVc->GetVolume());
	// new area
	vc->SetArea(fVc->GetArea() + bVc->GetArea());

	vector<VspKdLeaf *> fLeaves = fVc->mLeaves;
	vector<VspKdLeaf *> bLeaves = bVc->mLeaves;

	vector<VspKdLeaf *>::const_iterator it;

	//-- change view cells of all the other leaves the view cell belongs to
	for (it = fLeaves.begin(); it != fLeaves.end(); ++ it)
	{
		(*it)->SetViewCell(vc);
		vc->mLeaves.push_back(*it);
	}

	for (it = bLeaves.begin(); it != bLeaves.end(); ++ it)
	{
		(*it)->SetViewCell(vc);
		vc->mLeaves.push_back(*it);
	}
#endif
	vc->Mail();

	// clean up old view cells
	DEL_PTR(fVc);
	DEL_PTR(bVc);

	return true;
}

void VspKdTree::CollectMergeCandidates()
{
	VspKdMergeCandidate::sOverallCost = 0;
	vector<VspKdLeaf *> leaves;

	// collect the leaves, e.g., the "voxels" that will build the view cells
	CollectLeaves(leaves);

	VspKdLeaf::NewMail();

	vector<VspKdLeaf *>::const_iterator it, it_end = leaves.end();

	// generate view cells
	for (it = leaves.begin(); it != it_end; ++ it)
		GenerateViewCell(*it);

	// find merge candidates and push them into queue
	for (it = leaves.begin(); it != it_end; ++ it)
	{
		VspKdLeaf *leaf = *it;
		ViewCell *vc = leaf->GetViewCell();
		// no leaf is part of two merge candidates
		leaf->Mail();
		VspKdMergeCandidate::sOverallCost += 
			vc->GetVolume() * vc->GetPvs().GetSize();
		vector<VspKdLeaf *> neighbors;
		FindNeighbors(leaf, neighbors, true);

		vector<VspKdLeaf *>::const_iterator nit,
			nit_end = neighbors.end();

		for (nit = neighbors.begin(); nit != nit_end; ++ nit)
		{
			mMergeQueue.push(VspKdMergeCandidate(leaf, *nit));
		}
	}
}


int VspKdTree::MergeViewCells(const VssRayContainer &rays)
{
	CollectMergeCandidates();

	int merged = 0;

	int vcSize = mStat.Leaves();
	// use priority queue to merge leaves
	while (!mMergeQueue.empty() && (vcSize > mMergeMinViewCells) &&
		   (mMergeQueue.top().GetMergeCost() < 
		    mMergeMaxCostRatio * VspKdMergeCandidate::sOverallCost))
	{
		//Debug << "mergecost: " << mergeQueue.top().GetMergeCost() / 
		//VspKdMergeCandidate::sOverallCost << " " << mMergeMaxCostRatio << endl;
		VspKdMergeCandidate mc = mMergeQueue.top();
		mMergeQueue.pop();

		// both view cells equal!
		if (mc.GetLeaf1()->GetViewCell() == mc.GetLeaf2()->GetViewCell())
			continue;

		if (mc.Valid())
		{
			ViewCell::NewMail();
			MergeViewCells(mc.GetLeaf1(), mc.GetLeaf2());

			++ merged;
			-- vcSize;
			// increase absolute merge cost
			VspKdMergeCandidate::sOverallCost += mc.GetMergeCost();
		}
		// merge candidate not valid, because one of the leaves was already
		// merged with another one
		else
		{
			// validate and reinsert into queue
			mc.SetValid();
			mMergeQueue.push(mc);
			//Debug << "validate " << mc.GetMergeCost() << endl;
		}
	}

	Debug << "merged " << merged << " of " << mStat.Leaves() << " leaves" << endl;

	//TODO: should return sample contributions
	return merged;
}


void VspKdTree::RepairViewCellsLeafLists()
{
	// list not valid anymore => clear
	stack<VspKdNode *> nodeStack;
	nodeStack.push(mRoot);

	ViewCell::NewMail();

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

		if (node->IsLeaf())
		{
			VspKdLeaf *leaf = dynamic_cast<VspKdLeaf *>(node);

			VspKdViewCell *viewCell = leaf->GetViewCell();
#if HAS_TO_BE_REDONE
			if (!viewCell->Mailed())
			{
				viewCell->mLeaves.clear();
				viewCell->Mail();
			}

			viewCell->mLeaves.push_back(leaf);
#endif
		}
		else
		{
			VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

			nodeStack.push(interior->GetFront());
			nodeStack.push(interior->GetBack());
		}
	}
}


VspKdNode *VspKdTree::CollapseTree(VspKdNode *node, int &collapsed)
{
	if (node->IsLeaf())
		return node;

	VspKdInterior *interior = dynamic_cast<VspKdInterior *>(node);

	VspKdNode *front = CollapseTree(interior->GetFront(), collapsed);
	VspKdNode *back = CollapseTree(interior->GetBack(), collapsed);

	if (front->IsLeaf() && back->IsLeaf())
	{
		VspKdLeaf *frontLeaf = dynamic_cast<VspKdLeaf *>(front);
		VspKdLeaf *backLeaf = dynamic_cast<VspKdLeaf *>(back);

		//-- collapse tree
		if (frontLeaf->GetViewCell() == backLeaf->GetViewCell())
		{
			VspKdViewCell *vc = frontLeaf->GetViewCell();

			VspKdLeaf *leaf = new VspKdLeaf(interior->GetParent(), 0);
			leaf->SetViewCell(vc);
			//leaf->SetTreeValid(frontLeaf->TreeValid());

			// replace a link from node's parent
			if (leaf->mParent)
			{
				// replace a link from node's parent
				VspKdInterior *parent = 
					dynamic_cast<VspKdInterior *>(leaf->mParent);
				parent->ReplaceChildLink(node, leaf);
			}
			++ collapsed;
			delete interior;

			return leaf;
		}
	}

	return node;
}


int VspKdTree::CollapseTree()
{
	int collapsed = 0;
	CollapseTree(mRoot, collapsed);
	// revalidate leaves
	RepairViewCellsLeafLists();

	return collapsed;
}


int VspKdTree::RefineViewCells(const VssRayContainer &rays)
{
	int shuffled = 0;

	Debug << "refining " << (int)mMergeQueue.size() << " candidates " << endl;
	BspLeaf::NewMail();

	// Use priority queue of remaining leaf pairs 
	// These candidates either share the same view cells or
	// are border leaves which share a boundary.
	// We test if they can be shuffled, i.e.,
	// either one leaf is made part of one view cell or the other
	// leaf is made part of the other view cell. It is tested if the
	// remaining view cells are "better" than the old ones.
	while (!mMergeQueue.empty())
	{
		VspKdMergeCandidate mc = mMergeQueue.top();
		mMergeQueue.pop();

		// both view cells equal or already shuffled
		if ((mc.GetLeaf1()->GetViewCell() == mc.GetLeaf2()->GetViewCell()) ||
			(mc.GetLeaf1()->Mailed()) || (mc.GetLeaf2()->Mailed()))
			continue;
		
		// candidate for shuffling
		const bool wasShuffled = false;
			//ShuffleLeaves(mc.GetLeaf1(), mc.GetLeaf2());
			// matt: restore
		//-- stats
		if (wasShuffled)
			++ shuffled;
	}

	return shuffled;
}


inline int AddedPvsSize(ObjectPvs pvs1, const ObjectPvs &pvs2)
{
	return pvs1.AddPvs(pvs2);
}


inline int SubtractedPvsSize(ObjectPvs pvs1, const ObjectPvs &pvs2)
{
	return pvs1.SubtractPvs(pvs2);
}


float EvalShuffleCost(VspKdLeaf *leaf, VspKdViewCell *vc1, VspKdViewCell *vc2)
{
	const int pvs1 = SubtractedPvsSize(vc1->GetPvs(), *leaf->mPvs);
	const int pvs2 = AddedPvsSize(vc2->GetPvs(), *leaf->mPvs);

	const float vol1 = vc1->GetVolume() - leaf->mVolume;
	const float vol2 = vc2->GetVolume() + leaf->mVolume;

	const float cost1 = pvs1 * vol1;
	const float cost2 = pvs2 * vol2;

	return cost1 + cost2;
}


void VspKdTree::ShuffleLeaf(VspKdLeaf *leaf, 
							VspKdViewCell *vc1, 
							VspKdViewCell *vc2) const
{
	// compute new pvs and area
#if HAS_TO_BE_REDONE
	vc1->GetPvs().SubtractPvs(*leaf->mPvs);
	vc2->GetPvs().AddPvs(*leaf->mPvs);
	
	vc1->SetArea(vc1->GetVolume() - leaf->mVolume);
	vc2->SetArea(vc2->GetVolume() + leaf->mVolume);

	/// add to second view cell
	vc2->mLeaves.push_back(leaf);

	// erase leaf from old view cell
	vector<VspKdLeaf *>::iterator it = vc1->mLeaves.begin();

	for (; *it != leaf; ++ it);
	vc1->mLeaves.erase(it);

	leaf->SetViewCell(vc2);  // finally change view cell
#endif
}


bool VspKdTree::ShuffleLeaves(VspKdLeaf *leaf1, VspKdLeaf *leaf2) const
{
	VspKdViewCell *vc1 = leaf1->GetViewCell();
	VspKdViewCell *vc2 = leaf2->GetViewCell();

	const float cost1 = vc1->GetPvs().GetSize() * vc1->GetArea();
	const float cost2 = vc2->GetPvs().GetSize() * vc2->GetArea();

	const float oldCost = cost1 + cost2;
	
	float shuffledCost1 = Limits::Infinity;
	float shuffledCost2 = Limits::Infinity;

	// the view cell should not be empty after the shuffle
//todo
	/*	if (vc1->mLeaves.size() > 1)
		shuffledCost1 = EvalShuffleCost(leaf1, vc1, vc2);
	if (vc2->mLeaves.size() > 1)
		shuffledCost2 = EvalShuffleCost(leaf2, vc2, vc1);

*/	// shuffling unsuccessful
	if ((oldCost <= shuffledCost1) && (oldCost <= shuffledCost2))
		return false;
	
	if (shuffledCost1 < shuffledCost2)
	{
		ShuffleLeaf(leaf1, vc1, vc2);
		leaf1->Mail();
	}
	else
	{
		ShuffleLeaf(leaf2, vc2, vc1);
		leaf2->Mail();
	}

	return true;
}




/*********************************************************************/
/*                VspKdMergeCandidate implementation                      */
/*********************************************************************/


VspKdMergeCandidate::VspKdMergeCandidate(VspKdLeaf *l1, VspKdLeaf *l2):
mMergeCost(0),
mLeaf1(l1),
mLeaf2(l2),
mLeaf1Id(l1->GetViewCell()->mMailbox),
mLeaf2Id(l2->GetViewCell()->mMailbox)
{
	EvalMergeCost();
}

float VspKdMergeCandidate::GetLeaf1Cost() const
{
	VspKdViewCell *vc = mLeaf1->GetViewCell();
	return vc->GetPvs().GetSize() * vc->GetVolume();
}

float VspKdMergeCandidate::GetLeaf2Cost() const
{
	VspKdViewCell *vc = mLeaf2->GetViewCell();
	return vc->GetPvs().GetSize() * vc->GetVolume();
}

void VspKdMergeCandidate::EvalMergeCost()
{
	//-- compute pvs difference
	VspKdViewCell *vc1 = mLeaf1->GetViewCell();
	VspKdViewCell *vc2 = mLeaf2->GetViewCell();

	const int diff1 = vc1->GetPvs().Diff(vc2->GetPvs());
	const int vcPvs = diff1 + vc1->GetPvs().GetSize();

	//-- compute ratio of old cost
	//-- (added size of left and right view cell times pvs size)
	//-- to new rendering cost (size of merged view cell times pvs size)
	const float oldCost = GetLeaf1Cost() + GetLeaf2Cost();
	
	const float newCost =
		(float)vcPvs * (vc1->GetVolume() + vc2->GetVolume());

	mMergeCost = newCost - oldCost;

//	if (vcPvs > sMaxPvsSize) // strong penalty if pvs size too large
//		mMergeCost += 1.0;
}

void VspKdMergeCandidate::SetLeaf1(VspKdLeaf *l)
{
	mLeaf1 = l;
}

void VspKdMergeCandidate::SetLeaf2(VspKdLeaf *l)
{
	mLeaf2 = l;
}


VspKdLeaf *VspKdMergeCandidate::GetLeaf1()
{
	return mLeaf1;
}


VspKdLeaf *VspKdMergeCandidate::GetLeaf2()
{
	return mLeaf2;
}


bool VspKdMergeCandidate::Valid() const
{
	return
		(mLeaf1->GetViewCell()->mMailbox == mLeaf1Id) &&
		(mLeaf2->GetViewCell()->mMailbox == mLeaf2Id);
}


float VspKdMergeCandidate::GetMergeCost() const
{
	return mMergeCost;
}


void VspKdMergeCandidate::SetValid()
{
	mLeaf1Id = mLeaf1->GetViewCell()->mMailbox;
	mLeaf2Id = mLeaf2->GetViewCell()->mMailbox;

	EvalMergeCost();
}

}