#include "Plane3.h"
#include "ViewCellBsp.h"
#include "Mesh.h"
#include "common.h"
#include "ViewCell.h"
#include "Environment.h"
#include "Polygon3.h"
#include "Ray.h"
#include "AxisAlignedBox3.h"
#include <stack>
#include <time.h>
#include <iomanip>
#include "Exporter.h"

int BspTree::sTermMaxPolygons = 10;
int BspTree::sTermMaxDepth = 20;
int BspTree::sMaxCandidates = 10;
int BspTree::sSplitPlaneStrategy = NEXT_POLYGON; 
int BspTree::sConstructionMethod = FROM_INPUT_VIEW_CELLS;
int BspTree::sTermMaxPolysForAxisAligned = 50;

float BspTree::sCt_div_ci = 1.0f;
float BspTree::sSplitBorder = 1.0f;
float BspTree::sMaxCostRatio = 0.9f;

//-- factors for bsp tree split plane heuristics
float BspTree::sLeastSplitsFactor = 1.0f;
float BspTree::sBalancedPolysFactor = 2.0f;
float BspTree::sBalancedViewCellsFactor = 2.0f;
float BspTree::sVerticalSplitsFactor = 1.0f; // very important criterium for 2.5d scenes
float BspTree::sLargestPolyAreaFactor = 1.0f;

/** Evaluates split plane classification with respect to the plane's 
	contribution for a balanced tree.
*/
float BspTree::sLeastSplitsTable[] = {0, 0, 1, 0};
/** Evaluates split plane classification with respect to the plane's 
	contribution for a minimum number splits in the tree.
*/
float BspTree::sBalancedPolysTable[] = {-1, 1, 0, 0};

//int counter = 0;

/****************************************************************/
/*                  class BspNode implementation                */
/****************************************************************/

BspNode::BspNode(): mParent(NULL), mPolygons(NULL)//,mViewCellIdx(0)
{}

BspNode::BspNode(BspInterior *parent): mParent(parent), mPolygons(NULL)//,mViewCellIdx(0)
{}

BspNode::~BspNode()
{
	if (mPolygons)
	{
		CLEAR_CONTAINER(*mPolygons);
		DEL_PTR(mPolygons);
	}
}

bool BspNode::IsRoot() const 
{
	return mParent == NULL;
}

BspInterior *BspNode::GetParent() 
{ 
	return mParent; 
}

void BspNode::SetParent(BspInterior *parent)
{
	mParent = parent;
}

PolygonContainer *BspNode::GetPolygons()
{
	if (!mPolygons)
		mPolygons = new PolygonContainer();

	return mPolygons;
}

void BspNode::ProcessPolygons(PolygonContainer *polys, bool storePolys)
{
	if (storePolys)
	{
		while (!polys->empty())
		{
			GetPolygons()->push_back(polys->back());
			polys->pop_back();
		}
	}
	else CLEAR_CONTAINER(*polys);
}


/****************************************************************/
/*              class BspInterior implementation                */
/****************************************************************/


BspInterior::BspInterior(const Plane3 &plane): 
mPlane(plane), mFront(NULL), mBack(NULL)
{}

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

BspNode *BspInterior::GetBack() 
{
	return mBack;
}

BspNode *BspInterior::GetFront() 
{
	return mFront;
}

Plane3 *BspInterior::GetPlane()
{
	return &mPlane;
}

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

void BspInterior::SetupChildLinks(BspNode *b, BspNode *f) 
{
    mBack = b;
    mFront = f;
}

void BspInterior::ProcessPolygon(Polygon3 **poly, const bool storePolys)
{
	if (storePolys) 
		GetPolygons()->push_back(*poly);
	else
		DEL_PTR(*poly);
}

void BspInterior::SplitPolygons(PolygonContainer *polys, 
								PolygonContainer *frontPolys, 
								PolygonContainer *backPolys, 
								PolygonContainer *coincident,
								int &splits, 
								bool storePolys)
{
	Polygon3 *splitPoly = NULL;
#ifdef _Debug
	Debug << "splitting polygons of node " << this << " with plane " << mPlane << endl;
#endif
	while (!polys->empty())
	{
		Polygon3 *poly = polys->back();
		polys->pop_back();

		//Debug << "New polygon with plane: " << poly->GetSupportingPlane() << "\n";

		// get polygon classification
		int classification = poly->ClassifyPlane(mPlane);

		Polygon3 *front_piece = NULL;
		Polygon3 *back_piece = NULL;
	
		VertexContainer splitVertices;

		switch (classification)
		{
			case Polygon3::COINCIDENT:
				//Debug << "coincident" << endl;	
				coincident->push_back(poly);
				break;			
			case Polygon3::FRONT_SIDE:	
				//Debug << "front" << endl;
				frontPolys->push_back(poly);
				break;
			case Polygon3::BACK_SIDE:
				//Debug << "back" << endl;
				backPolys->push_back(poly);
				break;
			case Polygon3::SPLIT:
				front_piece = new Polygon3(poly->mParent);
				back_piece = new Polygon3(poly->mParent);

				//-- split polygon into front and back part
				poly->Split(mPlane, *front_piece, *back_piece, splitVertices);

				++ splits; // increase number of splits

				// check if polygons still valid 
				if (front_piece->Valid())
					frontPolys->push_back(front_piece);
				else
					DEL_PTR(front_piece);
				
				if (back_piece->Valid())
					backPolys->push_back(back_piece);
				else				
					DEL_PTR(back_piece);
				
#ifdef _DEBUG
				Debug << "split " << *poly << endl << *front_piece << endl << *back_piece << endl;
#endif
				ProcessPolygon(&poly, storePolys);
				
				break;
			default:
                Debug << "SHOULD NEVER COME HERE\n";
				break;
		}
	}
}

