// ===================================================================
// $Id: $
//
// ktbftrav.cpp
//
// class: CKTBTraversal
//
// REPLACEMENT_STRING
//
// Copyright by Vlastimil Havran, 2007 - email to "vhavran AT seznam.cz"
// Initial coding by Vlasta Havran, February 2007 (copy from kdrtrav.cpp)

// GOLEM headers
#include "ktbconf.h"
#include "ktbtrav.h"
#include "Intersectable.h"

namespace GtpVisibilityPreprocessor {

#ifdef TRV00F

// --------------------------------------------------------------
// Shooting a single ray without SSE
int
CKTBTraversal::FindNearestI(const SimpleRay &ray)
{
#if 0
  static int counter = 0;
  counter++;
  bool debug = false;
  if (counter == 530) {
    debug = true;
    cout << "COUNTER = " << counter << endl;
    cout << "DEBUG starts" << endl;    
  }
#endif
  
  // passing through parameters
  float tmin, tmax;
  SimpleRay::IntersectionRes[0].intersectable = 0;
  
  // test if the whole CKTB tree is missed by the input ray
  if ( (!root) ||
       (!bbox.ComputeMinMaxT(ray.mOrigin, ray.mDirection, &tmin, &tmax)) ||
       (tmax < tmin) ||
       (tmax <= 0.f) ) {
    return 0; // no object can be intersected
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0; 
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS

  Vector3 invertedDir;
  invertedDir.x = 1.0f / (ray.mDirection.x - 1e-25f);
  invertedDir.y = 1.0f / (ray.mDirection.y - 1e-25f);
  invertedDir.z = 1.0f / (ray.mDirection.z - 1e-25f);
  
  // start from the root node
  if (tmin < 0.f)
    tmin = 0.f;

  int index = 1;
  stack3[1].nodep = root;
  stack3[1].tmax = tmax;
  tmax = tmin;
  SKTBNodeT * childNodes[2];
  int RayDirs[3];
  RayDirs[0] = ray.mDirection.x < 0.f ? 1 : 0;
  RayDirs[1] = ray.mDirection.y < 0.f ? 1 : 0;
  RayDirs[2] = ray.mDirection.z < 0.f ? 1 : 0;
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (index) {
    register SKTBNodeT *currNode = stack3[index].nodep;
    tmin = tmax;
    tmax = stack3[index].tmax;
#if 0
    if (debug) {
      cout << "node = " << (void*)currNode
	   << " tmin = " << tmin << " tmax = " << tmax
	   << endl;
    }
#endif

CONTINUE_LINK:    

    assert(tmin <= tmax);
#ifdef __TRAVERSAL_STATISTICS
    allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS

    register const int nodeType = GetNodeType(currNode);

    // cout << " tmin = " << tmin << " tmax = " << tmax << " nodeType = " << (int)nodeType << endl;
    
    if (nodeType < CKTBAxes::EE_Leaf) {
      float tval = (GetSplitValue(currNode) - ray.mOrigin[nodeType]);
      tval *= invertedDir[nodeType];
      SKTBNodeT *near, *far;
      childNodes[0] = GetLeft(currNode);
      childNodes[1] = GetRight(currNode);
      int rayDir = RayDirs[nodeType];
      near = childNodes[rayDir];
      far = childNodes[rayDir ^ 0x1];
      
      stack3[index].nodep = far;
      // stack3[index].tmax = tmax; // not necessary, this is already there !
      // int c = tval < tmax ? 1 : 0;
      index += tval < tmax ? 1 : 0;
      stack3[index].nodep = near;
      stack3[index].tmax = Min(tval, tmax);
      tmax = tmin;
      // int d = tval < tmin ? -1 : 0;
      index += tval < tmin ? -1 : 0;
    }
    else {
      if (nodeType == CKTBAxes::EE_Leaf) {
	// test objects for intersection
#ifdef _DEBUGKTB
	cout << "Leaf " << endl;
	depth++;
#endif
#ifdef _DEBUGKTB
	DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
	      << " extp.t = " << extp->t << endl;
#endif
	if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
	  cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
	  fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
	  // test the objects in the full leaf against the ray
	  
	  SimpleRay::IntersectionRes[0].maxt =
	    stack3[index].tmax + Limits::Small;
#if 0
	  // using subroutine
	  if (TestFullLeaf(ray, currNode))
#else
	  // Avoiding function call by copying the code
	  const ObjectContainer * const list = GetObjList(currNode); 
	  int intersected = 0;
	  // iterate the whole list and find out the nearest intersection
	  ObjectContainer::const_iterator sc_end = list->end();
	  for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
	    // if the intersection realy lies in the node        
	    intersected += ((*sc)->CastSimpleRay(ray));
	  } // for all objects
	  if (intersected)
#endif
	  {
#ifdef _DEBUGKTB
	    cout << "Full leaf HIT " << endl; 
#endif 	
	      
#ifdef __TRAVERSAL_STATISTICS
	    _allNodesTraversed += allNodesTraversed;
	    _fullLeavesTraversed += fullLeavesTraversed;
	    _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS
	    
	    // signed distance should be already set in TestFullLeaf
	    // the first object intersected was found	
	    return 1;
	  }
	} // full leaf
#ifdef __TRAVERSAL_STATISTICS
	else {
#ifdef _DEBUGKTB
	  cout << "Empty leaf at depth= " << depth << endl; 
#endif      
	  emptyLeavesTraversed++;
	}
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
	cout << "Pop the node" << endl;
#endif      
    
	// pop farChild from the stack
	// restore the current values
	index--;
	continue;
      }
      else {
	assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
	cout << "Link " << endl;
#endif
	// cout << "Link node was accessed" << endl;
	currNode = GetLinkNode(currNode);
	goto CONTINUE_LINK;
      }
    }
  } // while current node is not the leaf

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return 0;
} // FindNearestI - single ray


