#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "TraversalTree.h"
#include "ViewCell.h"
#include "Beam.h"
#include "Exporter.h"


// $$JB HACK
#define KD_PVS_AREA (1e-5f)

namespace GtpVisibilityPreprocessor {

int TraversalNode::sMailId = 1;
int TraversalNode::sReservedMailboxes = 1;


inline static bool ilt(Intersectable *obj1, Intersectable *obj2)
{
	return obj1->mId < obj2->mId;
}


TraversalNode::TraversalNode(TraversalInterior *parent): 
mParent(parent), mMailbox(0)
{
}


TraversalInterior::TraversalInterior(TraversalInterior *parent): 
TraversalNode(parent), mBack(NULL), mFront(NULL) 
{
}


TraversalInterior::~TraversalInterior()
{
	// recursivly destroy children
	DEL_PTR(mFront);
	DEL_PTR(mBack);
}


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


void TraversalInterior::SetupChildLinks(TraversalNode *b, TraversalNode *f) 
{
	mBack = b;
	mFront = f;
	
	b->mParent = f->mParent = this;
}


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


TraversalLeaf::TraversalLeaf(TraversalInterior *parent, const int objects):
TraversalNode(parent)
{
	mViewCells.reserve(objects);
}


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


TraversalLeaf::~TraversalLeaf()
{
}


TraversalTree::TraversalTree()
{ 
	TraversalLeaf *leaf = new TraversalLeaf(NULL, 0);
	leaf->mDepth = 0;
	mRoot = leaf;

	Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.maxNodes", mTermMaxNodes);
	Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.maxDepth", mTermMaxDepth);
	Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.minCost", mTermMinCost);
	Environment::GetSingleton()->GetFloatValue("TraversalTree.Termination.maxCostRatio", mMaxCostRatio);
	Environment::GetSingleton()->GetFloatValue("TraversalTree.Termination.ct_div_ci", mCt_div_ci);
	Environment::GetSingleton()->GetFloatValue("TraversalTree.splitBorder", mSplitBorder);

	Environment::GetSingleton()->GetBoolValue("TraversalTree.sahUseFaces", mSahUseFaces);

	char splitType[64];
	Environment::GetSingleton()->GetStringValue("TraversalTree.splitMethod", splitType);

	splitCandidates = NULL;
	mSplitMethod = SPLIT_SPATIAL_MEDIAN;

	if (strcmp(splitType, "spatialMedian") == 0)
	{
		mSplitMethod = SPLIT_SPATIAL_MEDIAN;
	}
	else
	{
		if (strcmp(splitType, "objectMedian") == 0)
		{
			mSplitMethod = SPLIT_OBJECT_MEDIAN;
		}
		else
		{
			if (strcmp(splitType, "SAH") == 0)
			{
				mSplitMethod = SPLIT_SAH;
			}
			else 
			{
				cerr << "Wrong kd split type " << splitType << endl;
				exit(1);
			}
		}
	}
	cout << "Traversal Tree Split method: " << mSplitMethod << endl;
}


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


bool TraversalTree::Construct(const ViewCellContainer &viewCells)
{
	if (!splitCandidates)
	{
		splitCandidates = new vector<SortableEntry *>;
	}

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

	leaf->mViewCells = viewCells;
	mStat.nodes = 1;
	mBox.Initialize();

	ViewCellContainer::const_iterator mi;

	for ( mi = leaf->mViewCells.begin(); mi != leaf->mViewCells.end(); ++ mi) 
	{
		mBox.Include((*mi)->GetBox());
	}

	cout << "TraversalTree Root Box:" << mBox << endl;
	mRoot = Subdivide(TraversalData(leaf, mBox, 0));
	
	// remove the allocated array
	if (splitCandidates)
	{
		CLEAR_CONTAINER(*splitCandidates);
		delete splitCandidates;
	}
	
	return true;
}


TraversalNode *TraversalTree::Subdivide(const TraversalData &tdata)
{
	TraversalNode *result = NULL;

	//priority_queue<TraversalData> tStack;
	stack<TraversalData> tStack;

	tStack.push(tdata);
	AxisAlignedBox3 backBox, frontBox;

	while (!tStack.empty()) 
	{
		if (mStat.Nodes() > mTermMaxNodes) 
		{
			while (!tStack.empty()) 
			{
				EvaluateLeafStats(tStack.top());
				tStack.pop();
			}
			break;
		}

		TraversalData data = tStack.top();
		tStack.pop();

		TraversalLeaf *tLeaf = static_cast<TraversalLeaf *> (data.mNode);
		TraversalNode *node = SubdivideNode(tLeaf,
											data.mBox,
											backBox,
											frontBox);

		if (result == NULL)
			result = node;

		if (!node->IsLeaf()) 
		{
			TraversalInterior *interior = static_cast<TraversalInterior *>(node);

			// push the children on the stack
			tStack.push(TraversalData(interior->mBack, backBox, data.mDepth + 1));
			tStack.push(TraversalData(interior->mFront, frontBox, data.mDepth + 1));

		} 
		else 
		{
			EvaluateLeafStats(data);
		}
	}

	return result;
}


bool TraversalTree::TerminationCriteriaMet(const TraversalLeaf *leaf)
{
	const bool criteriaMet = (
		((int)leaf->mViewCells.size() <= mTermMinCost) ||
		 (leaf->mDepth >= mTermMaxDepth) 
		 || (GetBox(leaf).SurfaceArea() < 0.00001f)
		 );

	if (criteriaMet)
	{
		cerr << "\nOBJECTS=" << (int)leaf->mViewCells.size() << endl;
		cerr << "\nDEPTH=" << (int)leaf->mDepth << endl;
	}

	return criteriaMet;
}


int TraversalTree::SelectPlane(TraversalLeaf *leaf,
							   const AxisAlignedBox3 &box,
							   float &position)
{
	int axis = -1;

	switch (mSplitMethod)
	{
	case SPLIT_SPATIAL_MEDIAN: 
		{
			axis = box.Size().DrivingAxis();
			position = (box.Min()[axis] + box.Max()[axis]) * 0.5f;
			break;
		}
	case SPLIT_SAH: 
		{
			int objectsBack, objectsFront;
			float costRatio = 99999;//MAX_FLOAT;

			for (int i=0; i < 3; ++ i) 
			{
				float p;
				float r = BestCostRatio(leaf,
										box,
										i,
										p,
										objectsBack,
										objectsFront);
	
				if (r < costRatio) 
				{
					costRatio = r;
					axis = i;
					position = p;
				}
			}

			if (costRatio > mMaxCostRatio) 
			{
				cout << "Too big cost ratio " << costRatio << endl;
				axis = -1;
			}
			break;
		}
	}

	return axis;
}


TraversalNode *TraversalTree::SubdivideNode(TraversalLeaf *leaf,
											const AxisAlignedBox3 &box,
											AxisAlignedBox3 &backBBox,
											AxisAlignedBox3 &frontBBox)
{

	if (TerminationCriteriaMet(leaf))
		return leaf;

	float position;

	// select subdivision axis
	int axis = SelectPlane( leaf, box, position );

	if (axis == -1) {
		cout << "terminate on cost ratio" << endl;
		++ mStat.costRatioNodes;
		return leaf;
	}

	mStat.nodes+=2;
	mStat.splits[axis]++;

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

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

	backBBox = box;
	frontBBox = box;

	// first count ray sides
	int objectsBack = 0;
	int objectsFront = 0;

	backBBox.SetMax(axis, position);
	frontBBox.SetMin(axis, position);

	ViewCellContainer::const_iterator mi, mi_end = leaf->mViewCells.end();

	for (mi = leaf->mViewCells.begin(); mi != mi_end; ++ mi)
	{
		// determine the side of this ray with respect to the plane
		AxisAlignedBox3 box = (*mi)->GetBox();
		if (box.Max(axis) > position) 
			++ objectsFront;

		if (box.Min(axis) < position)
			++ objectsBack;
	}

	TraversalLeaf *back = new TraversalLeaf(node, objectsBack);
	TraversalLeaf *front = new TraversalLeaf(node, objectsFront);

	back->mDepth = front->mDepth = leaf->mDepth + 1;

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

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

	for (mi = leaf->mViewCells.begin(); mi != mi_end; ++ mi) 
	{
		// determine the side of this ray with respect to the plane
		AxisAlignedBox3 box = (*mi)->GetBox();

		if (box.Max(axis) >= position )
		{
			front->mViewCells.push_back(*mi);
		}

		if (box.Min(axis) < position )
		{
			back->mViewCells.push_back(*mi);
		}

		mStat.objectRefs -= (int)leaf->mViewCells.size();
		mStat.objectRefs += objectsBack + objectsFront;
	}

	delete leaf;

	return node;
}


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

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