/****************************************************************/
/*                  class BspLeaf implementation                */
/****************************************************************/
BspLeaf::BspLeaf(): mViewCell(NULL)
{
}

BspLeaf::BspLeaf(ViewCell *viewCell): mViewCell(viewCell)
{
}

BspLeaf::BspLeaf(BspInterior *parent): BspNode(parent), mViewCell(NULL)
{}

BspLeaf::BspLeaf(BspInterior *parent, ViewCell *viewCell): 
BspNode(parent), mViewCell(viewCell)
{
}
ViewCell *BspLeaf::GetViewCell()
{
	return mViewCell;
}

void BspLeaf::SetViewCell(ViewCell *viewCell)
{
	mViewCell = viewCell;
}

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

/****************************************************************/
/*                  class BspTree implementation                */
/****************************************************************/

BspTree::BspTree(ViewCell *viewCell): 
mRoot(NULL), 
mViewCell(viewCell),
//mStoreSplitPolys(true)
mStoreSplitPolys(false)
{
	Randomize(); // initialise random generator for heuristics
}
 
const BspTreeStatistics &BspTree::GetStatistics() const 
{
	return mStat;
}

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

	app << setprecision(4);

	app << "#N_RAYS Number of rays )\n"
		<< rays << endl;
	app << "#N_DOMAINS  ( Number of query domains )\n"
		<< queryDomains <<endl;

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

	app << "#N_INTERIORS ( Number of interior nodes )\n" << Interior() << "\n";

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

	app << "#N_SPLITS ( Number of splits )\n" << splits << "\n";

	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_MAXOBJECTREFS  ( Max number of rayRefs / leaf )\n" <<
	maxObjectRefs << "\n";

	app << "#N_NONEMPTYRAYREFS  ( Number of rayRefs in nonEmpty leaves / non empty leaf )\n" <<
	rayRefsNonZeroQuery/(double)(Leaves() - zeroQueryNodes) << "\n";

	app << "#N_LEAFDOMAINREFS  ( Number of query domain Refs / leaf )\n" <<
	objectRefs/(double)Leaves() << "\n";

	app << "#N_PEMPTYLEAVES  ( Percentage of leaves with zero query domains )\n"<<
	zeroQueryNodes*100/(double)Leaves() << endl;

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

	app << "#N_PMAXDEPTH ( Maximal reached depth )\n" << maxDepth << endl;

	app << "#N_PMINDEPTH ( Minimal reached depth )\n" << minDepth << endl;

	app << "#AVGDEPTH ( average depth )\n" << AvgDepth() << 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 << "#N_INPUT_POLYGONS (number of input polygons )\n" <<
		polys << endl;

	app << "#N_ROUTPUT_INPUT_POLYGONS ( ratio polygons after subdivision / input polygons )\n" <<
		 (polys + splits) / (double)polys << endl;
	
	app << "===== END OF BspTree statistics ==========\n";
}

BspTree::~BspTree()
{
	std::stack<BspNode *> tStack;

	tStack.push(mRoot);

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

	    tStack.pop();
	
		if (!node->IsLeaf())
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);

			// push the children on the stack (there are always two children)
			tStack.push(interior->GetBack());
			tStack.push(interior->GetFront());
		}

		DEL_PTR(node);
	}
}

void BspTree::InsertViewCell(ViewCell *viewCell)
{
	PolygonContainer *polys = new PolygonContainer();

	// extract polygons that guide the split process
	mStat.polys += AddMeshToPolygons(viewCell->GetMesh(), *polys, viewCell);
	mBox.Include(viewCell->GetBox()); // add to BSP aabb

	InsertPolygons(polys);
}

