#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "MeshKdTree.h"

namespace GtpVisibilityPreprocessor {


float MeshKdTree::mSplitBorder;
int MeshKdTree::mTermMaxDepth;
int MeshKdTree::mTermMinCost;
float MeshKdTree::mMaxCostRatio;
float MeshKdTree::mCt_div_ci;
int MeshKdTree::mSplitMethod;

MeshKdTree::MeshKdTree(Mesh *mesh):mMesh(mesh)
{
  mRoot = new MeshKdLeaf;
  mSplitCandidates = NULL;
}

void
MeshKdTree::ParseEnvironment()
{
  Environment::GetSingleton()->GetIntValue("MeshKdTree.Termination.maxDepth", mTermMaxDepth);
  Environment::GetSingleton()->GetIntValue("MeshKdTree.Termination.minCost", mTermMinCost);
  Environment::GetSingleton()->GetFloatValue("MeshKdTree.Termination.maxCostRatio", mMaxCostRatio);
  Environment::GetSingleton()->GetFloatValue("MeshKdTree.Termination.ct_div_ci", mCt_div_ci);
  Environment::GetSingleton()->GetFloatValue("MeshKdTree.splitBorder", mSplitBorder);
  
  char splitType[64];
  Environment::GetSingleton()->GetStringValue("MeshKdTree.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);
      }
}



int
MeshKdTree::SelectPlane(MeshKdLeaf *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 = false;
      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;
}

MeshKdNode *
MeshKdTree::SubdivideNode(
			  MeshKdLeaf *leaf,
			  MeshKdInterior *parent,
			  const AxisAlignedBox3 &box,
			  const int depth,
			  AxisAlignedBox3 &backBBox,
			  AxisAlignedBox3 &frontBBox
			  )
{
  
  if (TerminationCriteriaMet(leaf, depth))
    return leaf;
  
  float position;
  // select subdivision axis
  int axis = SelectPlane( leaf, box, position );
  
  if (axis == -1) {
    return leaf;
  }
  
  // add the new nodes to the tree
  MeshKdInterior *node = new MeshKdInterior;
  
  node->mAxis = axis;
  node->mPosition = position;
  
  backBBox = box;
  frontBBox = box;
  
  // first count ray sides
  
  backBBox.SetMax(axis, position);
  frontBBox.SetMin(axis, position);

  vector<int>::const_iterator fi;
  vector<int> objectsFront, objectsBack;
  
  for ( fi = leaf->mFaces.begin();
	fi != leaf->mFaces.end();
	fi++) {
    // determine the side of this ray with respect to the plane
    AxisAlignedBox3 box = mMesh->GetFaceBox(*fi);

    if (box.Max(axis) > position ) 
      objectsFront.push_back(*fi);
    
    if (box.Min(axis) < position )
      objectsBack.push_back(*fi);
  }
  
  MeshKdLeaf *back = new MeshKdLeaf(objectsBack);
  MeshKdLeaf *front = new MeshKdLeaf(objectsFront);

  // replace a link from node's parent
  if (  parent )
    parent->ReplaceChildLink(leaf, node);
  
  // and setup child links
  node->SetupChildLinks(back, front);
  
  delete leaf;
  return node;
}




void
MeshKdTree::SortSplitCandidates(
			    MeshKdLeaf *node,
			    const int axis
			    )
{
  mSplitCandidates->clear();
  
  int requestedSize = 2*(int)node->mFaces.size();
  // creates a sorted split candidates array
  if (mSplitCandidates->capacity() > 500000 &&
      requestedSize < (int)(mSplitCandidates->capacity()/10) ) {
    delete mSplitCandidates;
    mSplitCandidates = new vector<SortableEntry>;
  }
  
  mSplitCandidates->reserve(requestedSize);
  
  vector<int>::const_iterator fi;
  // insert all queries 
  for(fi = node->mFaces.begin();
      fi < node->mFaces.end();
      fi++) {
    
    AxisAlignedBox3 box = mMesh->GetFaceBox(*fi);
    
    mSplitCandidates->push_back(SortableEntry(SortableEntry::FACE_MIN,
					     box.Min(axis),
					     *fi)
			       );
    
    
    mSplitCandidates->push_back(SortableEntry(SortableEntry::FACE_MAX,
					     box.Max(axis),
					     *fi)
			       );
  }
  
  stable_sort(mSplitCandidates->begin(), mSplitCandidates->end());
}


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

