#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>

#define INITIAL_COST 999999// unreachable high initial cost for heuristic evaluation

int BspTree::sTermMaxPolygons = 10;
int BspTree::sTermMaxDepth = 20;
int BspTree::sSplitPlaneStrategy = NEXT_POLYGON; 
int BspTree::sMaxCandidates = 10;

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

BspNode::BspNode(): mParent(NULL)
{}

BspNode::BspNode(BspInterior *parent): mParent(parent)
{}


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

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

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

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


BspInterior::BspInterior(Plane3 plane): mPlane(plane) 
{}

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::SplitPolygons(PolygonContainer *polys, 
								PolygonContainer *frontPolys, 
								PolygonContainer *backPolys, 
								int &splits)
{
	while (!polys->empty())
	{
		Polygon3 *poly = polys->back();
		polys->pop_back();

		// test if split is neccessary
		int result = poly->ClassifyPlane(mPlane);

		Polygon3 *front_piece = NULL;
		Polygon3 *back_piece = NULL;

		switch (result)
		{
			case Polygon3::COINCIDENT:
				break; // TODO: compare normals
			case Polygon3::FRONT_SIDE:
				frontPolys->push_back(poly);
				break;
			case Polygon3::BACK_SIDE:
				backPolys->push_back(poly);
				break;
			case Polygon3::SPLIT:
				front_piece = new Polygon3();
				back_piece = new Polygon3();

				//-- split polygon
				poly->Split(&mPlane, front_piece, back_piece, splits);

				frontPolys->push_back(front_piece);
				backPolys->push_back(back_piece);
				
#ifdef _DEBUG
				Debug << "split " << poly << endl << front << endl << back << endl;
#endif
				// don't need polygon anymore
				DEL_PTR(poly);

				break;
			default:
				Debug << "SHOULD NEVER COME HERE\n";
				break;
		}
	}
}

/****************************************************************/
/*                  class BspLeaf implementation                */
/****************************************************************/

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

ViewCell *BspLeaf::GetViewCell()
{
	return mViewCell;
}

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


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

BspTree::BspTree(): mTermMaxPolygons(0), mTermMaxDepth(0), mRoot(NULL)
{
	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_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_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 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)
			interior->GetBack()->mParent = NULL;
			interior->GetFront()->mParent = NULL;

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

		DEL_PTR(node);
	}
}


void BspTree::InsertViewCell(ViewCell *viewCell)
{	
	std::stack<BspTraversalData> tStack;
	
	PolygonContainer polys;

	// copy polygon information to guide the split process
	AddMesh2Polygons(viewCell->GetMesh(), polys);
	mBox.Include(viewCell->GetBox()); // also add to BSP root box

	BspNode *firstNode = mRoot ? mRoot : new BspLeaf(); // traverse tree or create new one

	tStack.push(BspTraversalData(firstNode, NULL, &polys, 0));

	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
			PolygonContainer *frontPolys = new PolygonContainer();
			PolygonContainer *backPolys = new PolygonContainer();

			int splits = 0;
			// split viecell polygons with respect to split plane
			interior->SplitPolygons(tData.mPolygons, frontPolys, backPolys, splits);
			mStat.splits += splits;

			// push the children on the stack
			if (frontPolys->size() > 0) // if polygons on this side of bsp tree
				tStack.push(BspTraversalData(interior->GetFront(), interior->GetParent(), 
							frontPolys, tData.mDepth + 1));
			else
				delete frontPolys;

			if (backPolys->size() > 0) // if polygons on this side of bsp tree
				tStack.push(BspTraversalData(interior->GetBack(), interior->GetParent(), 
							backPolys, tData.mDepth + 1));
			else
				delete backPolys;
		}
		else // reached leaf => subdivide current viewcell
		{
			BspNode *root = Subdivide(tStack, tData, NULL);

			if (!mRoot) // take as root if there is none yet
				mRoot = root;				
		}
	}
}

void BspTree::Construct(const ViewCellContainer &viewCells)
{
	// for this type of construction we split until no polygons is left
	mTermMaxPolygons = 0;
	mTermMaxDepth = sTermMaxDepth;

	mStat.nodes = 1;

	// insert all viewcells
	ViewCellContainer::const_iterator it;

	for (it = viewCells.begin(); it != viewCells.end(); ++ it)
	{
		InsertViewCell(*it);
	}
}

void BspTree::AddMesh2Polygons(Mesh *mesh, PolygonContainer &polys)
{
	FaceContainer::const_iterator fi;
	
	// copy the face data to polygons
	for (fi = mesh->mFaces.begin(); fi != mesh->mFaces.end(); ++ fi)
	{
		Polygon3 *poly = new Polygon3((*fi), mesh);
		polys.push_back(poly);
	}
}