void BspTree::InsertPolygons(PolygonContainer *polys, ViewCellContainer *viewCells)
{	
	std::stack<BspTraversalData> tStack;
		
	// traverse tree or create new one
	BspNode *firstNode = mRoot ? mRoot : new BspLeaf();

	tStack.push(BspTraversalData(firstNode, polys, 0, mViewCell));

	while (!tStack.empty())
	{
		// filter polygons donw the tree
		BspTraversalData tData = tStack.top();
	    tStack.pop();
			
		if (!tData.mNode->IsLeaf())
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(tData.mNode);

			//-- filter view cell polygons down the tree until a leaf is reached
			if ((int)tData.mPolygons->size() > 0)
			{
				PolygonContainer *frontPolys = new PolygonContainer();
				PolygonContainer *backPolys = new PolygonContainer();
				PolygonContainer coincident;

				int splits = 0;
		
				// split viewcell polygons with respect to split plane
				interior->SplitPolygons(tData.mPolygons, 
										frontPolys, 
										backPolys,
										&coincident,
										splits, 
										mStoreSplitPolys);
				
				// extract view cells associated with the split polygons
				ViewCell *frontViewCell = mViewCell;
				ViewCell *backViewCell = mViewCell;
	
				if (!viewCells)
				{
					ExtractViewCells(&backViewCell,
									 &frontViewCell,
									 coincident, 
									 interior->mPlane, 
									 frontPolys->empty(), 
									 backPolys->empty());
				}

				// don't need coincident polygons anymore
				interior->ProcessPolygons(&coincident, mStoreSplitPolys);

				mStat.splits += splits;

				// push the children on the stack
				tStack.push(BspTraversalData(interior->GetFront(), frontPolys, tData.mDepth + 1, frontViewCell));
				tStack.push(BspTraversalData(interior->GetBack(), backPolys, tData.mDepth + 1, backViewCell));
			}

			// cleanup
			DEL_PTR(tData.mPolygons);
		}
		else // reached leaf => subdivide current viewcell
		{
			//if (tData.mPolygons->size() > 0) Debug << "Subdividing further: polygon size: " << (int)tData.mPolygons->size() << ", view cell: " << tData.mViewCell << endl;
			BspNode *root = Subdivide(tStack, tData);		

			if (viewCells && root->IsLeaf())
			{	// generate new view cell for each leaf
				ViewCell *viewCell = new ViewCell();
				viewCells->push_back(viewCell);
				
				dynamic_cast<BspLeaf *>(root)->SetViewCell(viewCell);
			}

			if (!mRoot) // tree empty => new root
				mRoot = root;
		}
	}
}

int BspTree::AddMeshToPolygons(Mesh *mesh, PolygonContainer &polys, MeshInstance *parent)
{
	FaceContainer::const_iterator fi;
	int polysNum = 0;

	// copy the face data to polygons
	for (fi = mesh->mFaces.begin(); fi != mesh->mFaces.end(); ++ fi)
	{
		Polygon3 *poly = new Polygon3((*fi), mesh);
		poly->mParent = parent; // set parent intersectable
		polys.push_back(poly);

		//if (!poly->Valid())
		//	Debug << "Input polygon not valid: " << *poly << endl << "Area: " << poly->GetArea() << endl;
		
		++ polysNum;
	}
	return polysNum;
}

int BspTree::Copy2PolygonSoup(const ViewCellContainer &viewCells, PolygonContainer &polys, int maxObjects)
{
	int limit = (maxObjects > 0) ? Min((int)viewCells.size(), maxObjects) : (int)viewCells.size();
  
	for (int i = 0; i < limit; ++i)
	{//if ((i == 12) ||(i==14))
		if (viewCells[i]->GetMesh()) // copy the mesh data to polygons
		{
			mBox.Include(viewCells[i]->GetBox()); // add to BSP tree aabb
			AddMeshToPolygons(viewCells[i]->GetMesh(), polys, viewCells[i]);
		}
	}

	return (int)polys.size();
}

int BspTree::Copy2PolygonSoup(const ObjectContainer &objects, PolygonContainer &polys, int maxObjects)
{
	int limit = (maxObjects > 0) ? Min((int)objects.size(), maxObjects) : (int)objects.size();
  
	for (int i = 0; i < limit; ++i)
	{
		Intersectable *object = objects[i];//*it;
		Mesh *mesh = NULL;

		switch (object->Type()) // extract the meshes
		{
		case Intersectable::MESH_INSTANCE:
			mesh = dynamic_cast<MeshInstance *>(object)->GetMesh();
			break;
		case Intersectable::VIEW_CELL:
			mesh = dynamic_cast<ViewCell *>(object)->GetMesh();
			break;
			// TODO: handle transformed mesh instances
		default:
			Debug << "intersectable type not supported" << endl;
			break;
		}
		
        if (mesh) // copy the mesh data to polygons
		{
			mBox.Include(object->GetBox()); // add to BSP tree aabb
			AddMeshToPolygons(mesh, polys, mViewCell);
		}
	}

	return (int)polys.size();
}

void BspTree::Construct(const ViewCellContainer &viewCells)
{
	mStat.nodes = 1;
	mBox.Initialize(); 	// initialise bsp tree bounding box

 	// copy view cell meshes into one big polygon soup
	PolygonContainer *polys = new PolygonContainer();
	mStat.polys = Copy2PolygonSoup(viewCells, *polys);

	// construct tree from viewcell polygons
	Construct(polys);
}