	app << "#N_AVGOBJECTREFS  ( Avg number of object refs / leaf )\n" <<
		totalObjectRefs / (double)Leaves() << "\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_PMINCOSTLEAVES  ( Percentage of leaves with minCost )\n"<<
		minCostNodes * 100 / (double)Leaves() << endl;

	app << "#N_COSTRATIOLEAVES ( Percentage of leaves with cost ratio termination )\n"<<
		costRatioNodes * 100 / (double)Leaves() << endl;

	//  app << setprecision(4);
	//  app << "#N_CTIME  ( Construction time [s] )\n"
	//      << Time() << " \n";

	app << "======= END OF TraversalTree statistics ========\n";
}


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

	if (data.mDepth >= mTermMaxDepth)
		++ mStat.maxDepthNodes;

	if ((int)(leaf->mViewCells.size()) <= mTermMinCost)
		++ mStat.minCostNodes;

	mStat.totalObjectRefs += (int)leaf->mViewCells.size();

	if ((int)(leaf->mViewCells.size()) > mStat.maxObjectRefs)
		mStat.maxObjectRefs = (int)leaf->mViewCells.size();
}


void
TraversalTree::SortSubdivisionCandidates(TraversalLeaf *node,
										 const int axis)
{
	CLEAR_CONTAINER(*splitCandidates);
	//splitCandidates->clear();

    int requestedSize = 2 * (int)node->mViewCells.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(ViewCellContainer::const_iterator mi = node->mViewCells.begin();
		mi != node->mViewCells.end();
		mi++) 
	{
		AxisAlignedBox3 box = (*mi)->GetBox();

		//cout << "box: " << box << endl;
		splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MAX,
			box.Max(axis),
			*mi)
			);
	}

	// insert all queries 
	for(ViewCellContainer::const_iterator mi = node->mViewCells.begin();
		mi != node->mViewCells.end();
		mi++) 
	{
		AxisAlignedBox3 box = (*mi)->GetBox();

		splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MIN,
			box.Min(axis),
			*mi)
			);
		/*splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MAX,
			box.Max(axis),
			*mi)
			);*/
	}

	stable_sort(splitCandidates->begin(), splitCandidates->end(), iltS);
}