void BspTree::Copy2PolygonSoup(const ObjectContainer &objects, PolygonContainer &polys, int maxObjects)
{
	ObjectContainer::const_iterator it, it_end = objects.end();

	int limit = (maxObjects > 0) ? min((int)objects.size(), maxObjects) : (int)objects.size();
	
	// initialise bounding box
	mBox.Initialize();
  
	for (int i = 0; i < limit; ++i)
	{
		Intersectable *object = objects[i];//*it;
		Mesh *mesh = NULL;

		// extract the meshes
		switch (object->Type())
		{
		case Intersectable::MESH_INSTANCE:
			mesh = dynamic_cast<MeshInstance *>(object)->GetMesh();
			break;
		case Intersectable::VIEWCELL:
			mesh = dynamic_cast<ViewCell *>(object)->GetMesh();
			break;
			// TODO: handle transformed mesh instances
		default:
			break;
		}
		
        if (mesh) // copy the mesh data to polygons
		{
			mBox.Include(object->GetBox()); // also add to BSP root box
			AddMesh2Polygons(mesh, polys);
		}
	}

	Debug << "Number of polygons: " << polys.size() << ", BSP root box: " << mBox << endl;
}

void BspTree::Construct(const ObjectContainer &objects)
{
	Debug << "Constructing tree using object container\n";

	mTermMaxPolygons = sTermMaxPolygons;
	mTermMaxDepth = sTermMaxDepth;

	mStat.nodes = 1;

	std::stack<BspTraversalData> tStack;
	PolygonContainer *polys = new PolygonContainer();
	
	// copy mesh instance polygons into one big polygon soup
	Copy2PolygonSoup(objects, *polys, 100);

	BspTraversalData tData(new BspLeaf(), mRoot->GetParent(), polys, 0); // new root corresponding to unbounded space

	tStack.push(tData);

	while (!tStack.empty()) 
	{
		tData = tStack.top();
	    tStack.pop();

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

		if (!mRoot)
			mRoot = root;
	}
}

BspNode * BspTree::Subdivide(BspTraversalStack &tStack, BspTraversalData &tData, ViewCell *viewCell)
{
	PolygonContainer *backPolys = new PolygonContainer();
	PolygonContainer *frontPolys = new PolygonContainer();

	BspNode *node = SubdivideNode(dynamic_cast<BspLeaf *>(tData.mNode),
								  tData.mParent,
								  tData.mPolygons,
								  tData.mDepth,
								  viewCell,
								  frontPolys,
								  backPolys);

	if (!node->IsLeaf()) // node was subdivided
	{
		BspInterior *interior = dynamic_cast<BspInterior *>(node);

		// push the children on the stack (there are always two children)
		tStack.push(BspTraversalData(interior->GetBack(), interior, backPolys, tData.mDepth + 1));
		tStack.push(BspTraversalData(interior->GetFront(), interior, frontPolys, tData.mDepth + 1));
	}
	else // tree terminates here
	{
		EvaluateLeafStats(tData);
		// don't need to store polygon information => delete polygons
		Polygon3::DeletePolygons(tData.mPolygons);
	}

	return node;
}

Plane3 BspTree::SelectPlane(PolygonContainer *polygons)  const
{
	// simple strategy: just take next polygon
	if (sSplitPlaneStrategy == NEXT_POLYGON)
	{
		return polygons->front()->GetSupportingPlane();
	}
	
	return SelectPlaneHeuristics(polygons, sMaxCandidates);
}

Plane3 BspTree::SelectPlaneHeuristics(PolygonContainer *polygons, int maxTests) const
{
	int lowestCost = INITIAL_COST;
	Plane3 *bestPlane = NULL;
	
	int limit = min((int)polygons->size(), maxTests);

	for (int i = 0; i < limit; ++i)
	{
		int candidateIdx = Random((int)polygons->size());
		Plane3 candidatePlane = (*polygons)[candidateIdx]->GetSupportingPlane();
		
		// evaluate current candidate
		int candidateCost = EvalSplitPlane(polygons, candidatePlane);
			
		if (candidateCost < lowestCost)
		{
			bestPlane = &candidatePlane;
			lowestCost = candidateCost;
			//Debug << "new plane cost " << lowestCost << endl;
		}
	}
		
	return *bestPlane;
}

int BspTree::EvalSplitPlane(PolygonContainer *polygons, const Plane3 &candidatePlane) const
{
	PolygonContainer::const_iterator it;

	int sum = 0, sum2 = 0;

	for (it = polygons->begin(); it < polygons->end(); ++ it)
	{
		int classification = (*it)->ClassifyPlane(candidatePlane);
		
		if ((sSplitPlaneStrategy == BALANCED_TREE) || (sSplitPlaneStrategy == COMBINED))
		{
			sum += EvalForBalancedTree(classification);
		}
		
		if ((sSplitPlaneStrategy == LEAST_SPLITS) || (sSplitPlaneStrategy == COMBINED))
		{
			sum2 += EvalForLeastSplits(classification);
		}
	}

	// return linear combination of both sums
	return abs(sum) + abs(sum2);
}

int BspTree::EvalForBalancedTree(const int classification)
{
	if (classification == Polygon3::FRONT_SIDE)
		return 1;
	else if (classification == Polygon3::BACK_SIDE)
		return -1;

	return 0;
}