void BspTree::Construct(const ObjectContainer &objects, ViewCellContainer *viewCells)
{
	mStat.nodes = 1;
	mBox.Initialize(); 	// initialise bsp tree bounding box
	
	PolygonContainer *polys = new PolygonContainer();
	
	// copy mesh instance polygons into one big polygon soup
	mStat.polys = Copy2PolygonSoup(objects, *polys);

	// construct tree from polygon soup
	Construct(polys, viewCells);
}

void BspTree::Construct(const RayContainer &rays, ViewCellContainer *viewCells)
{
	// TODO
}

void BspTree::Construct(PolygonContainer *polys, ViewCellContainer *viewCells)
{
	std::stack<BspTraversalData> tStack;
	
	BspTraversalData tData(new BspLeaf(), polys, 0, mViewCell);
	tStack.push(tData);

	long startTime = GetTime();
	Debug << "**** Contructing tree using objects ****\n";
	while (!tStack.empty()) 
	{
		tData = tStack.top();
	    tStack.pop();

		// subdivide leaf node
		BspNode *root = Subdivide(tStack, tData);

		if (viewCells && root->IsLeaf())
		{	// generate new view cell for each leaf
			ViewCell *viewCell = new ViewCell();
			viewCells->push_back(viewCell);

			dynamic_cast<BspLeaf *>(root)->SetViewCell(viewCell);
		}

		// empty tree => new root corresponding to unbounded space
		if (!mRoot) 
			mRoot = root;
	}

	Debug << "**** Finished tree construction ****\n";
	Debug << "BSP tree contruction time: " << TimeDiff(startTime, GetTime())*1e-3 << "s" << endl;
}


BspNode *BspTree::Subdivide(BspTraversalStack &tStack, BspTraversalData &tData)
{
	//-- terminate traversal  
	if ((tData.mPolygons->size() <= sTermMaxPolygons) || (tData.mDepth >= sTermMaxDepth))
	{
#ifdef _DEBUG
		Debug << "subdivision terminated at depth " << tData.mDepth << ", #polys: " 
			  << (int)tData.mPolygons->size() << endl;
#endif

		EvaluateLeafStats(tData);

		BspLeaf *leaf = dynamic_cast<BspLeaf *>(tData.mNode);
		
		// add view cell to leaf
		if (!leaf->GetViewCell())
			leaf->SetViewCell(tData.mViewCell);

		// remaining polygons are discarded or added to node
		leaf->ProcessPolygons(tData.mPolygons, mStoreSplitPolys);
		DEL_PTR(tData.mPolygons);

		return leaf;
	}

	//-- continue subdivision
	PolygonContainer *backPolys = new PolygonContainer();
	PolygonContainer *frontPolys = new PolygonContainer();
	PolygonContainer coincident;
	
	// create new interior node and two leaf nodes
	BspInterior *interior = SubdivideNode(dynamic_cast<BspLeaf *>(tData.mNode),
										  tData.mPolygons,
										  frontPolys,
										  backPolys, 
										  &coincident);

	ViewCell *frontViewCell = mViewCell;
	ViewCell *backViewCell = mViewCell;

#ifdef _DEBUG	
	if (frontPolys->empty() && backPolys->empty() && (coincident.size() > 2))
	{	for (PolygonContainer::iterator it = coincident.begin(); it != coincident.end(); ++it)
			Debug << (*it) << " " << (*it)->GetArea() << " " << (*it)->mParent << endl ;
		Debug << endl;}
#endif

	//-- extract view cells from coincident polygons according to plane normal
   	ExtractViewCells(&backViewCell, &frontViewCell, coincident, interior->mPlane, 
						 frontPolys->empty(), backPolys->empty());
	
	// don't need coincident polygons anymore
	interior->ProcessPolygons(&coincident, mStoreSplitPolys);

	// push the children on the stack
	// split polygon is only needed in the back leaf
	tStack.push(BspTraversalData(interior->GetFront(), frontPolys, tData.mDepth + 1, frontViewCell));
	tStack.push(BspTraversalData(interior->GetBack(), backPolys, tData.mDepth + 1, backViewCell));

	// cleanup
	DEL_PTR(tData.mNode);
	DEL_PTR(tData.mPolygons);

	return interior;
}

