// ===================================================================
// $Id: $
//
// ktb.cpp
//
//  Implementation of basic functions to create kd-trees
//
// REPLACEMENT_STRING
//
// Copyright by Vlastimil Havran, 2006 - email to "vhavran AT seznam.cz"
// Initial coding by Vlasta Havran, 1998-2001.

// GOLEM library
#include "ktb.h"
#include "Environment.h"

// standard headers
#include <algorithm>

namespace GtpVisibilityPreprocessor {


//-------------------------------------------------------------
// class CKTBNodeIterator .. implementation

// test all objects in the leaf for intersection with ray
// and returns the pointer to closest one if exists
// and passing through parameter returns in tmax
int
CKTBNodeIterator::TestFullLeaf(const SimpleRay &ray, const SKTBNode *p)
{
  ObjectContainer *list = GetObjList(p);  
  if (!list) // no object 
    return 0;
  ObjectContainer::iterator sc_end = list->end();

  float tclosest = Limits::Infinity;
  int intersected = 0;
  // iterate the whole list and find out the nearest intersection
  for (ObjectContainer::iterator sc = list->begin(); sc != sc_end; sc++) {
    // if the intersection realy lies in the node        
    if ((*sc)->CastSimpleRay(ray) == Ray::INTERSECTION) {
      // update tclosest !!!!!!!!!!!!!!!!!!!!!!!!!!!!
      // tclosest = ray.
      intersected = 1;
    }
  } // for all objects

  return intersected;
}

int
CKTBNodeIterator::TestFullLeaf(const SimpleRay &ray, const SKTBNode *p,
			       int rayIndex)
{
  ObjectContainer *list = GetObjList(p);  
  if (!list) // no object 
    return 0;
  ObjectContainer::iterator sc_end = list->end();

  float tclosest = Limits::Infinity;
  int intersected = 0;
  // rayIndex += rayOffset;
  // iterate the whole list and find out the nearest intersection
  for (ObjectContainer::iterator sc = list->begin(); sc != sc_end; sc++) {
    // if the intersection realy lies in the node        
    if ((*sc)->CastSimpleRay(ray, rayIndex)) {
      // update tclosest !!!!!!!!!!!!!!!!!!!!!!!!!!!!
      // tclosest = ray.
      intersected = 1;
    }
  } // for all objects

  return intersected;
}

  
// test all objects in the leaf for intersection with ray
// and find any if exist .. returns this object, otherwise 0
int
CKTBNodeIterator::HitAnyObj(const SimpleRay &ray, const SKTBNode *p)
{
  ObjectContainer *list = GetObjList(p);  
  if (!list) // no object 
    return 0;
  ObjectContainer::iterator sc_end = list->end();

  float tclosest = Limits::Infinity;
  int intersected = 0;
  // iterate the whole list and find out the nearest intersection
  for (ObjectContainer::iterator sc = list->begin(); sc != sc_end; sc++) {
    Intersectable *is = (*sc);
    // if the intersection realy lies in the node        
    if (is->CastSimpleRay(ray)) {
      // update tclosest !!!!!!!!!!!!!!!!!!!!!!!!!!!!
      // tclosest = ray.
      return 1; // intersected
    }
  } // for all objects

  return 0;
}

const CKTBNodeAbstract::SKTBNode*
CKTBNodeIterator::Locate(const Vector3 & /*position*/)
{
  cerr << "Locate vector - Not yet implemented" << endl;
  return (SKTBNode*)0;
}

// ---------------------------------------------------------------------
// Allocator for KTB tree

// forget the content that is created by previous kd-tree construction
// or just init for the first use.
void
CKTBAllocMan::InitForConstructor()
{
#ifdef _KTB_CONSTR_STATS
  _stats_interiorCount = 0;
  _stats_bboxCount = 0;
  _stats_minbboxCount = 0;
  _stats_leafNodeCount = 0;
  _stats_emptyLeftNodeCount = 0;
  // Aggregate statistics
  _sumLeafDepth = 0;
  _sumFullLeafDepth = 0;
  // The count of object references in leaves
  _sumObjectRefCount = 0;
  // The maximum number of object references in a leaf
  _maxObjectRefInLeaf = 0;
  // surface areas
  _sumSurfaceAreaLeaves = 0.f;
  _sumSurfaceAreaMULcntLeaves = 0.f;
  _sumSurfaceAreaInteriorNodes = 0.f;
#endif
  
  // This is the statistics
  _currDepth = 0;
  _maxDepth = -1;
  InitPars();
}

// init the stack of auxiliary variables from min to max
void
CKTBAllocMan::InitAux(int /*min*/, int /*maxD*/, int maxItemsAtOnce)
{
  // The size of one entry
  int sizeEntryV = sizeof(SKTBNode);
  // The number of entries to be allocated at once in a block
  // (=size of the block)
  int numEntriesInBlock = 1024;
  // the allignment
  int allignEntrySizeV = sizeof(SKTBNode);
  int allignBlockSizeV = 128;

  // Create an allocator in DFS order
  alloc2 = new CAllocContinuous(sizeEntryV, numEntriesInBlock,
				maxItemsAtOnce,
				allignEntrySizeV,
			        allignBlockSizeV);
  assert(alloc2);
  // the first allocation is enabled by this command
  alloc2->AllocNewBlock();
  return;
}

// Read some basic parameters from the environment file
void
CKTBAllocMan::InitPars()
{
  Environment::GetSingleton()->GetIntValue("BSP.maxDepthAllowed",
					   maxDepthAllowed);
  Environment::GetSingleton()->GetIntValue("BSP.maxListLength",
					   maxListLength);
}

void
CKTBAllocMan::PostBuild()
{
  // Here it can be some postprocessing of the tree, such as branches
  // collapsing for the same content of leaves etc.
}

void
CKTBAllocMan::Remove()
{
  // Release the all memory by blocks, so all the interior nodes
  // and leaves representations. This should be fast.
  alloc2->ReleaseMemory();
}


// Create the representation of the interior node
SKTBNodeT*
CKTBAllocMan::AllocInteriorNode(int axis, float position,
				int cntLeft, int cntRight)
{
#ifdef _DEBUG
  nodeToLink = 0;
#endif

#ifdef _KTB_CONSTR_STATS
  _stats_interiorCount++;
#endif
  
  // Just to satisfy the compiler
  cntLeft = cntLeft;
  cntRight = cntRight;
  
  // Alloc a single node
  SKTBNodeT *n = (SKTBNodeT*)(alloc2->New(1));
  nodeToLink = n;
  if (n == 0) {
    // we have to insert a special node that links only
    nodeToLink = (SKTBNodeT*)alloc2->NewLastEntry(1);
    assert(nodeToLink);
    nodeToLink->nodeType = CKTBAxes::EE_Link;
    n = (SKTBNodeT*)(alloc2->NewEntryInNewBlock(1));
    // This is the link
    nodeToLink->right = n;    
  } // if n

  assert(n);
  assert(nodeToLink);

  // Set the interior node
  assert((axis >=0) && (axis < 3));
  n->nodeType = (CKTBAxes::Axes)axis;
  n->splitPlane = position;

  // Return the setupped node, but do not forget to
  // use in the parent node to use nodeToLink !!!!
  return n;
}

// Create the representation of the interior node
SKTBNodeT*
CKTBAllocMan::AllocInteriorNodeWithBox(int axis, float position,
				       int cntLeft, int cntRight,
				       const SBBox &tsbox,
				       SKTBNodeT* prevMinBoxNode,
				       int depthStore)
{
#ifdef _DEBUG
  nodeToLink = 0;
  if ( (position < tsbox.Min(axis)) ||
    (position > tsbox.Max(axis)) ) {
    cerr << "Something wrong with the tree axis = " << axis;
    cerr << " Min(axis) = " << tsbox.Min(axis)
	  << " splitValue = " << position
	  << " Max(axis) = " << tsbox.Max(axis) << endl;
    abort();
  }
#endif

#ifdef _KTB_CONSTR_STATS
  _stats_interiorCount++;
#endif
  
  // Just to satisfy the compiler
  cntLeft = cntLeft;
  cntRight = cntRight;

#ifdef _SHORT_FORM_MINBOX
  // Alloc a single node + node to store the pointer to box, in total 24 Bytes
  SKTBNodeT *n = (SKTBNodeT*)(alloc2->New(2));
  nodeToLink = n;
  if (n == 0) {
    // we have to insert a special node that links only
    nodeToLink = (SKTBNodeT*)alloc2->NewLastEntry(1);
    assert(nodeToLink);
    nodeToLink->nodeType = CKTBAxes::EE_Link;
    n = (SKTBNodeT*)(alloc2->NewEntryInNewBlock(2));
    // This is the link
    nodeToLink->right = n;    
  } // if n

  assert(n);
  assert(nodeToLink);

  // Set the interior node
  assert((axis >=0) && (axis < 3));
  n->nodeType = (CKTBAxes::Axes)(axis + (int)CKTBAxes::EE_X_axisBox);
  n->splitPlane = position;

  // Set the box itself
  // the address to the parent min box node
  (n+1)->parentBoxNode = prevMinBoxNode;

  // Here we simply allocate box to the address in the node
  SBBox *badr = new SBBox;
  assert(badr);
  (n+1)->minbox = badr;
  // and store the depth for debugging
  (n+1)->nodeType = CKTBAxes::Axes(depthStore);
  
#else // _SHORT_FORM_MINBOX
  // Alloc two single nodes (24 Bytes) + two nodes for box (24 Bytes) = 48 Bytes
  SKTBNodeT *n = (SKTBNodeT*)(alloc2->New(4));
  nodeToLink = n;
  if (n == 0) {
    // we have to insert a special node that links only
    nodeToLink = (SKTBNodeT*)alloc2->NewLastEntry(1);
    assert(nodeToLink);
    nodeToLink->nodeType = CKTBAxes::EE_Link;
    n = (SKTBNodeT*)(alloc2->NewEntryInNewBlock(4));
    // This is the link
    nodeToLink->right = n;    
  } // if n

  assert(n);
  assert(nodeToLink);

  // Set the interior node
  assert((axis >=0) && (axis < 3));
  n->nodeType = (CKTBAxes::Axes)(axis + (int)CKTBAxes::EE_X_axisBox);
  n->splitPlane = position;

  // Set the min box node itself

  // the address to the parent min box node
  (n+1)->parentBoxNode = prevMinBoxNode;
  // and store the depth for debugging
  (n+1)->nodeType = CKTBAxes::Axes(depthStore);
  (n+1)->minbox = 0; // only to make it zero for debugging
  
  // Here we simply allign to 48 Bytes, since we have one node of size 12 Bytes,
  // and SBBox takes 24 Bytes, so we just want to align to the boundary of 8 Bytes.
  SBBox *badr = (SBBox*)(((char*)n)+24);  
#endif // _SHORT_FORM_MINBOX

  // and copy the box content
  *(badr) = tsbox;
    
  // Return the set node, but do not forget to
  // use in the parent node to use nodeToLink !!!!
  return n;
}

// Set the pointers to children for the interior node
void
CKTBAllocMan::SetInteriorNodeLinks(SKTBNodeT *node,
				   SKTBNodeT *leftChild,
				   SKTBNodeT *rightChild)
{
  leftChild = leftChild; // to satisfy the compiler

  // Check on correctness of DFS order
  assert( (node+1 == leftChild) || (node+2 == leftChild) || (node+4 == leftChild) );
  
  node->right = rightChild;
}

// Set the pointers to children for the interior node
void
CKTBAllocMan::SetInteriorNodeLeft(SKTBNodeT *node,
				  SKTBNodeT *leftChild)
{
  leftChild = leftChild; // to satisfy the compiler

  // Check on correctness of DFS order
  assert( (node+1 == leftChild) || (node+2 == leftChild) || (node+4 == leftChild) );
}

// Set the pointers to children for the interior node
void
CKTBAllocMan::SetInteriorNodeRight(SKTBNodeT *node,
				   SKTBNodeT *rightChild)
{
  node->right = rightChild;
}

  
// Create the representation of the leaf. Note that possibly there
// can be special cases, such as 0, 1, 2, or 3 objects, or in general
// N objects.
SKTBNodeT*
CKTBAllocMan::AllocLeaf(int cntObjects)
{
#ifdef _DEBUG
  nodeToLink = 0;
#endif

#ifdef _KTB_CONSTR_STATS
  _stats_leafNodeCount++;
  _sumLeafDepth += _currDepth;
  if (cntObjects) {
    _sumFullLeafDepth += _currDepth;
    // The count of object references in leaves
    _sumObjectRefCount += cntObjects;
    // The maximum number of object references in a leaf
    if (cntObjects > _maxObjectRefInLeaf)
      _maxObjectRefInLeaf = cntObjects;
  }
  else
    _stats_emptyLeftNodeCount++;
#endif
  
  // Alloc a single node
  SKTBNodeT *n = (SKTBNodeT*)(alloc2->New(1));
  nodeToLink = n;
  if (n == 0) {
    // we have to insert a special node that links only
    n = nodeToLink = (SKTBNodeT*)alloc2->NewLastEntry(1);
    assert(nodeToLink);
    // Allocate a new block for the next allocation
    alloc2->AllocNewBlock();
  } // if n

  n->nodeType = CKTBAxes::EE_Leaf;
  n->objlist = 0;
  n->right = 0;
  
  // Return the node
  return n;
}

// if active node is empty, then is replaced by full leaf with
// the object list. In success returns 0, for failure returns 1.
// The object list is used as it is .. it is not copied !!
int
CKTBAllocMan::SetFullLeaf(SKTBNodeT *node, const ObjectContainer *objlist)
{
  assert(node);
  assert(node->nodeType == CKTBAxes::EE_Leaf);

  if ( (objlist == NULL) ||
       (objlist->size() == 0) ) {
    node->objlist = 0;
  }
  else {
    // Set the pointer to the list of objects
    node->objlist = (ObjectContainer *)objlist;
  }

  return 0;
}

} // namespace