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


#define DEBUG_DIR_SPLIT 0



// Static variables
int
VspKdTreeLeaf::mailID = 0;

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);
		}
	}
}


// Constructor
VspKdTree::VspKdTree()
{
	environment->GetIntValue("VspKdTree.maxDepth", termMaxDepth);
	environment->GetIntValue("VspKdTree.minPvs", termMinPvs);
	environment->GetIntValue("VspKdTree.minRays", termMinRays);
	environment->GetFloatValue("VspKdTree.maxRayContribution", termMaxRayContribution);
	environment->GetFloatValue("VspKdTree.maxCostRatio", termMaxCostRatio);

	environment->GetFloatValue("VspKdTree.minSize", termMinSize);
	termMinSize = sqr(termMinSize);
	
	environment->GetFloatValue("VspKdTree.refDirBoxMaxSize", refDirBoxMaxSize);
	refDirBoxMaxSize = sqr(refDirBoxMaxSize);
  
	environment->GetFloatValue("VspKdTree.epsilon", epsilon);
	environment->GetFloatValue("VspKdTree.ct_div_ci", ct_div_ci);
	
	environment->GetFloatValue("VspKdTree.maxTotalMemory", maxTotalMemory);
	environment->GetFloatValue("VspKdTree.maxStaticMemory", maxStaticMemory);
  
  
	float refDirAngle;
	environment->GetFloatValue("VspKdTree.refDirAngle", refDirAngle);

	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);
		}
	}

	environment->GetBoolValue("VspKdTree.randomize", randomize);

	root = NULL;

	splitCandidates = new vector<SortableEntry>;
}