void BspTree::ExtractViewCells(ViewCell **backViewCell, 
							   ViewCell **frontViewCell,
							   const PolygonContainer &coincident,
							   const Plane3 splitPlane, 
							   const bool extractFront, 
							   const bool extractBack) const
{
	bool foundFront = !extractFront, foundBack = !extractBack;

	PolygonContainer::const_iterator it, it_end = coincident.end();

	//-- find first view cells in front and back leafs
	for (it = coincident.begin(); !(foundFront && foundBack) && 
		(it != it_end); ++ it)
	{
		if (DotProd((*it)->GetSupportingPlane().mNormal, splitPlane.mNormal > 0))
		{
			*backViewCell = dynamic_cast<ViewCell *>((*it)->mParent);
			foundBack = true;
		}
		else
		{
			*frontViewCell = dynamic_cast<ViewCell *>((*it)->mParent);
			foundFront = true;
		}
		//if (foundFront) Debug << "front viewCell: " << *frontViewCell << endl;
		//if (foundBack) Debug << "back viewCell: " << *backViewCell << endl;
	}
}

BspInterior *BspTree::SubdivideNode(BspLeaf *leaf, 
									PolygonContainer *polys, 
									PolygonContainer *frontPolys,
									PolygonContainer *backPolys, 
									PolygonContainer *coincident)
{
	mStat.nodes += 2;

	// add the new nodes to the tree + select subdivision plane
	BspInterior *interior = new BspInterior(SelectPlane(leaf, *polys)); 

#ifdef _DEBUG
	Debug << interior << endl;
#endif

	// split polygon according to current plane
	int splits = 0;
	
	interior->SplitPolygons(polys, frontPolys, backPolys, coincident, splits, mStoreSplitPolys);
	
 	mStat.splits += splits;

	BspInterior *parent = leaf->GetParent();

	// replace a link from node's parent
	if (parent)
	{
		parent->ReplaceChildLink(leaf, interior);
		interior->SetParent(parent);
	}

	// and setup child links
	interior->SetupChildLinks(new BspLeaf(interior), new BspLeaf(interior));
	
	return interior;
}

void BspTree::SortSplitCandidates(const PolygonContainer &polys, 
								  const int axis, 
								  vector<SortableEntry> &splitCandidates) const
{
  splitCandidates.clear();
  
  int requestedSize = 2 * (int)polys.size();
  // creates a sorted split candidates array  
  splitCandidates.reserve(requestedSize);
  
  PolygonContainer::const_iterator it, it_end = polys.end();

  AxisAlignedBox3 box;

  // insert all queries 
  for(it = polys.begin(); it != it_end; ++ it)
  {
	  box.Initialize();
	  box.Include(*(*it));
	  
	  splitCandidates.push_back(SortableEntry(SortableEntry::POLY_MIN, box.Min(axis), *it));
      splitCandidates.push_back(SortableEntry(SortableEntry::POLY_MAX, box.Max(axis), *it));
  }
  
  stable_sort(splitCandidates.begin(), splitCandidates.end());
}


float BspTree::BestCostRatio(const PolygonContainer &polys,
							 const AxisAlignedBox3 &box,
							 const int axis,
							 float &position,
							 int &objectsBack,
							 int &objectsFront) const
{
	vector<SortableEntry> splitCandidates;

	SortSplitCandidates(polys, axis, splitCandidates);
	
	// go through the lists, count the number of objects left and right
	// and evaluate the following cost funcion:
	// C = ct_div_ci  + (ol + or)/queries
	
	int objectsLeft = 0, objectsRight = (int)polys.size();
	
	float minBox = box.Min(axis);
	float maxBox = box.Max(axis);
	float boxArea = box.SurfaceArea();
  
	float minBand = minBox + sSplitBorder * (maxBox - minBox);
	float maxBand = minBox + (1.0f - sSplitBorder) * (maxBox - minBox);
	
	float minSum = 1e20f;
	vector<SortableEntry>::const_iterator ci, ci_end = splitCandidates.end();

	for(ci = splitCandidates.begin(); ci != ci_end; ++ ci) 
	{
		switch ((*ci).type) 
		{
			case SortableEntry::POLY_MIN:
				++ objectsLeft;
				break;
			case SortableEntry::POLY_MAX:
			    -- objectsRight;
				break;
			default:
				break;
		}
		
		if ((*ci).value > minBand && (*ci).value < maxBand) 
		{
			AxisAlignedBox3 lbox = box;
			AxisAlignedBox3 rbox = box;
			lbox.SetMax(axis, (*ci).value);
			rbox.SetMin(axis, (*ci).value);

			float sum = objectsLeft * lbox.SurfaceArea() + 
						objectsRight * rbox.SurfaceArea();
      
			if (sum < minSum) 
			{
				minSum = sum;
				position = (*ci).value;

				objectsBack = objectsLeft;
				objectsFront = objectsRight;
			}
		}
	}
  
	float oldCost = (float)polys.size();
	float newCost = sCt_div_ci + minSum / boxArea;
	float ratio = newCost / oldCost;


#if 0
  Debug << "====================" << endl;
  Debug << "costRatio=" << ratio << " pos=" << position<<" t=" << (position - minBox)/(maxBox - minBox)
      << "\t o=(" << objectsBack << "," << objectsFront << ")" << endl;
#endif
  return ratio;
}