  SortSplitCandidates(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

  int objectsLeft = 0, objectsRight = (int)node->mFaces.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;
  vector<SortableEntry>::const_iterator ci;

  for(ci = mSplitCandidates->begin();
      ci != mSplitCandidates->end();
      ci++) {
    switch ((*ci).type) {
    case SortableEntry::FACE_MIN:
      objectsLeft++;
      break;
    case SortableEntry::FACE_MAX:
      objectsRight--;
      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();
      
      //      cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
      //      cout<<"cost= "<<sum<<endl;
      
      if (sum < minSum) {
	minSum = sum;
	position = (*ci).value;
	
	objectsBack = objectsLeft;
	objectsFront = objectsRight;
      }
    }
  }
  
  float oldCost = (float)node->mFaces.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
MeshKdTree::CastRay(
					Ray &ray,
					MeshInstance *instance
					)
{
  int hits = 0;
  
  stack<RayTraversalData> tStack;
  
  float maxt = 1e6;
  float mint = 0;
  
  AxisAlignedBox3 box = GetBox();

  if (!box.GetMinMaxT(ray, &mint, &maxt))
    return 0;

  if (mint < 0)
    mint = 0;
  

  if (ray.GetType() == Ray::LOCAL_RAY &&
	  ray.intersections.size() && ray.intersections[0].mT < mint) {
	return 0;
  }
  
  maxt += Limits::Threshold;
  
  Vector3 entp = ray.Extrap(mint);
  Vector3 extp = ray.Extrap(maxt);
  
  MeshKdNode *node = mRoot;
  MeshKdNode *farChild;
  float position;
  int axis;

  while (1) {
    if (!node->IsLeaf()) {
      MeshKdInterior *in = (MeshKdInterior *) 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 - ray.GetLoc(axis)) / ray.GetDir(axis);
      tStack.push(RayTraversalData(farChild, extp, maxt));
      extp = ray.GetLoc() + ray.GetDir()*tdist;
      maxt = tdist;
    } else {
      // compute intersection with all objects in this leaf
      MeshKdLeaf *leaf = (MeshKdLeaf *) node;
      //      cout<<"leaf mfaces size="<<leaf->mFaces.size()<<endl<<flush;
      hits += instance->CastRay(ray, leaf->mFaces);
      
      if (ray.GetType() == Ray::LOCAL_RAY && ray.intersections.size())
				if (ray.intersections[0].mT <= maxt) {
					break;
				}
      
      // get the next node from the stack
      if (tStack.empty())
				break;
      
      entp = extp;
      mint = maxt;

      if (ray.GetType() == Ray::LINE_SEGMENT && mint > 1.0f)
				break;
			
      RayTraversalData &s  = tStack.top();
      node = s.mNode;
      extp = s.mExitPoint;
      maxt = s.mMaxT;
      tStack.pop();
    }
  }
  
  
  return hits;
}

bool
MeshKdTree::TerminationCriteriaMet(const MeshKdLeaf *leaf, const int depth)
{
  //  cerr<<"\n OBJECTS="<<leaf->mObjects.size()<<endl;
  return
    (leaf->mFaces.size() <= mTermMinCost) ||
    (depth >= mTermMaxDepth);
  
}

bool
MeshKdTree::Construct()
{
  if (!mSplitCandidates)
    mSplitCandidates = new vector<SortableEntry>;
  
  // first construct a leaf that will get subdivide
  MeshKdLeaf *leaf = (MeshKdLeaf *) mRoot;

  mRoot = Subdivide(TraversalData(leaf, NULL, GetBox(), 0));
  
  // remove the allocated array
  delete mSplitCandidates;
  mSplitCandidates = NULL;
  
  return true;
}


MeshKdNode *
MeshKdTree::Subdivide(const TraversalData &tdata)
{
  MeshKdNode *result = NULL;
  
  priority_queue<TraversalData> tStack;
  //  stack<STraversalData> tStack;
  
  tStack.push(tdata);
  AxisAlignedBox3 backBox, frontBox;

  
  while (!tStack.empty()) {

    TraversalData data = tStack.top();
    tStack.pop();
    
    MeshKdNode *node = SubdivideNode((MeshKdLeaf *) data.mNode,
				     data.mParent,
				     data.mBox,
				     data.mDepth,
				     backBox,
				     frontBox
				     );
    if (result == NULL)
      result = node;
    
    if (!node->IsLeaf()) {

      MeshKdInterior *interior = (MeshKdInterior *) node;
      // push the children on the stack
      tStack.push(TraversalData(interior->mBack, interior, backBox, data.mDepth+1));
      tStack.push(TraversalData(interior->mFront, interior, frontBox, data.mDepth+1));
      
    }
  }
  
  return result;

}

}