#include <stack>
#include <time.h>
#include <iomanip>

#include "Plane3.h"
#include "VspBspTree.h"
#include "Mesh.h"
#include "common.h"
#include "ViewCell.h"
#include "Environment.h"
#include "Polygon3.h"
#include "Ray.h"
#include "AxisAlignedBox3.h"
#include "Exporter.h"
#include "Plane3.h"
#include "ViewCellBsp.h"
#include "ViewCellsManager.h"
#include "Beam.h"

#define USE_FIXEDPOINT_T 0


//-- static members


int VspBspTree::sFrontId = 0;
int VspBspTree::sBackId = 0;
int VspBspTree::sFrontAndBackId = 0;

typedef pair<BspNode *, BspNodeGeometry *> bspNodePair;


// pvs penalty can be different from pvs size
inline float EvalPvsPenalty(const int pvs, 
							const int lower,
							const int upper)
{
	// clamp to minmax values
	if (pvs < lower)
		return (float)lower;
	if (pvs > upper)
		return (float)upper;

	return (float)pvs;
}




/******************************************************************************/
/*                       class ViewCellBspTree implementation                 */
/******************************************************************************/


BspViewCell *VspBspTree::GetOutOfBoundsCell()
{
	return mOutOfBoundsCell;
}

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

void VspBspTree::CollectViewCells(BspNode *root,
								  bool onlyValid,
								  ViewCellContainer &viewCells,
								  bool onlyUnmailed) const
{
	stack<BspNode *> nodeStack;

	if (!root)
		return;

	nodeStack.push(root);
	
	while (!nodeStack.empty()) 
	{
		BspNode *node = nodeStack.top();
		nodeStack.pop();
		
		if (node->IsLeaf())
		{
			if (!onlyValid || node->TreeValid())
			{
				ViewCell *leafVc = dynamic_cast<BspLeaf *>(node)->GetViewCell();

				ViewCell *viewCell = mViewCellsTree->GetActiveViewCell(leafVc);
						
				if (!onlyUnmailed || !viewCell->Mailed()) 
				{
					viewCell->Mail();
					viewCells.push_back(viewCell);
				}
			}
		}
		else
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);
		
			nodeStack.push(interior->GetFront());
			nodeStack.push(interior->GetBack());
		}
	}

}


int ViewCellBspTree::FindNeighbors(BspNode *n, vector<BspLeaf *> &neighbors,
								   const bool onlyUnmailed) const
{
	stack<bspNodePair> nodeStack;
	
	BspNodeGeometry nodeGeom;
	ConstructGeometry(n, nodeGeom);
	
	// split planes from the root to this node
	// needed to verify that we found neighbor leaf
	// TODO: really needed?
	vector<Plane3> halfSpaces;
	ExtractHalfSpaces(n, halfSpaces);


	BspNodeGeometry *rgeom = new BspNodeGeometry();
	ConstructGeometry(mRoot, *rgeom);

	nodeStack.push(bspNodePair(mRoot, rgeom));

	while (!nodeStack.empty())
	{
		BspNode *node = nodeStack.top().first;
		BspNodeGeometry *geom = nodeStack.top().second;
	
		nodeStack.pop();

		if (node->IsLeaf())
		{
			// test if this leaf is in valid view space
			if (node->TreeValid() &&
				(node != n) && 
				(!onlyUnmailed || !node->Mailed()))
			{
				bool isAdjacent = true;

				if (1)
				{
					// test all planes of current node if still adjacent
					for (int i = 0; (i < halfSpaces.size()) && isAdjacent; ++ i)
					{
						const int cf =
							Polygon3::ClassifyPlane(geom->GetPolys(),
													halfSpaces[i],
													mEpsilon);

						if (cf == Polygon3::FRONT_SIDE)
						{
							isAdjacent = false;
						}
					}
				}
				else
				{
					// TODO: why is this wrong??
					// test all planes of current node if still adjacent
					for (int i = 0; (i < nodeGeom.Size()) && isAdjacent; ++ i)
					{
						Polygon3 *poly = nodeGeom.GetPolys()[i];

						const int cf =
							Polygon3::ClassifyPlane(geom->GetPolys(),
													poly->GetSupportingPlane(),
													mEpsilon);

						if (cf == Polygon3::FRONT_SIDE)
						{
							isAdjacent = false;
						}
					}
				}
				// neighbor was found
				if (isAdjacent)
				{	
					neighbors.push_back(dynamic_cast<BspLeaf *>(node));
				}
			}
		}
		else
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);

			const int cf = Polygon3::ClassifyPlane(nodeGeom.GetPolys(),
												   interior->GetPlane(),
												   mEpsilon);
			
			BspNode *front = interior->GetFront();
			BspNode *back = interior->GetBack();
            
			BspNodeGeometry *fGeom = new BspNodeGeometry();
			BspNodeGeometry *bGeom = new BspNodeGeometry();

			geom->SplitGeometry(*fGeom,
								*bGeom,
								interior->GetPlane(),
								mBox,
								//0.0000001f);
								mEpsilon);
		
			if (cf == Polygon3::BACK_SIDE)
			{
				nodeStack.push(bspNodePair(interior->GetBack(), bGeom));
				DEL_PTR(fGeom);
			}
			else
			{
				if (cf == Polygon3::FRONT_SIDE)
				{
					nodeStack.push(bspNodePair(interior->GetFront(), fGeom));
					DEL_PTR(bGeom);
				}
				else
				{	// random decision
					nodeStack.push(bspNodePair(front, fGeom));
					nodeStack.push(bspNodePair(back, bGeom));
				}
			}
		}
	
		DEL_PTR(geom);
	}

	return (int)neighbors.size();
}