Plane3 BspTree::SelectPlane(BspLeaf *leaf, PolygonContainer &polys)
{
	if (polys.size() == 0)
	{
		Debug << "Warning: No split candidate available\n";
		return Plane3();
	}

	if ((sSplitPlaneStrategy & AXIS_ALIGNED) &&
		((int)polys.size() > sTermMaxPolysForAxisAligned))
	{
		AxisAlignedBox3 box;
		box.Initialize();
	
		// todo: area subdivision
		Polygon3::IncludeInBox(polys, box);
        
		int objectsBack = 0, objectsFront = 0;
		int axis = 0;
		float costRatio = MAX_FLOAT;
		Vector3 position;

		for (int i = 0; i < 3; ++ i) 
		{
			float p = 0;
			float r = BestCostRatio(polys, box, i, p, objectsBack, objectsFront);
			
			if (r < costRatio)
			{
				costRatio = r;
				axis = i;
				position = p;
			}
		}
		
		if (costRatio < sMaxCostRatio)
		{
			Vector3 norm(0,0,0); norm[axis] = 1.0f;
			return Plane3(norm, position);
		}
		/*int axis = box.Size().DrivingAxis();
		Vector3 norm(0,0,0); norm[axis] = 1;
		Vector3 position = (box.Min()[axis] + box.Max()[axis]) * 0.5f;
		return Plane3(norm, position);*/
	}

	// simplest strategy: just take next polygon
	if (sSplitPlaneStrategy & NEXT_POLYGON)
	{
		return polys.front()->GetSupportingPlane();
	}

	// use heuristics to find appropriate plane
	return SelectPlaneHeuristics(polys, sMaxCandidates);
}

Plane3 BspTree::SelectPlaneHeuristics(PolygonContainer &polys, int maxTests)
{
	float lowestCost = MAX_FLOAT;
	int bestPlaneIdx = 0;
	
	int limit = maxTests > 0 ? Min((int)polys.size(), maxTests) : (int)polys.size();
	
	int candidateIdx = limit;
	
	for (int i = 0; i < limit; ++ i)
	{
		candidateIdx = GetNextCandidateIdx(candidateIdx, polys);

		Plane3 candidatePlane = polys[candidateIdx]->GetSupportingPlane();
		
		// evaluate current candidate
		float candidateCost = EvalSplitPlane(polys, candidatePlane);
			
		if (candidateCost < lowestCost)
		{
			bestPlaneIdx = candidateIdx;
			lowestCost = candidateCost;
		}
	}
	
	//Debug << "Plane lowest cost: " << lowestCost << endl;
	return polys[bestPlaneIdx]->GetSupportingPlane();
}

int BspTree::GetNextCandidateIdx(int currentIdx, PolygonContainer &polys)
{
	int candidateIdx = Random(currentIdx--);
	
	//Debug << "new candidate " << candidateIdx << endl;

	// swap candidates to avoid testing same plane 2 times
	Polygon3 *p = polys[candidateIdx];
	polys[candidateIdx] = polys[currentIdx];
	polys[currentIdx] = p;
	
	return currentIdx;
}

