// ================================================================
// $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"

// Static variables
int VspKdTreeLeaf::mailID = 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 VspKdTreeNode implementation          */
/**************************************************************/

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

VspKdTreeNode::~VspKdTreeNode() 
{};

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

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

bool VspKdTreeNode::IsLeaf() const 
{ 
	return mAxis == -1; 
}
  
int VspKdTreeNode::GetAccessTime() 
{
	return 0x7FFFFFF;
}

/**************************************************************/
/*                VspKdTreeInterior implementation            */
/**************************************************************/

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

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

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

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

int VspKdTreeInterior::Type() const  
{ 
	return EInterior; 
}
  
VspKdTreeInterior::~VspKdTreeInterior()
{
	DEL_PTR(mBack);
	DEL_PTR(mFront);
}
  
void VspKdTreeInterior::Print(ostream &s) const 
{
	if (mAxis == 0)
		s << "x ";
	else
		if (mAxis == 1)
			s << "y ";
		else
			s << "z ";
	s << mPosition << " ";

	mBack->Print(s);
	mFront->Print(s);
}
	
int VspKdTreeInterior::ComputeRayIntersection(const RayInfo &rayData, float &t) 
{
	return rayData.ComputeRayIntersection(mAxis, mPosition, t);
}

VspKdTreeNode *VspKdTreeInterior::GetBack() const
{
	return mBack;
}

VspKdTreeNode *VspKdTreeInterior::GetFront() const
{
	return mFront;
}


/*********************************************************/
/*            class VspKdTreeLeaf implementation         */
/*********************************************************/
VspKdTreeLeaf::VspKdTreeLeaf(VspKdTreeInterior *p,	const int nRays):
VspKdTreeNode(p), mRays(), mPvsSize(0), mValidPvs(false) 
{
	mRays.reserve(nRays);
}

VspKdTreeLeaf::~VspKdTreeLeaf() 
{}

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

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

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

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

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

void VspKdTreeLeaf::Mail()
{ 
	mMailbox = mailID; 
}

void VspKdTreeLeaf::NewMail() 
{ 
	++ mailID; 
}

bool VspKdTreeLeaf::Mailed() const 
{ 
	return mMailbox == mailID; 
}

bool VspKdTreeLeaf::Mailed(const int mail) 
{
	return mMailbox >= mailID + mail;
}

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

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

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


// Constructor
VspKdTree::VspKdTree()
{	  
	environment->GetIntValue("VspKdTree.Termination.maxDepth", mTermMaxDepth);
	environment->GetIntValue("VspKdTree.Termination.minPvs", mTermMinPvs);
	environment->GetIntValue("VspKdTree.Termination.minRays", mTermMinRays);
	environment->GetFloatValue("VspKdTree.Termination.maxRayContribution", mTermMaxRayContribution);
	environment->GetFloatValue("VspKdTree.Termination.maxCostRatio", mTermMaxCostRatio);

	environment->GetFloatValue("VspKdTree.Termination.minSize", mTermMinSize);
	mTermMinSize = sqr(mTermMinSize);

	environment->GetFloatValue("VspKdTree.epsilon", epsilon);
	environment->GetFloatValue("VspKdTree.ct_div_ci", ct_div_ci);
	
	environment->GetFloatValue("VspKdTree.maxTotalMemory", maxTotalMemory);
	environment->GetFloatValue("VspKdTree.maxStaticMemory", maxStaticMemory);
  
  
	environment->GetIntValue("VspKdTree.accessTimeThreshold", accessTimeThreshold);
	//= 1000;
	environment->GetIntValue("VspKdTree.minCollapseDepth", minCollapseDepth);
 

	// split type
	char sname[128];
	environment->GetStringValue("VspKdTree.splitType", sname);
	string name(sname);
	
	if (name.compare("regular") == 0)
		splitType = ESplitRegular;
	else
	{
		if (name.compare("heuristic") == 0)
			splitType = ESplitHeuristic;
		else 
		{
			cerr << "Invalid VspKdTree split type " << name << endl;
			exit(1);
		}
	}

	mRoot = NULL;

	splitCandidates = new vector<SortableEntry>;
}


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

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 dx dy dz \n";
  for (int i=0; i<7; 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 minCost )\n"<<
    minPvsNodes*100/(double)Leaves()<<endl;

  app << "#N_PMINRAYSLEAVES  ( Percentage of leaves with minCost )\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_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_CTIME  ( Construction time [s] )\n"
      << Time() << " \n";

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

}