BspLeaf *ViewCellBspTree::GetRandomLeaf(const Plane3 &halfspace)
{
    stack<BspNode *> nodeStack;
  	nodeStack.push(mRoot);

	int mask = rand();

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

		if (node->IsLeaf())
		{
			return dynamic_cast<BspLeaf *>(node);
		}
		else
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);
			BspNode *next;
			BspNodeGeometry geom;

			// todo: not very efficient: constructs full cell everytime
			ConstructGeometry(interior, geom);

			const int cf = 
				Polygon3::ClassifyPlane(geom.GetPolys(), halfspace, mEpsilon);

			if (cf == Polygon3::BACK_SIDE)
				next = interior->GetFront();
			else
				if (cf == Polygon3::FRONT_SIDE)
					next = interior->GetFront();
			else
			{
				// random decision
				if (mask & 1)
					next = interior->GetBack();
				else
					next = interior->GetFront();
				mask = mask >> 1;
			}

			nodeStack.push(next);
		}
	}

	return NULL;
}


BspLeaf *ViewCellBspTree::GetRandomLeaf(const bool onlyUnmailed)
{
	stack<BspNode *> nodeStack;

	nodeStack.push(mRoot);

	int mask = rand();

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

		if (node->IsLeaf())
		{
			if ( (!onlyUnmailed || !node->Mailed()) )
				return dynamic_cast<BspLeaf *>(node);
		}
		else
		{
			BspInterior *interior = dynamic_cast<BspInterior *>(node);

			// random decision
			if (mask & 1)
				nodeStack.push(interior->GetBack());
			else
				nodeStack.push(interior->GetFront());

			mask = mask >> 1;
		}
	}

	return NULL;
}


float ViewCellBspTree::GetEpsilon() const
{
	return mEpsilon;
}


int ViewCellBspTree::CastLineSegment(const Vector3 &origin,
									 const Vector3 &termination,
									 vector<ViewCell *> &viewcells)
{
	int hits = 0;
	stack<BspRayTraversalData> tQueue;

	float mint = 0.0f, maxt = 1.0f;

	Intersectable::NewMail();
	ViewCell::NewMail();

	Vector3 entp = origin;
	Vector3 extp = termination;

	BspNode *node = mRoot;
	BspNode *farChild = NULL;

	float t;
	const float thresh = 1e-6f; // matt: change this
	
	while (1)
	{
		if (!node->IsLeaf())
		{
			BspInterior *in = dynamic_cast<BspInterior *>(node);

			Plane3 splitPlane = in->GetPlane();
			
			const int entSide = splitPlane.Side(entp, thresh);
			const int extSide = splitPlane.Side(extp, thresh);

			if (entSide < 0) 
			{
				node = in->GetBack();
				
				// plane does not split ray => no far child
				if (extSide <= 0) 
					continue;
  
				farChild = in->GetFront(); // plane splits ray
			} 
			else if (entSide > 0)
			{
				node = in->GetFront();

				if (extSide >= 0) // plane does not split ray => no far child
					continue;

				farChild = in->GetBack(); // plane splits ray
			}
			else // one of the ray end points is on the plane
			{	// NOTE: what to do if ray is coincident with plane?
				if (extSide < 0)
					node = in->GetBack();
				else //if (extSide > 0)
					node = in->GetFront();
				//else break; // coincident => count no intersections

				continue; // no far child
			}

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

			// find intersection of ray segment with plane
			extp = splitPlane.FindIntersection(origin, extp, &t);
		} 
		else
		{
			// reached leaf => intersection with view cell
			BspLeaf *leaf = dynamic_cast<BspLeaf *>(node);
			ViewCell *viewCell;
			
			if (0)
				viewCell = mViewCellsTree->GetActiveViewCell(leaf->GetViewCell());
			else
				viewCell = leaf->GetViewCell();

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

			//-- fetch the next far child from the stack
			if (tQueue.empty())
				break;

			entp = extp;
			
			BspRayTraversalData &s = tQueue.top();

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

			tQueue.pop();
		}
	}

	return hits;
}


bool ViewCellBspTree::ViewPointValid(const Vector3 &viewPoint) const
{
	BspNode *node = mRoot;

	while (1)
	{
		// early exit
		if (node->TreeValid())
			return true;

		if (node->IsLeaf())
			return false;
			
		BspInterior *in = dynamic_cast<BspInterior *>(node);
					
		if (in->GetPlane().Side(viewPoint) <= 0) 
		{
			node = in->GetBack();
		} 
		else
		{
			node = in->GetFront();
		}
	}

	// should never come here
	return false;
}