float BspTree::EvalSplitPlane(PolygonContainer &polys, const Plane3 &candidatePlane)
{
	float val = 0;
	
	if (sSplitPlaneStrategy & VERTICAL_AXIS)
	{
		Vector3 tinyAxis(0,0,0); tinyAxis[mBox.Size().TinyAxis()] = 1.0f;
		// we put a penalty on the dot product between the "tiny" vertical axis
		// and the split plane axis
		val += sVerticalSplitsFactor * 
			   fabs(DotProd(candidatePlane.mNormal, tinyAxis));
	}

	//-- strategies where the effect of the split plane on the polygons is tested
	if (!((sSplitPlaneStrategy & BALANCED_POLYS) ||
		  (sSplitPlaneStrategy & LEAST_SPLITS)   ||
		  (sSplitPlaneStrategy & LARGEST_POLY_AREA) ||
		  (sSplitPlaneStrategy & BALANCED_VIEW_CELLS)))
		return val;
	
	PolygonContainer::const_iterator it, it_end = polys.end();
	float sumBalancedPolys = 0;
	float sumSplits = 0;
	float sumPolyArea = 0;
	float sumBalancedViewCells = 0;
	//float totalArea = 0;

	// container for view cells
	ObjectContainer frontViewCells;
	ObjectContainer backViewCells;

	for (it = polys.begin(); it != it_end; ++ it)
	{
		int classification = (*it)->ClassifyPlane(candidatePlane);

		if (sSplitPlaneStrategy & BALANCED_POLYS)
			sumBalancedPolys += sBalancedPolysTable[classification];
		
		if (sSplitPlaneStrategy & LEAST_SPLITS)
			sumSplits += sLeastSplitsTable[classification];

		if (sSplitPlaneStrategy & LARGEST_POLY_AREA)
		{
			if (classification == Polygon3::COINCIDENT)
			{		
				float area = (*it)->GetArea();
				//Debug << "polygon area: " << area << " ";
				sumPolyArea += area;
			}
			//totalArea += area;
		}

		// assign view cells to back or front according to classificaion
		if (sSplitPlaneStrategy & BALANCED_VIEW_CELLS)
		{
			MeshInstance *viewCell = (*it)->mParent;
		
			if (classification == Polygon3::FRONT_SIDE)
				frontViewCells.push_back(viewCell);
			else if (viewCell && (classification == Polygon3::BACK_SIDE))
				backViewCells.push_back(viewCell);
		}
	}

	if (sSplitPlaneStrategy & BALANCED_POLYS)
		val += sBalancedPolysFactor * fabs(sumBalancedPolys) / (float)polys.size();

	if (sSplitPlaneStrategy & LEAST_SPLITS)    
		val += sLeastSplitsFactor * sumSplits / (float)polys.size();

	if (sSplitPlaneStrategy & LARGEST_POLY_AREA) 
		val += sLargestPolyAreaFactor * (float)polys.size() / sumPolyArea; // HACK

	if (sSplitPlaneStrategy & BALANCED_VIEW_CELLS)
	{
		// count number of unique view cells
		sort(frontViewCells.begin(), frontViewCells.end());
		sort(backViewCells.begin(), backViewCells.end());

		ObjectContainer::const_iterator frontIt, frontIt_end = frontViewCells.end();
	
		Intersectable *vc = NULL;
		// increase counter for view cells in front of plane
		for (frontIt = frontViewCells.begin(); frontIt != frontIt_end; ++frontIt)
		{
			if (*frontIt != vc)
			{
				vc = *frontIt;
				sumBalancedViewCells += 1.0f;
			}
		}

		ObjectContainer::const_iterator backIt, backIt_end = backViewCells.end();
		vc = NULL;
		// decrease counter for view cells on back side of plane
		for (backIt = backViewCells.begin(); backIt != backIt_end; ++backIt)
		{
			if (*backIt != vc)
			{
				vc = *backIt;
				sumBalancedViewCells -= 1.0f;
			}
		}

		val += sBalancedViewCellsFactor * fabs(sumBalancedViewCells) / 
			(float)(frontViewCells.size() + backViewCells.size());
	}

	// return linear combination of the sums
	return val;
}

void BspTree::ParseEnvironment()
{
	//-- parse bsp cell tree construction method
	char constructionMethodStr[60];
	
	environment->GetStringValue("BspTree.constructionMethod", constructionMethodStr);

	sConstructionMethod = FROM_INPUT_VIEW_CELLS;
	
	if (strcmp(constructionMethodStr, "fromViewCells") == 0)
		sConstructionMethod = FROM_INPUT_VIEW_CELLS;
	else if (strcmp(constructionMethodStr, "fromSceneGeometry") == 0)
		sConstructionMethod = FROM_SCENE_GEOMETRY;
	else if (strcmp(constructionMethodStr, "fromRays") == 0)
		sConstructionMethod = FROM_RAYS;
	else 
	{
		cerr << "Wrong construction method " << constructionMethodStr << endl;
		exit(1);
    }

	Debug << "Construction method: " << constructionMethodStr << endl;

	environment->GetIntValue("BspTree.Termination.maxDepth", sTermMaxDepth);
	environment->GetIntValue("BspTree.Termination.maxPolygons", sTermMaxPolygons);
	environment->GetIntValue("BspTree.Termination.maxPolysForAxisAligned", 
							 sTermMaxPolysForAxisAligned);
	environment->GetIntValue("BspTree.maxCandidates", sMaxCandidates);
	environment->GetIntValue("BspTree.splitPlaneStrategy", sSplitPlaneStrategy);

	// need kdtree criteria for axis aligned split
	environment->GetFloatValue("MeshKdTree.Termination.ct_div_ci", sCt_div_ci);
	environment->GetFloatValue("MeshKdTree.splitBorder", sSplitBorder);
	environment->GetFloatValue("MeshKdTree.Termination.maxCostRatio", sMaxCostRatio);

    Debug << "BSP max depth: " << sTermMaxDepth << endl;
	Debug << "BSP max polys: " << sTermMaxPolygons << endl;
	Debug << "BSP max candidates: " << sMaxCandidates << endl;
	
	Debug << "Split plane strategy: ";
	if (sSplitPlaneStrategy & NEXT_POLYGON)
		Debug << "next polygon ";
	if (sSplitPlaneStrategy & AXIS_ALIGNED)
		Debug << "axis aligned ";
	if (sSplitPlaneStrategy & LEAST_SPLITS)     
		Debug << "least splits ";
	if (sSplitPlaneStrategy & BALANCED_POLYS)
		Debug << "balanced polygons ";
	if (sSplitPlaneStrategy & BALANCED_VIEW_CELLS)
		Debug << "balanced view cells ";
	if (sSplitPlaneStrategy & LARGEST_POLY_AREA)
		Debug << "largest polygon area ";
	if (sSplitPlaneStrategy & VERTICAL_AXIS)
		Debug << "vertical axis ";

	Debug << endl;
}