void
VspKdTreeLeaf::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;
	}
}


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

	DEL_PTR(mRoot);

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

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

	
	mStat.nodes = 1;
  	
	mBox.Initialize();
	
	for (VssRayContainer::const_iterator ri = rays.begin(); ri != rays.end(); ++ ri)
	{
		leaf->AddRay(RayInfo(*ri));

		mBox.Include((*ri)->GetOrigin());
		mBox.Include((*ri)->GetTermination()); 
	}
	
	if (forcedBoundingBox)
		mBox = *forcedBoundingBox;

	cout << "Bbox = " << mBox << endl;
	
	mStat.rays = (int)leaf->mRays.size();
	leaf->UpdatePvsSize();
	
	mStat.initialPvsSize = leaf->GetPvsSize();
	
	// Subdivide();
	mRoot = Subdivide(TraversalData(leaf, mBox, 0));

	if (splitCandidates) 
	{
		// force realease of this vector
		delete splitCandidates;
		splitCandidates = new vector<SortableEntry>;
	}
  
	mStat.Stop();

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



VspKdTreeNode *VspKdTree::Subdivide(const TraversalData &tdata)
{
	VspKdTreeNode *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) 
		{
			cout << mem << " MB" << endl;
		}
		lastMem = (int)mem;
		
		if (  mem > maxMemory ) 
		{
			// count statistics on unprocessed leafs
			while (!tStack.empty()) 
			{
				EvaluateLeafStats(tStack.top());
				tStack.pop();
			}
			break;
		}
    
		TraversalData data = tStack.top();
		tStack.pop();
    
		
		VspKdTreeNode *node = SubdivideNode((VspKdTreeLeaf *) data.mNode,
											data.mBox, backBox,	frontBox);
		if (result == NULL)
			result = node;
    
		if (!node->IsLeaf()) 
		{
			VspKdTreeInterior *interior = (VspKdTreeInterior *) 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(VspKdTreeLeaf *leaf,
					   const AxisAlignedBox3 &box,
					   float &position,
					   int &raysBack,
					   int &raysFront,
					   int &pvsBack,
					   int &pvsFront)
{
	int minDirDepth = 6;
	int axis;
	float costRatio;
	
	if (splitType == ESplitRegular) {
		costRatio = BestCostRatioRegular(leaf,
										 axis,
										 position,
										 raysBack,
										 raysFront,
										 pvsBack,
										 pvsFront);

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

	if (costRatio > mTermMaxCostRatio) 
	{
		//		cout<<"Too big cost ratio "<<costRatio<<endl;
		return -1;
	}

#if 0	
	cout<<
		"pvs="<<leaf->mPvsSize<<
		" rays="<<leaf->mRays.size()<<
		" rc="<<leaf->GetAvgRayContribution()<<
		" axis="<<axis<<endl;
#endif
	
	return axis;
}


							

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

	float newCost;

	Intersectable::NewMail(3);
	
	// eval pvs size
	int pvsSize = leaf->GetPvsSize();

	Intersectable::NewMail(3);

	// 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
		int side = (*ri).ComputeRayIntersection(axis, position, (*ri).mRay->mT);
		//				(*ri).mRay->mSide = side;
			
		if (side <= 0)
			++ raysBack;
				
		if (side >= 0)
			++ raysFront;
				
		AddObject2Pvs((*ri).mRay->mTerminationObject, side, pvsBack, pvsFront);
	}	

	AxisAlignedBox3 box = GetBBox(leaf);
	
	float minBox = box.Min(axis);
	float maxBox = box.Max(axis);
	float sizeBox = maxBox - minBox;
		
	//		float sum = raysBack*(position - minBox) + raysFront*(maxBox - position);
	float sum = pvsBack*(position - minBox) + pvsFront*(maxBox - position);

	newCost = ct_div_ci + sum/sizeBox;
	
	//	cout<<axis<<" "<<pvsSize<<" "<<pvsBack<<" "<<pvsFront<<endl;
	//  float oldCost = leaf->mRays.size();
	float oldCost = (float)pvsSize;
	
	float ratio = newCost / oldCost;
	//	cout<<"ratio="<<ratio<<endl;
	return ratio;
}