float
TraversalTree::BestCostRatio(TraversalLeaf *node,
							 const AxisAlignedBox3 &box,
							 const int axis,
							 float &position,
							 int &objectsBack,
							 int &objectsFront
							 )
{

#define DEBUG_COST 1

#if DEBUG_COST
	static int nodeId = -1;
	char filename[256];

	static int lastAxis = 100;

	if (axis <= lastAxis)
		++ nodeId;

	lastAxis = axis;

	sprintf(filename, "sah-cost%d-%d.log", nodeId, axis);
	ofstream costStream;

	if (nodeId < 100)
		costStream.open(filename);

#endif

	SortSubdivisionCandidates(node, axis);

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

	vector<SortableEntry *>::const_iterator ci;

	int objectsLeft = 0, objectsRight = (int)node->mViewCells.size();

	int dummy1 = objectsLeft, dummy2 = objectsRight;

	float minBox = box.Min(axis);
	float maxBox = box.Max(axis);
	float boxArea = box.SurfaceArea();

	float minBand = minBox + mSplitBorder*(maxBox - minBox);
	float maxBand = minBox + (1.0f - mSplitBorder)*(maxBox - minBox);

	float minSum = 1e20f;

	int openBoxes = 0;

	for(ci = splitCandidates->begin(); ci < splitCandidates->end(); ++ ci) 
	{
		switch ((*ci)->type) 
		{
		case SortableEntry::BOX_MIN:
			++ objectsLeft;
			++ openBoxes;
			break;
		case SortableEntry::BOX_MAX:
			-- objectsRight;
			-- openBoxes;
			break;
		}

		if ((*ci)->value >= minBand && (*ci)->value <= maxBand) 
		{
			AxisAlignedBox3 lbox = box;
			AxisAlignedBox3 rbox = box;

			lbox.SetMax(axis, (*ci)->value);
			rbox.SetMin(axis, (*ci)->value);

			const float sum = objectsLeft * lbox.SurfaceArea() + objectsRight * rbox.SurfaceArea();

			// cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
			// cout<<"cost= "<<sum<<endl;

#if DEBUG_COST
			if (nodeId < 100) 
			{
				float oldCost = (float)node->mViewCells.size();
				float newCost = mCt_div_ci + sum / boxArea;
				float ratio = newCost / oldCost;

				costStream << (*ci)->value << " " << ratio << " open: " << openBoxes;

				if ((*ci)->type == SortableEntry::BOX_MAX)
					costStream << " max event" << endl;
				else 
					costStream << " min event" << endl;
			}
#endif

			if (sum < minSum) 
			{
				minSum = sum;
				position = (*ci)->value;

				objectsBack = objectsLeft;
				objectsFront = objectsRight;
			}
		}
	}

	const float oldCost = (float)node->mViewCells.size();
	const float newCost = mCt_div_ci + minSum / boxArea;
	const float ratio = newCost / oldCost;

	//if (boxArea == 0)
	//	cout << "error: " << boxArea << endl;
	if (ratio > 2)
	{
		cout << "costratio: " << ratio << " oldcost: " << oldCost << " box area: " << boxArea << " new: " << newCost << endl;
		cout << "obj left: " <<objectsBack<< " obj right: " << objectsFront << endl;
		cout << "dummy1: " << dummy1 << " dummy2: " << dummy2 << endl;
	}
#if 0
	cout<<"===================="<<endl;
	cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
			<<"\t o=("<<objectsBack<<","<<objectsFront<<")"<<endl;
#endif

	return ratio;
}