VspKdTree::~VspKdTree()
{
	if (root)
		delete root;
}




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(VspKdTreeNode::RayInfoContainer::iterator ri = rays.begin(); 
			ri != rays.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)
{
	stat.Start();
  
	maxMemory = maxStaticMemory;

	if (root)
		delete root;

	root = new VspKdTreeLeaf(NULL, rays.size());

	// first construct a leaf that will get subdivide
	VspKdTreeLeaf *leaf = (VspKdTreeLeaf *) root;

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

		bbox.Include((*ri)->GetOrigin());
		bbox.Include((*ri)->GetTermination());    
		
		dirBBox.Include(Vector3((*ri)->GetDirParametrization(0), (*ri)->GetDirParametrization(1), 0));
	}
	
	if (forcedBoundingBox)
		bbox = *forcedBoundingBox;

	cout<<"Bbox = "<<bbox<<endl;
	cout<<"Dirr Bbox = "<<dirBBox<<endl;

	stat.rays = leaf->rays.size();
	leaf->UpdatePvsSize();
	
	stat.initialPvsSize = leaf->GetPvsSize();
	
	// Subdivide();
	root = Subdivide(TraversalData(leaf, bbox, 0));

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

	stat.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.node,
											data.bbox, backBox,	frontBox);
		if (result == NULL)
			result = node;
    
		if (!node->IsLeaf()) 
		{
			VspKdTreeInterior *interior = (VspKdTreeInterior *) node;

			// push the children on the stack
			tStack.push(TraversalData(interior->back, backBox, data.depth+1));
			tStack.push(TraversalData(interior->front, frontBox, data.depth+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 > termMaxCostRatio) 
	{
		//		cout<<"Too big cost ratio "<<costRatio<<endl;
		return -1;
	}

#if 0	
	cout<<
		"pvs="<<leaf->mPvsSize<<
		" rays="<<leaf->rays.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);

	if (axis <= VspKdTreeNode::SPLIT_Z) {
    // this is the main ray classification loop!
    for(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
		ri != leaf->rays.end();	ri++)
		if ((*ri).mRay->IsActive()) 
		{
			// 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;
	} 
	else 
	{
		// directional split
		for(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
			ri != leaf->rays.end();	++ ri)
			if ((*ri).mRay->IsActive()) 
			{
				// determine the side of this ray with respect to the plane
				int side;
				if ((*ri).mRay->GetDirParametrization(axis - 3) > position)
					side = 1;
				else
					side = -1;

				if (side <= 0)
					raysBack++;
				
				if (side >= 0)
					raysFront++;

				//				(*ri).mRay->mSide = side;
				AddObject2Pvs((*ri).mRay->mTerminationObject, side, pvsBack, pvsFront);
			}
	
			AxisAlignedBox3 box = GetDirBBox(leaf);
		
			float minBox = box.Min(axis - 3);
			float maxBox = box.Max(axis - 3);
			float sizeBox = maxBox - minBox;
		
			// float sum = raysBack*(position - minBox) + raysFront*(maxBox - position);
			float sum =	pvsBack*(position - minBox) + pvsFront*(maxBox - position);
		//		float sum = pvsBack + pvsFront;
		newCost = ct_div_ci + sum/sizeBox;
	}

	//	cout<<axis<<" "<<pvsSize<<" "<<pvsBack<<" "<<pvsFront<<endl;
	//  float oldCost = leaf->rays.size();
	float oldCost = 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);
	AxisAlignedBox3 dBox = GetDirBBox(leaf);

	// int sAxis = box.Size().DrivingAxis();
	int sAxis = sBox.Size().DrivingAxis();
	int dAxis = dBox.Size().DrivingAxis() + 3;

	bool onlyDrivingAxis = true; 

	for (axis = 0; axis < 6; ++ axis) 
	{
		if (!onlyDrivingAxis || axis == sAxis || axis == dAxis) 
		{
			if (axis < 3)
				nPosition[axis] = (sBox.Min()[axis] + sBox.Max()[axis])*0.5f;
			else
				nPosition[axis] = (dBox.Min()[axis-3] + dBox.Max()[axis-3])*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 = leaf->rays.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.1*(maxBox - minBox);
	float maxBand = minBox + 0.9*(maxBox - minBox);
	
	float sum = rr*sizeBox;
	float minSum = 1e20;
	
	Intersectable::NewMail();
	// set all object as belonging to the fron pvs
	for(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
		ri != leaf->rays.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 = 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*(node->rays.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(VspKdTreeNode::RayInfoContainer::const_iterator ri = node->rays.begin();
		ri < node->rays.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 = (VspKdTreeLeaf *)data.node;

	if (data.depth >= termMaxDepth)
		++ stat.maxDepthNodes;
  
	//  if ( (int)(leaf->rays.size()) < termMinCost)
	//    stat.minCostNodes++;
	if (leaf->GetPvsSize() < termMinPvs)
		++ stat.minPvsNodes;

	if (leaf->GetPvsSize() < termMinRays)
		++ stat.minRaysNodes;

	if (0 && leaf->GetAvgRayContribution() > termMaxRayContribution)
		++ stat.maxRayContribNodes;
	
	if (SqrMagnitude(data.bbox.Size()) <= termMinSize) 
		++ stat.minSizeNodes;

	if ((int)(leaf->rays.size()) > stat.maxRayRefs)
		stat.maxRayRefs = leaf->rays.size();
}



VspKdTreeNode *VspKdTree::SubdivideNode(VspKdTreeLeaf *leaf,
										const AxisAlignedBox3 &box,
										AxisAlignedBox3 &backBBox,
										AxisAlignedBox3 &frontBBox)
{
	if ( (leaf->GetPvsSize() < termMinPvs) || (leaf->rays.size() < termMinRays) ||
	// (leaf->GetAvgRayContribution() > termMaxRayContribution ) ||
       (leaf->depth >= termMaxDepth) || SqrMagnitude(box.Size()) <= termMinSize ) 
	{

#if 0
		if (leaf->depth >= termMaxDepth) {
			cout<<"Warning: max depth reached depth="<<(int)leaf->depth<<" rays="<<leaf->rays.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;
	}
  
	stat.nodes += 2;
	++ stat.splits[axis];

	// add the new nodes to the tree
	VspKdTreeInterior *node = new VspKdTreeInterior(leaf->parent);

	node->axis = axis;
	node->position = position;
	node->bbox = box;
	node->dirBBox = GetDirBBox(leaf);
  
	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->parent)
		leaf->parent->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(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
				ri != leaf->rays.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(VspKdTreeNode::RayInfo((*ri).mRay,
															(*ri).mMinT,
															(*ri).mRay->mT));
						front->AddRay(VspKdTreeNode::RayInfo((*ri).mRay,
															 (*ri).mRay->mT,
															 (*ri).mMaxT));
					} 
					else 
					{
						back->AddRay(VspKdTreeNode::RayInfo((*ri).mRay,
															(*ri).mRay->mT,
															(*ri).mMaxT));
						front->AddRay(VspKdTreeNode::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(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
			ri != leaf->rays.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
	stat.rayRefs -= leaf->rays.size();
	stat.rayRefs += raysBack + raysFront;

	delete 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;

	tstack.push(root);

	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
    
		if (!node->IsLeaf()) 
		{
			VspKdTreeInterior *in = (VspKdTreeInterior *)node;
			// cout<<"depth="<<(int)in->depth<<" time="<<in->lastAccessTime<<endl;
			if (in->depth >= minCollapseDepth && in->lastAccessTime <= maxAccessTime) 
			{
				released = CollapseSubtree(node, time);
				break;
			}
			if (in->back->GetAccessTime() <  in->front->GetAccessTime()) 
			{
				tstack.push(in->front);
				tstack.push(in->back);
			} 
			else 
			{
				tstack.push(in->back);
				tstack.push(in->front);
			}
		}
	}

	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->rays.size() > (unsigned)termMinCost &&
		(leaf->GetPvsSize() >= termMinPvs) && 
		(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(root, VspKdTreeNode::RayInfo(ray)));
  
	RayTraversalData data;

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

		if (!data.node->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.node;
      
			if (!leaf->Mailed()) 
			{
				leaf->Mail();
				
				if (affectedLeaves)
					affectedLeaves->push_back(leaf);
	
				if (removeAllScheduledRays) 
				{
					int tail = leaf->rays.size() - 1;

					for (int i=0; i < (int)(leaf->rays.size()); ++ i) 
					{
						if (leaf->rays[i].mRay->ScheduledForRemoval()) 
						{
							// find a ray to replace it with
							while (tail >= i && leaf->rays[tail].mRay->ScheduledForRemoval()) 
							{
								++ stat.removedRayRefs;
								leaf->rays[tail].mRay->Unref();
								leaf->rays.pop_back();
								
								-- tail;
							}
							
							if (tail < i)
								break;
	      
							++ stat.removedRayRefs;
	      
							leaf->rays[i].mRay->Unref();
							leaf->rays[i] = leaf->rays[tail];
							leaf->rays.pop_back();
							-- tail;
						}
					}
				}
			}
      
			if (!removeAllScheduledRays)
				for (int i=0; i < (int)leaf->rays.size(); i++) 
				{
					if (leaf->rays[i].mRay == ray) 
					{
						++ stat.removedRayRefs;
						ray->Unref();
						leaf->rays[i] = leaf->rays[leaf->rays.size() - 1];
						leaf->rays.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(root, VspKdTreeNode::RayInfo(ray)));
  
	RayTraversalData data;
  
	while (!tstack.empty()) 
	{
		data = tstack.top();
		tstack.pop();

		if (!data.node->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 = (VspKdTreeLeaf *)data.node;
			leaf->AddRay(data.rayData);
			++ stat.addedRayRefs;
		}
	}
}

void VspKdTree::TraverseInternalNode(RayTraversalData &data, 
									 stack<RayTraversalData> &tstack)
{
	VspKdTreeInterior *in =  (VspKdTreeInterior *) data.node;
  
	if (in->axis <= VspKdTreeNode::SPLIT_Z) 
	{
	    // determine the side of this ray with respect to the plane
		int side = in->ComputeRayIntersection(data.rayData, data.rayData.mRay->mT);
    
      if (side == 0) 
	  {
		if (data.rayData.mRay->HasPosDir(in->axis)) 
		{
			tstack.push(RayTraversalData(in->back, 
						VspKdTreeNode::RayInfo(data.rayData.mRay, data.rayData.mMinT, 
											   data.rayData.mRay->mT)));
				
			tstack.push(RayTraversalData(in->front,
						VspKdTreeNode::RayInfo(data.rayData.mRay, data.rayData.mRay->mT, 
											   data.rayData.mMaxT)));
	
		} 
		else 
		{
			tstack.push(RayTraversalData(in->back,
										 VspKdTreeNode::RayInfo(data.rayData.mRay,
																data.rayData.mRay->mT,
																data.rayData.mMaxT)));
			tstack.push(RayTraversalData(in->front,
										 VspKdTreeNode::RayInfo(data.rayData.mRay,
																data.rayData.mMinT,
																data.rayData.mRay->mT)));
		}
	  } 
	  else
     	  if (side == 1)
			  tstack.push(RayTraversalData(in->front, data.rayData));
		  else
			  tstack.push(RayTraversalData(in->back, data.rayData));
	} 
	else 
	{
		// directional split
		if (data.rayData.mRay->GetDirParametrization(in->axis - 3) > in->position)
			tstack.push(RayTraversalData(in->front, data.rayData));
		else
			tstack.push(RayTraversalData(in->back, data.rayData));
	}
}


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(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
					ri != leaf->rays.end(); ++ ri) 
			{
				++ totalRayCount;
				
				if ((*ri).mRay->IsActive() && !(*ri).mRay->Mailed()) 
				{
					(*ri).mRay->Mail();
					++ rayCount;
				}
			}
		} 
		else 
		{
			tstack.push(((VspKdTreeInterior *)node)->back);
			tstack.push(((VspKdTreeInterior *)node)->front);
		}
	}
  
	VssRay::NewMail();

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

	tstack.push(sroot);

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

		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *) node;

			for(VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
					ri != leaf->rays.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 
		{
			tstack.push(((VspKdTreeInterior *)node)->back);
			tstack.push(((VspKdTreeInterior *)node)->front);
		}
	}
  
	// delete the node and all its children
	delete sroot;
  
	// for(VspKdTreeNode::SRayContainer::iterator ri = newLeaf->rays.begin();
    //      ri != newLeaf->rays.end(); ++ ri)
	//     (*ri).ray->UnMail(2);

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

	stat.nodes -= collapsedNodes - 1;
	stat.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(root);

	Intersectable::NewMail();
	int pvsSize = 0;
	
	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
    
	  if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)node;
			for (VspKdTreeNode::RayInfoContainer::iterator ri = leaf->rays.begin();
				ri != leaf->rays.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 = (VspKdTreeInterior *)node;
	
			if (in->axis < 3) 
			{
				if (box.Max(in->axis) >= in->position)
					tstack.push(in->front);
				
				if (box.Min(in->axis) <= in->position)
					tstack.push(in->back);
			} 
			else 
			{
				// both nodes for directional splits
				tstack.push(in->front);
				tstack.push(in->back);
			}
		}
	}

	return pvsSize;
}

void VspKdTree::GetRayContributionStatistics(float &minRayContribution,
											 float &maxRayContribution,
											 float &avgRayContribution)
{
	stack<VspKdTreeNode *> tstack;
	tstack.push(root);
	
	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->front);
			tstack.push(in->back);
		}
	}
	
	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(root);
	
	while (!tstack.empty()) 
	{
		VspKdTreeNode *node = tstack.top();
		tstack.pop();
		
		if (node->IsLeaf()) 
		{
			VspKdTreeLeaf *leaf = (VspKdTreeLeaf *)node;
			float c = leaf->GetAvgRayContribution();
			int num = (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 = (VspKdTreeInterior *)node;
			// both nodes for directional splits
			tstack.push(in->front);
			tstack.push(in->back);
		}
	}

	return rays.size();
}


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

	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->front);
			tstack.push(in->back);
		}
	}

	return sumPvs / (float)leaves;
}