float
VspKdTree::BestCostRatioRegular(VspKdTreeLeaf *leaf,
								int &axis,
								float &position,
								int &raysBack,
								int &raysFront,
								int &pvsBack,
								int &pvsFront)
{
	int nRaysBack[6], nRaysFront[6];
	int nPvsBack[6], nPvsFront[6];
	float nPosition[6];
	float nCostRatio[6];
	int bestAxis = -1;
	
	AxisAlignedBox3 sBox = GetBBox(leaf);

	// int sAxis = box.Size().DrivingAxis();
	int sAxis = sBox.Size().DrivingAxis();
	
	bool onlyDrivingAxis = true; 

	for (axis = 0; axis < 6; ++ axis) 
	{
		if (!onlyDrivingAxis || axis == sAxis) 
		{
			nPosition[axis] = (sBox.Min()[axis] + sBox.Max()[axis])*0.5f;
						
			nCostRatio[axis] = EvalCostRatio(leaf,
											 axis,
											 nPosition[axis],
											 nRaysBack[axis],
											 nRaysFront[axis],
											 nPvsBack[axis],
											 nPvsFront[axis]);
			
			if (bestAxis == -1)
				bestAxis = axis;
			else
				if (nCostRatio[axis] < nCostRatio[bestAxis])
					bestAxis = axis;
		}
	}

	axis = bestAxis;
	position = nPosition[bestAxis];

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

	pvsBack = nPvsBack[bestAxis];
	pvsFront = nPvsFront[bestAxis];
	
	return nCostRatio[bestAxis];
}

