#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "TraversalTree.h"
#include "ViewCell.h"
#include "Beam.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), mIntersectable(NULL)

{
  if (parent)
    mDepth = parent->mDepth+1;
  else
    mDepth = 0;
}


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


TraversalLeaf::~TraversalLeaf()
{
	DEL_PTR(mViewCell);  
}


TraversalTree::TraversalTree()
{

  
  mRoot = new TraversalLeaf(NULL, 0);
  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);
  
  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);
      }
  splitCandidates = NULL;
}


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

	CLEAR_CONTAINER(mKdIntersectables);
}


bool
TraversalTree::Construct()
{

  if (!splitCandidates)
    splitCandidates = new vector<SortableEntry *>;

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

  mStat.nodes = 1;

  mBox.Initialize();
  ObjectContainer::const_iterator mi;
  for ( mi = leaf->mObjects.begin();
	mi != leaf->mObjects.end();
	mi++) {
	//	cout<<(*mi)->GetBox()<<endl;
	mBox.Include((*mi)->GetBox());
  }

  cout <<"TraversalTree Root Box:"<<mBox<<endl;
  mRoot = Subdivide(TraversalData(leaf, mBox, 0));

  // remove the allocated array
  CLEAR_CONTAINER(*splitCandidates);
  delete splitCandidates;

  float area = GetBox().SurfaceArea()*KD_PVS_AREA;

  SetPvsTerminationNodes(area);
  
  return true;
}

TraversalNode *
TraversalTree::Subdivide(const TraversalData &tdata)
{

  TraversalNode *result = NULL;

  priority_queue<TraversalData> tStack;
  //  stack<STraversalData> tStack;
  
  tStack.push(tdata);
  AxisAlignedBox3 backBox, frontBox;

  while (!tStack.empty()) {
	//	cout<<mStat.Nodes()<<" "<<mTermMaxNodes<<endl;
	if (mStat.Nodes() > mTermMaxNodes) {
	  //    if ( GetMemUsage() > maxMemory ) {
      // count statistics on unprocessed leafs
      while (!tStack.empty()) {
		EvaluateLeafStats(tStack.top());
		tStack.pop();
      }
      break;
    }
	  
	
    TraversalData data = tStack.top();
    tStack.pop();
    
    TraversalNode *node = SubdivideNode((TraversalLeaf *) data.mNode,
								 data.mBox,
								 backBox,
								 frontBox
								 );

	if (result == NULL)
      result = node;
    
    if (!node->IsLeaf()) {

      TraversalInterior *interior = (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->mObjects.size() <= mTermMinCost) ||
		 (leaf->mDepth >= mTermMaxDepth);
 
	if (criteriaMet)
		cerr<<"\n OBJECTS="<<leaf->mObjects.size()<<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;
      bool mOnlyDrivingAxis = true;

	  if (mOnlyDrivingAxis) {
		axis = box.Size().DrivingAxis();
		costRatio = BestCostRatio(leaf,
								  box,
								  axis,
								  position,
								  objectsBack,
								  objectsFront);
      } else {
		costRatio = 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) {
		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);

	ObjectContainer::const_iterator mi;

	for ( mi = leaf->mObjects.begin();
		mi != leaf->mObjects.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);


	// 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->mObjects.begin(); mi != leaf->mObjects.end(); ++ mi) 
	{
		// determine the side of this ray with respect to the plane
		AxisAlignedBox3 box = (*mi)->GetBox();

		// matt: no more ref
		// for handling multiple objects: keep track of references
		//if (leaf->IsRoot()) 
		//	(*mi)->mReferences = 1; // initialise references at root

		// matt: no more ref
		//-- (*mi)->mReferences; // remove parent ref


		if (box.Max(axis) >= position )
		{
			front->mObjects.push_back(*mi);
			//++ (*mi)->mReferences;
		}

		if (box.Min(axis) < position )
		{
			back->mObjects.push_back(*mi);
			// matt: no more ref
			//  ++ (*mi)->mReferences;
		}

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

	// store objects referenced in more than one leaf
	// for easy access
	ProcessMultipleRefs(back);
	ProcessMultipleRefs(front);

	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_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 object refs / 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 << setprecision(4);

  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_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 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->mObjects.size()) < mTermMinCost)
    mStat.minCostNodes++;
  
  
  if ( (int)(leaf->mObjects.size()) > mStat.maxObjectRefs)
    mStat.maxObjectRefs = (int)leaf->mObjects.size();
  
}



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

    int requestedSize = 2*(int)node->mObjects.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(ObjectContainer::const_iterator mi = node->mObjects.begin();
      mi != node->mObjects.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 0

#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

  float totalIntersections = 0.0f;
  vector<SortableEntry *>::const_iterator ci;

  for(ci = splitCandidates->begin();
      ci < splitCandidates->end();
      ci++) 
    if ((*ci)->type == SortableEntry::BOX_MIN) {
      totalIntersections += (*ci)->intersectable->IntersectionComplexity();
    }
	
  float intersectionsLeft = 0;
  float intersectionsRight = totalIntersections;
	
  int objectsLeft = 0, objectsRight = (int)node->mObjects.size();
  
  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;
  
  for(ci = splitCandidates->begin();
      ci < splitCandidates->end();
      ci++) {
    switch ((*ci)->type) {
    case SortableEntry::BOX_MIN:
      objectsLeft++;
      intersectionsLeft += (*ci)->intersectable->IntersectionComplexity();
      break;
    case SortableEntry::BOX_MAX:
      objectsRight--;
      intersectionsRight -= (*ci)->intersectable->IntersectionComplexity();
      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;
      if (mSahUseFaces)
		sum = intersectionsLeft*lbox.SurfaceArea() + intersectionsRight*rbox.SurfaceArea();
      else
		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 = mSahUseFaces ? totalIntersections : node->mObjects.size();
	float newCost = mCt_div_ci + sum/boxArea;
	float ratio = newCost/oldCost;
	costStream<<(*ci)->value<<" "<<ratio<<endl;
  }
#endif
	  
      if (sum < minSum) {
		minSum = sum;
		position = (*ci)->value;
		
		objectsBack = objectsLeft;
		objectsFront = objectsRight;
      }
    }
  }
  
  float oldCost = mSahUseFaces ? totalIntersections : node->mObjects.size();
  float newCost = mCt_div_ci + minSum/boxArea;
  float ratio = newCost/oldCost;
  
#if 0
  cout<<"===================="<<endl;
  cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
      <<"\t o=("<<objectsBack<<","<<objectsFront<<")"<<endl;
#endif
  return ratio;
}


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

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

	stack<RayTraversalData> tStack;

	Intersectable::NewMail();

	//maxt += Limits::Threshold;

	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
			float tdist = (position - origin[axis]) / dir[axis];
			//tStack.push(RayTraversalData(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);

			// add view cell to intersections
			ViewCell *vc = leaf->mViewCell;

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

			// get the next node from the stack
			if (tStack.empty())
				break;

			entp = extp;
			mint = maxt;
			
			RayTraversalData &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);
    }
  }
}


}