// Shooting a single ray without SSE with prespecified box of the scene, assuming
// to be contained in the scene box!!!
int
CKTBTraversal::FindNearestI(const SimpleRay &oray, const AxisAlignedBox3 &localbox)
{
#if 0
  static int counter = 0;
  counter++;
  bool debug = false;
  if (counter == 530) {
    debug = true;
    cout << "COUNTER = " << counter << endl;
    cout << "DEBUG starts" << endl;    
  }
#endif
  
  // passing through parameters
  float tmin, tmax;
  SimpleRay::IntersectionRes[0].intersectable = 0;
  SimpleRay ray(oray.mOrigin, oray.mDirection, 0, 0.f, 0);
  
  // test if the whole CKTB tree is missed by the input ray
  if ( (!root) ||
       (!localbox.ComputeMinMaxT(ray.mOrigin, ray.mDirection, &tmin, &tmax)) ||
       (tmax <= tmin) ||
       (tmax <= 0.f) ) {
    return 0; // no object can be intersected
  }
  float tminOffset = 0.f;

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0; 
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS

  Vector3 invertedDir;
  invertedDir.x = 1.0f / (ray.mDirection.x - 1e-25f);
  invertedDir.y = 1.0f / (ray.mDirection.y - 1e-25f);
  invertedDir.z = 1.0f / (ray.mDirection.z - 1e-25f);
  
  // start from the root node
  if (tmin < 0.f)
    tmin = 0.f;
  else {
    // Here the origin of the ray is always zero - it starts
    // at the box face!!
    tminOffset = tmin;
    tmin = 0.f;
    tmax -= tminOffset;
    ray.mOrigin += ray.mDirection * tminOffset;
  }

  int index = 1;
  stack3[1].nodep = root;
  stack3[1].tmax = tmax;
  tmax = tmin;
  SKTBNodeT * childNodes[2];
  int RayDirs[3];
  RayDirs[0] = ray.mDirection.x < 0.f ? 1 : 0;
  RayDirs[1] = ray.mDirection.y < 0.f ? 1 : 0;
  RayDirs[2] = ray.mDirection.z < 0.f ? 1 : 0;
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (index) {
    register SKTBNodeT *currNode = stack3[index].nodep;
    tmin = tmax;
    tmax = stack3[index].tmax;
#if 0
    if (debug) {
      cout << "node = " << (void*)currNode
	   << " tmin = " << tmin << " tmax = " << tmax
	   << endl;
    }
#endif
        
CONTINUE_LINK:
    assert(tmin <= tmax);

#ifdef __TRAVERSAL_STATISTICS
    allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS

    register const int nodeType = GetNodeType(currNode);
    if (nodeType < CKTBAxes::EE_Leaf) {
      float tval = (GetSplitValue(currNode) - ray.mOrigin[nodeType]);
      tval *= invertedDir[nodeType];
      SKTBNodeT *near, *far;
      childNodes[0] = GetLeft(currNode);
      childNodes[1] = GetRight(currNode);
      int rayDir = RayDirs[nodeType];
      near = childNodes[rayDir];
      far = childNodes[rayDir ^ 0x1];
      
      stack3[index].nodep = far;
      // stack3[index].tmax = tmax; // not necessary, this is already there !
      // int c = tval < tmax ? 1 : 0;
      index += tval < tmax ? 1 : 0;
      stack3[index].nodep = near;
      stack3[index].tmax = Min(tval, tmax);
      tmax = tmin;
      // int d = tval < tmin ? -1 : 0;
      index += tval < tmin ? -1 : 0;
    }
    else {
      if (nodeType == CKTBAxes::EE_Leaf) {
	// test objects for intersection
#ifdef _DEBUGKTB
	cout << "Leaf " << endl;
	depth++;
#endif
#ifdef _DEBUGKTB
	DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
	      << " extp.t = " << extp->t << endl;
#endif
	if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
	  cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
	  fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
	  // test the objects in the full leaf against the ray
	  
	  SimpleRay::IntersectionRes[0].maxt =
	    stack3[index].tmax + Limits::Small;
#if 0
	  // using subroutine
	  if (TestFullLeaf(ray, currNode))
#else
	  // Avoiding function call by copying the code
	  const ObjectContainer * const list = GetObjList(currNode); 
	  int intersected = 0;
	  // iterate the whole list and find out the nearest intersection
	  ObjectContainer::const_iterator sc_end = list->end();
	  for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
	    // if the intersection realy lies in the node        
	    intersected += ((*sc)->CastSimpleRay(ray));
	  } // for all objects
	  if (intersected)
#endif
	  {
#ifdef _DEBUGKTB
	    cout << "Full leaf HIT " << endl; 
#endif 	
	      
#ifdef __TRAVERSAL_STATISTICS
	    _allNodesTraversed += allNodesTraversed;
	    _fullLeavesTraversed += fullLeavesTraversed;
	    _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

	    // We have to add the distance from the original ray origin
	    SimpleRay::IntersectionRes[0].tdist += tminOffset;
	    
	    // signed distance should be already set in TestFullLeaf
	    // the first object intersected was found	
	    return 1;
	  }
	} // full leaf
#ifdef __TRAVERSAL_STATISTICS
	else {
#ifdef _DEBUGKTB
	  cout << "Empty leaf at depth= " << depth << endl; 
#endif      
	  emptyLeavesTraversed++;
	}
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
	cout << "Pop the node" << endl;
#endif      
    
	// pop farChild from the stack
	// restore the current values
	index--;
	continue;
      }
      else {
	assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
	cout << "Link " << endl;
#endif
	// cout << "Link node was accessed" << endl;
	currNode = GetLinkNode(currNode);
	goto CONTINUE_LINK;
      }
    }
  } // while current node is not the leaf

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return 0;
} // FindNearestI - single ray