float VspKdTree::BestCostRatioHeuristic(VspKdTreeLeaf *leaf,
										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 = splitCandidates->begin();
		ci < splitCandidates->end(); ++ ci) 
	{
		VssRay *ray;
			
		switch ((*ci).type) 
		{
			case SortableEntry::ERayMin: 
				{
					++ rl;
					ray = (VssRay *) (*ci).data;
					Intersectable *object = ray->mTerminationObject;
					if (object && !object->Mailed()) 
					{
						object->Mail();
						++ pl;
					}
					break;
				}
			case SortableEntry::ERayMax: 
				{
					-- rr;
					
					ray = (VssRay *) (*ci).data;
					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 = ct_div_ci + minSum / sizeBox;
	float ratio = newCost / oldCost;
  
	//  cout<<"===================="<<endl;
	//  cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
	//      <<"\t q=("<<queriesBack<<","<<queriesFront<<")\t r=("<<raysBack<<","<<raysFront<<")"<<endl;
	
	return ratio;
}

void VspKdTree::SortSplitCandidates(VspKdTreeLeaf *node,
									const int axis)
{
	splitCandidates->clear();
  
	int requestedSize = 2 * (int)(node->mRays.size());
	// creates a sorted split candidates array
	if (splitCandidates->capacity() > 500000 &&
		requestedSize < (int)(splitCandidates->capacity()/10) ) 
	{
    	delete splitCandidates;
   		splitCandidates = new vector<SortableEntry>;
	}
  
	splitCandidates->reserve(requestedSize);

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


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

	if (data.mDepth >= mTermMaxDepth)
		++ mStat.maxDepthNodes;
  
	//  if ( (int)(leaf->mRays.size()) < termMinCost)
	//    stat.minCostNodes++;
	if (leaf->GetPvsSize() < mTermMinPvs)
		++ mStat.minPvsNodes;

	if (leaf->GetPvsSize() < mTermMinRays)
		++ mStat.minRaysNodes;

	if (0 && leaf->GetAvgRayContribution() > mTermMaxRayContribution)
		++ mStat.maxRayContribNodes;
	
	if (SqrMagnitude(data.mBox.Size()) <= mTermMinSize) 
		++ mStat.minSizeNodes;

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


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


VspKdTreeNode *VspKdTree::SubdivideNode(VspKdTreeLeaf *leaf,
										const AxisAlignedBox3 &box,
										AxisAlignedBox3 &backBBox,
										AxisAlignedBox3 &frontBBox)
{
	if (TerminationCriteriaMet(leaf, box))
	{

#if 0
		if (leaf->mDepth >= termMaxDepth) {
			cout<<"Warning: max depth reached depth="<<(int)leaf->mDepth<<" rays="<<leaf->mRays.size()<<endl;
			cout<<"Bbox: "<<GetBBox(leaf)<<" dirbbox:"<<GetDirBBox(leaf)<<endl;
		}
#endif
		
		return leaf;
	}
	
	float position;
	// first count ray sides
	int raysBack;
	int raysFront;
	int pvsBack;
	int pvsFront;
	
	// select subdivision axis
	int axis = SelectPlane( leaf, box, position, raysBack, raysFront, pvsBack, pvsFront);

	//	cout<<"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
	VspKdTreeInterior *node = new VspKdTreeInterior(leaf->mParent);

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

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

	// replace a link from node's parent
	if (leaf->mParent)
		leaf->mParent->ReplaceChildLink(leaf, node);
	// and setup child links
	node->SetupChildLinks(back, front);
	
	if (axis <= VspKdTreeNode::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();

				// determine the side of this ray with respect to the plane
				int side = node->ComputeRayIntersection(*ri, (*ri).mRay->mT);

				if (side == 0) 
				{
					if ((*ri).mRay->HasPosDir(axis)) 
					{
						back->AddRay(RayInfo((*ri).mRay,
											 (*ri).mMinT,
											 (*ri).mRay->mT));
						front->AddRay(RayInfo((*ri).mRay,
									 		  (*ri).mRay->mT,
											  (*ri).mMaxT));
					} 
					else 
					{
						back->AddRay(RayInfo((*ri).mRay,
											 (*ri).mRay->mT,
											 (*ri).mMaxT));
						front->AddRay(RayInfo((*ri).mRay,
									 		  (*ri).mMinT,
											  (*ri).mRay->mT));
					}
				} 
				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<VspKdTreeNode *> tstack;

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

	tstack.push(mRoot);

	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
    
		if (!node->IsLeaf()) 
		{
			VspKdTreeInterior *in = dynamic_cast<VspKdTreeInterior *>(node);
			// cout<<"depth="<<(int)in->depth<<" time="<<in->lastAccessTime<<endl;
			if (in->mDepth >= minCollapseDepth && 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;
}




VspKdTreeNode *VspKdTree::SubdivideLeaf(VspKdTreeLeaf *leaf,
										const float sizeThreshold)
{
	VspKdTreeNode *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 realese...
		if (GetMemUsage() > maxTotalMemory) 
			ReleaseMemory(pass);
		
		AxisAlignedBox3 backBBox, frontBBox;

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



void
VspKdTree::UpdateRays(VssRayContainer &remove,
					  VssRayContainer &add)
{
	VspKdTreeLeaf::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<VspKdTreeLeaf *> *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...
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)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
			VspKdTreeLeaf *leaf = dynamic_cast<VspKdTreeLeaf *>(data.mNode);
			leaf->AddRay(data.mRayData);
			++ mStat.addedRayRefs;
		}
	}
}

void VspKdTree::TraverseInternalNode(RayTraversalData &data, 
									 stack<RayTraversalData> &tstack)
{
	VspKdTreeInterior *in =  (VspKdTreeInterior *) data.mNode;
  
	if (in->mAxis <= VspKdTreeNode::SPLIT_Z) 
	{
	    // determine the side of this ray with respect to the plane
		int side = in->ComputeRayIntersection(data.mRayData, data.mRayData.mRay->mT);
    
      if (side == 0) 
	  {
		if (data.mRayData.mRay->HasPosDir(in->mAxis)) 
		{
			tstack.push(RayTraversalData(in->GetBack(), 
						RayInfo(data.mRayData.mRay, data.mRayData.mMinT, data.mRayData.mRay->mT)));
				
			tstack.push(RayTraversalData(in->GetFront(),
						RayInfo(data.mRayData.mRay, data.mRayData.mRay->mT, data.mRayData.mMaxT)));
	
		} 
		else 
		{
			tstack.push(RayTraversalData(in->GetBack(),
										 RayInfo(data.mRayData.mRay,
												 data.mRayData.mRay->mT,
												 data.mRayData.mMaxT)));
			tstack.push(RayTraversalData(in->GetFront(),
										 RayInfo(data.mRayData.mRay,
												 data.mRayData.mMinT,
												 data.mRayData.mRay->mT)));
		}
	  } 
	  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(VspKdTreeNode *sroot, const int time)
{
	// first count all rays in the subtree
	// use mail 1 for this purpose
	stack<VspKdTreeNode *> tstack;
	
	int rayCount = 0;
	int totalRayCount = 0;
	int collapsedNodes = 0;

#if DEBUG_COLLAPSE
	cout << "Collapsing subtree" << endl;
	cout << "acessTime=" << 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;
		
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *) 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(((VspKdTreeInterior *)node)->GetFront());
			tstack.push(((VspKdTreeInterior *)node)->GetBack());
		}
	}
  
	VssRay::NewMail();

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

	tstack.push(sroot);

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

		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = dynamic_cast<VspKdTreeLeaf *>(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 
		{
			VspKdTreeInterior *interior = 
				dynamic_cast<VspKdTreeInterior *>(node);
			tstack.push(interior->GetBack());
			tstack.push(interior->GetFront());
		}
	}
  
	// delete the node and all its children
	DEL_PTR(sroot);
  
	// for(VspKdTreeNode::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(VspKdTreeNode *node, const AxisAlignedBox3 &box) const
{
	stack<VspKdTreeNode *> tstack;
	tstack.push(mRoot);

	Intersectable::NewMail();
	int pvsSize = 0;
	
	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
    
	  if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)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 
		{
			VspKdTreeInterior *in = dynamic_cast<VspKdTreeInterior *>(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<VspKdTreeNode *> tstack;
	tstack.push(mRoot);
	
	minRayContribution = 1.0f;
	maxRayContribution = 0.0f;

	float sumRayContribution = 0.0f;
	int leaves = 0;

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

			if (c > maxRayContribution)
				maxRayContribution = c;
			if (c < minRayContribution)
				minRayContribution = c;
			sumRayContribution += c;
			
		} 
		else {
			VspKdTreeInterior *in = (VspKdTreeInterior *)node;
			// both nodes for directional splits
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}
	
	cout << "sum=" << sumRayContribution << endl;
	cout << "leaves=" << leaves << endl;
	avgRayContribution = sumRayContribution/(float)leaves;
}


int VspKdTree::GenerateRays(const float ratioPerLeaf,
							SimpleRayContainer &rays)
{
	stack<VspKdTreeNode *> tstack;
	tstack.push(mRoot);
	
	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
		
		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)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 
		{
			VspKdTreeInterior *in = 
				dynamic_cast<VspKdTreeInterior *>(node);
			// both nodes for directional splits
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}

	return (int)rays.size();
}


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

	int sumPvs = 0;
	int leaves = 0;
	
	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
		
		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)node;
			// update pvs size
			leaf->UpdatePvsSize();
			sumPvs += leaf->GetPvsSize();
			leaves++;
		} 
		else 
		{
			VspKdTreeInterior *in = (VspKdTreeInterior *)node;
			// both nodes for directional splits
			tstack.push(in->GetFront());
			tstack.push(in->GetBack());
		}
	}

	return sumPvs / (float)leaves;
}

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

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

	if (!node->IsLeaf())
		return (dynamic_cast<VspKdTreeInterior *>(node))->mBox;

	if (node->mParent->mAxis >= 3)
		return node->mParent->mBox;
    
	AxisAlignedBox3 box(node->mParent->mBox);
	if (node->mParent->mFront == node)
		box.SetMin(node->mParent->mAxis, node->mParent->mPosition);
	else
		box.SetMax(node->mParent->mAxis, node->mParent->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(VspKdTreeLeaf) +
		 mStat.Interior() * sizeof(VspKdTreeInterior) +
		 mStat.rayRefs * sizeof(RayInfo)) / (1024.0f * 1024.0f);
}
	
float VspKdTree::GetRayMemUsage() const 
{
	return mStat.rays * (sizeof(VssRay))/(1024.0f * 1024.0f);
}