int TraversalTree::FindViewCellIntersections(const Vector3 &lStart, 
											 const Vector3 &lEnd,
											 const ViewCellContainer &viewCells,
											 ViewCellContainer &hitViewCells,
											 const bool useMailboxing)
{
	int hits = 0;
	ViewCellContainer::const_iterator vit, vit_end = viewCells.end();

	for (vit = viewCells.begin(); vit != vit_end; ++ vit)
	{
		ViewCell *viewCell = *vit;
		// don't have to mail if each view cell belongs to exactly one leaf
		if (!useMailboxing || !viewCell->Mailed())
		{
			if (useMailboxing)
                viewCell->Mail();

			// hack: assume that we use vsp tree, 
			//P so we can just test intersection with bounding boxes
			if (viewCell->GetBox().Intersects(lStart, lEnd))
			{
				hitViewCells.push_back(viewCell);
				++ hits;
			}
		}
	}

	return hits;
}


int TraversalTree::CastLineSegment(const Vector3 &origin,
								   const Vector3 &termination,
								   ViewCellContainer &viewCells,
								   const bool useMailboxing)
{
	int hits = 0;

	float mint = 0.0f, maxt = 1.0f;
	const Vector3 dir = termination - origin;

	stack<LineTraversalData> tStack;

	Vector3 entp = origin;
	Vector3 extp = termination;

	TraversalNode *node = mRoot;
	TraversalNode *farChild;

	float position;
	int axis;

	while (1)
	{
		if (!node->IsLeaf())
		{
			TraversalInterior *in = static_cast<TraversalInterior *>(node);
			position = in->mPosition;
			axis = in->mAxis;

			if (entp[axis] <= position)
			{
				if (extp[axis] <= position)
				{
					node = in->mBack;
					// cases N1,N2,N3,P5,Z2,Z3
					continue;
				} else
				{
					// case N4
					node = in->mBack;
					farChild = in->mFront;
				}
			}
			else
			{
				if (position <= extp[axis])
				{
					node = in->mFront;
					// cases P1,P2,P3,N5,Z1
					continue;
				}
				else
				{
					node = in->mFront;
					farChild = in->mBack;
					// case P4
				}
			}

 			// $$ modification 3.5.2004 - hints from Kamil Ghais
			// case N4 or P4
			const float tdist = (position - origin[axis]) / dir[axis];
			tStack.push(LineTraversalData(farChild, extp, maxt)); //TODO

			extp = origin + dir * tdist;
			maxt = tdist;
		}
		else
		{
			// compute intersection with all objects in this leaf
			TraversalLeaf *leaf = static_cast<TraversalLeaf *>(node);

			hits += FindViewCellIntersections(origin, 
											  termination, 
											  leaf->mViewCells,
											  viewCells,
											  useMailboxing);
			
			// get the next node from the stack
			if (tStack.empty())
				break;

			entp = extp;
			mint = maxt;
			
			LineTraversalData &s  = tStack.top();
			node = s.mNode;
			extp = s.mExitPoint;
			maxt = s.mMaxT;

			tStack.pop();
		}
	}

	return hits;
}


void TraversalTree::CollectLeaves(vector<TraversalLeaf *> &leaves)
{
	stack<TraversalNode *> nodeStack;
	nodeStack.push(mRoot);

	while (!nodeStack.empty()) 
	{
		TraversalNode *node = nodeStack.top();
		nodeStack.pop();
		
		if (node->IsLeaf()) 
		{
			TraversalLeaf *leaf = (TraversalLeaf *)node;
			leaves.push_back(leaf);
		} 
		else 
		{
			TraversalInterior *interior = (TraversalInterior *)node;
			nodeStack.push(interior->mBack);
			nodeStack.push(interior->mFront);
		}
	}
}


AxisAlignedBox3 TraversalTree::GetBox(const TraversalNode *node) const 
{	
	TraversalInterior *parent = node->mParent;
	
	if (parent == NULL)
		return mBox;

	if (!node->IsLeaf())
		return ((TraversalInterior *)node)->mBox;

	AxisAlignedBox3 box(parent->mBox);
	
	if (parent->mFront == node)
		box.SetMin(parent->mAxis, parent->mPosition);
	else
		box.SetMax(parent->mAxis, parent->mPosition);
	return box;
}

}