// Reasonably fast - about 101,500 rays per second for single dir!
// It allows fast switching context from one ray to the next ray so it is
// virtually independent of memory latency !
int
CKTBTraversal::FindNearestI_16oneDirNoSSE(SimpleRayContainer &rays, int offset)
{
  // passing through parameters
  int cntRays = 0;
  if (!root)
    return 0;

  // The auxiliary variables to be precomputed
  static float invertedDir[cntMaxRays * 4];
  static float rayOrig[cntMaxRays * 4];
  static int rayDirs[cntMaxRays * 4];
  static int indexRay[cntMaxRays];
  static int indexArray[cntMaxRays];
  static int indexStack[cntMaxRays];
  const int LOG2_MAX_HEIGHT = 5;
  const int MAX_HEIGHT = 1 << LOG2_MAX_HEIGHT;
  assert(MAX_HEIGHT == 32);
  static struct SStackElem3 stackA[cntMaxRays * MAX_HEIGHT];
  static float tmaxArray[cntMaxRays];
  int cntHits = 0;
  
  float tmin, tmax;
  for (int i = 0; i < cntMaxRays; i++) {
    // Setting zero intersection as original result
    SimpleRay::IntersectionRes[i+rayOffset].intersectable = 0;
    // test if the whole CKTB tree is missed by the input ray
    if ((!bbox.ComputeMinMaxT(rays[i+offset].mOrigin,
			      rays[i+offset].mDirection,
			      &tmin, &tmax)) ||
	(tmax <= tmin) ||
	(tmax <= 0.f) ) {
    }
    else {
      int indexR = (cntRays << 2);
      rayOrig[indexR + 0] = rays[i+offset].mOrigin.x;
      rayOrig[indexR + 1] = rays[i+offset].mOrigin.y;
      rayOrig[indexR + 2] = rays[i+offset].mOrigin.z;
      //rayOrig[indexR + 3] = 0.f;
      invertedDir[indexR + 0] = 1.0f / (rays[i+offset].mDirection.x - 1e-25f);
      invertedDir[indexR + 1] = 1.0f / (rays[i+offset].mDirection.y - 1e-25f);
      invertedDir[indexR + 2] = 1.0f / (rays[i+offset].mDirection.z - 1e-25f);
      //invertedDir[indexR + 2] = 0.f;
      rayDirs[indexR + 0] = rays[i+offset].mDirection.x < 0.f ? 1 : 0;
      rayDirs[indexR + 1] = rays[i+offset].mDirection.y < 0.f ? 1 : 0;
      rayDirs[indexR + 2] = rays[i+offset].mDirection.z < 0.f ? 1 : 0;
      //rayDirs[indexR + 3] = 0; 
      indexRay[cntRays] = i; // the index to the ray
      indexArray[cntRays] = cntRays; // the index to the array
      int indexS = (cntRays << LOG2_MAX_HEIGHT) + 1;
      indexStack[cntRays] = indexS; // the index in the stack
      stackA[indexS].nodep = root; // we start from the root
      stackA[indexS].tmax = tmax; // maximum distance
      if (tmin < 0.f) tmin = 0.f;
      tmaxArray[cntRays] = tmin;
      cntRays++;
    }
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0;
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS
  
  SKTBNodeT * childNodes[2];

#define PREF_DEFAULT _MM_HINT_T0
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (cntRays) {
    // we assume that all the nodes are interior nodes
    for (int i = 0; i < cntRays; i++) {
      // which indices to array should be used
      int indexA = indexArray[i];
      float tmin = tmaxArray[indexA];

      // the stack indexing is here
      int indexSA = indexStack[indexA];
      SKTBNodeT *currNode = stackA[indexSA].nodep;
      
#if 0
      if (debug) {
	cout << "node = " << (void*)currNode
	     << " tmin = " << tmin << " tmax = " << tmax
	     << endl;
      }
#endif

#if 0
      if (tmin > tmax) {
	cout << "PROBLEM tmin = " << tmin << " tmax = " << tmax;
	cout << endl;
      }
#endif      
    
#ifdef __TRAVERSAL_STATISTICS
      allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
      const unsigned int nodeType = (GetNodeType(currNode)) & 0x7;
      if (nodeType < CKTBAxes::EE_Leaf) {
	int indexRayOD = (indexA << 2) + nodeType;
	float tval = (GetSplitValue(currNode) - rayOrig[indexRayOD])
	  * invertedDir[indexRayOD];
	SKTBNodeT *near, *far;
	childNodes[0] = GetLeft(currNode);
	childNodes[1] = GetRight(currNode);
	int rayDir = rayDirs[indexRayOD];
	near = childNodes[rayDir];
	far = childNodes[rayDir ^ 0x1];
	
	stackA[indexSA].nodep = far; // store far node
	//stackA[indexSA].tmax = tmax; // with correct dist - the same as before
	float tmax = tmaxArray[indexA] = stackA[indexSA].tmax;
	int c = tval < tmax ? 1 : 0;
	indexSA += c;
	stackA[indexSA].nodep = near; // store near node
	stackA[indexSA].tmax = Min(tval, tmax); // with correct dist
	int d = tval < tmin ? 1 : 0;
	indexSA -= d;
	// This is prefetching - so the next time we have it
	// in the cache !
	GPREFETCH(stackA[indexSA].nodep, PREF_DEFAULT);
	// store tmax and index to the stack
	indexStack[indexA] = indexSA;
	tmaxArray[indexA] = tmin;	
      }
      else {
	if (nodeType == CKTBAxes::EE_Leaf) {
	  // test objects for intersection
#ifdef _DEBUGKTB
	  cout << "Leaf " << endl;
	  depth++;
#endif
#ifdef _DEBUGKTB
	  DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
		<< " extp.t = " << extp->t << endl;
#endif
	  float tmax = tmaxArray[indexA] = stackA[indexSA].tmax;
	  if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
	    cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
	    fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
	  // test the objects in the full leaf against the ray
	  
	    // which ray is processed
	    int indexR = indexRay[indexA];
	    SimpleRay::IntersectionRes[indexR + rayOffset].maxt =
	      tmax + Limits::Small;
	    if (TestFullLeaf(rays[indexR+offset], currNode, indexR)) {

	      // we remove the ray from the calculation
	      indexArray[i] = indexArray[cntRays-1];
	      cntRays--; // we decrease the number of rays
	      cntHits++;
#ifdef _DEBUGKTB
	      cout << "Full leaf HIT " << endl; 
#endif 	
	      
#ifdef __TRAVERSAL_STATISTICS
	      _allNodesTraversed += allNodesTraversed;
	      _fullLeavesTraversed += fullLeavesTraversed;
	      _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS	    
	    
	      // signed distance should be already set in TestFullLeaf
	      // the first object intersected was found	
	      continue;
	    }
	  } // full leaf
#ifdef __TRAVERSAL_STATISTICS
	  else {
#ifdef _DEBUGKTB
	    cout << "Empty leaf at depth= " << depth << endl; 
#endif      
	    emptyLeavesTraversed++;
	  }
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
	  cout << "Pop the node" << endl;
#endif      
    
	  // pop farChild from the stack
	  // restore the current values
	  --indexSA;
	  // This is bits 0,1,2,3,4,5 - the stack depth = 32 !
	  if ( (indexSA & 0x1f) == 0) {
	    // we remove the ray from the calculation
	    indexArray[i] = indexArray[cntRays-1];
	    cntRays--; // we decrease the number of rays
	  }
	  else {
	    indexStack[indexA] = indexSA;
	    // we prefetch the data to be accessible
	    // the next time
	    GPREFETCH(stackA[indexSA].nodep, PREF_DEFAULT);
	  }
	  continue;
	}
	else {
	  assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
	  cout << "Link " << endl;
#endif
	  stackA[indexSA].nodep = GetLinkNode(currNode);
	  GPREFETCH(stackA[indexSA].nodep, PREF_DEFAULT);
	  // cout << "Link node was accessed" << endl;
	  continue;
	}
      } // empty leaf or link
    } // for all active rays
  } // while cntRays

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return cntHits;
}

#endif //  TRV00F

} // namespace