int BspTree::EvalForLeastSplits(const int classification)
{
	if (classification == Polygon3::SPLIT)
		return 1;

	return 0;
}

BspNode *BspTree::SubdivideNode(BspLeaf *leaf, 
								BspInterior *parent, 
								PolygonContainer *polys, 
								const int depth, 
								ViewCell *viewCell,
								PolygonContainer *frontPolys, 
								PolygonContainer *backPolys)
{
	// terminate traversal
	if ((polys->size() <= mTermMaxPolygons) || (depth >= mTermMaxDepth))
	{
		return leaf;
	}

	 mStat.nodes += 2;

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

#ifdef _DEBUG
	Debug << node << endl;
#endif
	// split polygon according to current plane
	int splits = 0;
	int polySize = polys->size();
	
	node->SplitPolygons(polys, frontPolys, backPolys, splits);
	
 	mStat.splits += splits;

	// two new leaves
	BspLeaf *back = new BspLeaf();
	BspLeaf *front = new BspLeaf(viewCell);

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

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

	DEL_PTR(leaf); // leaf not member of tree anymore

	return node;
}

void BspTree::ParseEnvironment()
{
	environment->GetIntValue("BspTree.Termination.maxDepth", sTermMaxDepth);
	environment->GetIntValue("BspTree.Termination.maxPolygons", sTermMaxPolygons);
	environment->GetIntValue("BspTree.maxCandidates", sMaxCandidates);

	//-- extract strategy to choose the next split plane
	char splitPlaneStrategyStr[60];

	environment->GetStringValue("BspTree.splitPlaneStrategy", splitPlaneStrategyStr);
	
	sSplitPlaneStrategy = BspTree::NEXT_POLYGON;
	
	if (strcmp(splitPlaneStrategyStr, "nextPolygon") == 0)
		sSplitPlaneStrategy = BspTree::NEXT_POLYGON;
	else if (strcmp(splitPlaneStrategyStr, "leastSplits") == 0)
		sSplitPlaneStrategy = BspTree::LEAST_SPLITS;
	else if (strcmp(splitPlaneStrategyStr, "balancedTree") == 0)
		sSplitPlaneStrategy = BspTree::BALANCED_TREE;
	else 
	{
		cerr << "Wrong BSP split plane strategy " << splitPlaneStrategyStr << endl;
		exit(1);
    }
}

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

int BspTree::CollectLeafPvs()
{
	int totalPvsSize = 0;
  
	stack<BspNode *> nodeStack;
  
	nodeStack.push(mRoot);
  
	while (!nodeStack.empty()) 
	{
		BspNode *node = nodeStack.top();
		
		nodeStack.pop();
		
		if (node->IsLeaf()) 
		{
			BspLeaf *leaf = dynamic_cast<BspLeaf *>(node);

			ViewCell *viewcell = leaf->GetViewCell();
			
			if (!viewcell->Mailed()) 
			{
				viewcell->Mail();
					
				// add this node to pvs of all nodes it can see
				BspPvsMap::iterator ni; 
	  
	/*			for (ni = object->mBspPvs.mEntries.begin(); ni != object->mKdPvs.mEntries.end(); ni++) 
				{
					BspNode *node = (*ni).first;
	    
					// $$ JB TEMPORARY solution -> should add object PVS or explictly computed
					// BSP tree PVS
	    			if (leaf->mBspPvs.AddNodeSample(node))
						totalPvsSize++;
				}*/
			}
		} else 
		{
			// traverse tree
			BspInterior *interior = (BspInterior *)node;
      
			nodeStack.push(interior->GetFront());
			nodeStack.push(interior->GetBack());
		}
	}

	return totalPvsSize;
}

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

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

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 >= mTermMaxDepth)
	{
		++ mStat.maxDepthNodes;
	}

	// record maximal depth
	if (data.mDepth > mStat.maxDepth)
		mStat.maxDepth = data.mDepth;

#ifdef _DEBUG
	Debug << "BSP Traversal data. Depth: " << data.mDepth << " (max: " << mTermMaxDepth<< "), #polygons: " << 
	  data.mPolygons->size() << " (max: " << mTermMaxPolygons << ")" << 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;
	
	float position;
	
	while (1) // endless loop
	{
		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;
			}

			//-- split

			// 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 // compute intersections with objects in leaf
		{
			BspLeaf *leaf = dynamic_cast<BspLeaf *>(node);
			//ray.leaves.push_back(leaf);
      
   			/*ObjectContainer::const_iterator mi;
			for (mi = leaf->mObjects.begin(); mi != leaf->mObjects.end(); ++mi) 
			{
				Intersectable *object = *mi;
				if (!object->Mailed()) 
				{
					object->Mail();
					//ray.meshes.push_back(mesh);
					hits += object->CastRay(ray);
				}
			}*/

			if (hits && ray.GetType() == Ray::LOCAL_RAY)
				if (ray.intersections[0].mT <= maxt)
					break;
      
			// 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;
	}
}

//} // GtpVisibilityPreprocessor