void BspTree::CollectLeaves(vector<BspLeaf *> &leaves)
{
	stack<BspNode *> nodeStack;
	nodeStack.push(mRoot);
 
	while (!nodeStack.empty()) 
	{
		BspNode *node = nodeStack.top();
    
		nodeStack.pop();
    
		if (node->IsLeaf()) 
		{
			BspLeaf *leaf = (BspLeaf *)node;
			
			leaves.push_back(leaf);
		} else 
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);
			nodeStack.push(interior->GetBack());
			nodeStack.push(interior->GetFront());
		}
	}
}

AxisAlignedBox3 BspTree::GetBoundingBox() const
{
	return mBox;
}

BspNode *BspTree::GetRoot() const
{
	return mRoot;
}

bool BspTree::StorePolys() const
{
	return mStoreSplitPolys;
}

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

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

	// store maximal and minimal depth
	if (data.mDepth > mStat.maxDepth)
		mStat.maxDepth = data.mDepth;
	if (data.mDepth < mStat.minDepth)
		mStat.minDepth = data.mDepth;

	// accumulate depth to compute average
	mStat.accumDepth += data.mDepth;

	if (leaf->mViewCell)
		++ mStat.viewCellLeaves;

#ifdef _DEBUG
	Debug << "BSP Traversal data. Depth: " << data.mDepth << " (max: " << mTermMaxDepth << "), #polygons: " << 
	  data.mPolygons->size() << " (max: " << sTermMaxPolygons << ")" << endl;
#endif
}

int BspTree::CastRay(Ray &ray)
{
	int hits = 0;
  
	stack<BspRayTraversalData> tStack;
  
	float maxt = 1e6;
	float mint = 0;
	
	Intersectable::NewMail();

	// test with tree bounding box
	if (!mBox.GetMinMaxT(ray, &mint, &maxt))
	{
		return 0;
	}

	if (mint < 0)
		mint = 0;
  
	maxt += Limits::Threshold;
  
	Vector3 entp = ray.Extrap(mint);
	Vector3 extp = ray.Extrap(maxt);
  
	BspNode *node = mRoot;
	BspNode *farChild;
	
	while (1)
	{
		if (!node->IsLeaf()) 
		{
			BspInterior *in = (BspInterior *) node;
			
			Plane3 *splitPlane = in->GetPlane();

			float t = 0;
			bool coplanar = false;
		
			int entSide = splitPlane->Side(entp);
			int extSide = splitPlane->Side(extp);

			Vector3 intersection;

			if (entSide < 0)
			{
				node = in->GetBack();

				if(extSide <= 0) 
					continue;
					
				farChild = in->GetFront(); // plane splits ray

			} else if (entSide > 0)
			{
				node = in->GetFront();

				if (extSide >= 0)
					continue;

				farChild = in->GetBack();	 // plane splits ray			
			}
			else // ray and plane are coincident // WHAT TO DO IN THIS CASE ?
			{
				node = in->GetFront();
				continue;
			}

			// push data for far child
			tStack.push(BspRayTraversalData(farChild, extp, maxt));

			// find intersection with plane between ray origin and exit point
			extp = splitPlane->FindIntersection(ray.GetLoc(), extp, &maxt);
		} else // reached leaf => intersection with view cell
		{
			BspLeaf *leaf = dynamic_cast<BspLeaf *>(node);
			
			//ray.mBspLeaves.push_back(leaf);
			//Debug << "adding view cell" << leaf->mViewCell;
			if (!leaf->mViewCell->Mailed())
			{
				ray.viewCells.push_back(leaf->mViewCell);
				mViewCell->Mail();
				++ hits;
			}
			//Debug << "view cell added" << endl;
   			// get the next node from the stack
			if (tStack.empty())
				break;
      
			entp = extp;
			mint = maxt;
			BspRayTraversalData &s  = tStack.top();

			node = s.mNode;
			extp = s.mExitPoint;
			maxt = s.mMaxT;

			tStack.pop();
		}
	}

	return hits;
}

bool BspTree::Export(const string filename)
{
	Exporter *exporter = Exporter::GetExporter(filename);

	if (exporter) 
	{
		exporter->ExportBspTree(*this);
		return true;
	}	

	return false;
}

void BspTree::CollectViewCells(BspNode *n, ViewCellContainer &viewCells)
{
	stack<BspNode *> nodeStack;

	nodeStack.push(n);

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

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

			if (!viewCell->Mailed()) 
			{
				viewCell->Mail();
				viewCells.push_back(viewCell);
			}
		}
		else 
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);
			nodeStack.push(interior->mFront);
			nodeStack.push(interior->mBack);
		}
	}
}
//} // GtpVisibilityPreprocessor