#include <math.h>
#ifdef __OS_HP_UX__
#include <syslimits.h>
#else
#include <limits.h>
#endif
#include "ledsvertex.h"
#include "ledsedgeuse.h"
#include "vmerge.h"
#include "mathlib.h"


COctTree::COctTree()
{
  UINT_32 i,j,k;

  m_uiNumVerts = 0;
  for (i = 0; i<2; i++) {
    for (j = 0; j<2; j++) {
      for (k = 0; k<2; k++) {
		m_ppppoctDeeper[i][j][k] = NULL;
      }
    }
  }
  for (i = 0; i<MAX_VERTS; i++) {
	m_ppVerts[i] = NULL;
  }
}

COctTree::~COctTree()
{
  UINT_32 i,j,k;

  m_uiNumVerts = 0;
  for (i = 0; i<2; i++) {
    for (j = 0; j<2; j++) {
      for (k = 0; k<2; k++) {
		delete m_ppppoctDeeper[i][j][k];
      }
    }
  }
}


CEpsilonVtxMerger::CEpsilonVtxMerger()
{
  m_fEpsilon = 0.0;
  m_fLowx = FLT_MIN;
  m_fLowy = FLT_MIN;
  m_fLowz = FLT_MIN;
  m_fHighx = FLT_MAX;
  m_fHighy = FLT_MAX;
  m_fHighz = FLT_MAX;
  m_poctTree = NULL;
}

CEpsilonVtxMerger::~CEpsilonVtxMerger()
{
  m_fEpsilon = 0.0;
  m_fLowx = FLT_MIN;
  m_fLowy = FLT_MIN;
  m_fLowz = FLT_MIN;
  m_fHighx = FLT_MAX;
  m_fHighy = FLT_MAX;
  m_fHighz = FLT_MAX;
  delete m_poctTree;
}


void CEpsilonVtxMerger::Init(float fEpsilon, float lx, float ly, float lz, float hx, float hy, float hz)
{
  m_fEpsilon = fEpsilon;
  m_fLowx = lx;
  m_fLowy = ly;
  m_fLowz = lz;
  m_fHighx = hx;
  m_fHighy = hy;
  m_fHighz = hz;
}

// Make a CLEDSliteVertex out of the input point.
// If point is within epsilon of another stored here, 
// return a pointer to that one with new location averaged in.
CLEDSliteVertex *
CEpsilonVtxMerger::StoreVertex(CLEDSliteVertex *pVtx)
{
  CLEDSliteVertex *answer;

  answer = StorePoint(pVtx, &m_poctTree, 
					  m_fLowx, m_fLowy, m_fLowz, 
					  m_fHighx, m_fHighy, m_fHighz, NO_INSERT);
  if (answer == NULL) {
	return (StorePoint(pVtx, &m_poctTree, 
					   m_fLowx, m_fLowy, m_fLowz, 
					   m_fHighx, m_fHighy, m_fHighz, INSERT));
  }
  else
	return answer;
}


CLEDSliteVertex *
CEpsilonVtxMerger::ExistingVertex(CLEDSliteVertex *pVtx)
{
  return StorePoint(pVtx, &m_poctTree, 
					m_fLowx, m_fLowy, m_fLowz, 
					m_fHighx, m_fHighy, m_fHighz, NO_INSERT);
}

/* Continue to subdivide the space
 * into 8ths, putting the new points into the appropriate slot.
 * This makes it easy to find points later.
 */
CLEDSliteVertex *
CEpsilonVtxMerger::StorePoint(CLEDSliteVertex *pVtx,
							  COctTree **ppoctTree,
							  float lowx, float lowy, float lowz, 
							  float highx, float highy, float highz, 
							  UINT_32 insert)
{
  float halfx, halfy, halfz;
  UINT_32 i,j,k;
  UINT_32 v;
  float newlowx, newlowy, newlowz, newhighx, newhighy, newhighz;
  COctTree *poctNewTree;
  float x,y,z;
  CLEDSliteVertex *answer;
  
  halfx = (lowx+highx)/2.0f;
  halfy = (lowy+highy)/2.0f;
  halfz = (lowz+highz)/2.0f;

  x = pVtx->GetPoint()(X);
  y = pVtx->GetPoint()(Y);
  z = pVtx->GetPoint()(Z);

  // if it's null, make it a leaf containing our vertex 
  if ((*ppoctTree) == NULL) {
	if (insert == NO_INSERT) return(NULL);
	(*ppoctTree) =  new COctTree;
	(*ppoctTree)->m_ppVerts[0] = pVtx;
	(*ppoctTree)->m_uiNumVerts = 1;
	return(pVtx);
  }

  // if it's a leaf 
  if ((*ppoctTree)->m_uiNumVerts != 0) {
	// check to see if any existing points are close enough 
	//and return if so
	for (v = 0; v < (*ppoctTree)->m_uiNumVerts; v++) {
	  if (ClosePoints((*ppoctTree)->m_ppVerts[v]->GetPoint(), 
					  pVtx->GetPoint())) 
		return((*ppoctTree)->m_ppVerts[v]);
	}
	// if we don't want to insert it, we're done. 
	if (insert == NO_INSERT) return(NULL);
	// we need to insert it in the tree.  
	//If there's room, put it in this leaf 
	if ((*ppoctTree)->m_uiNumVerts < MAX_VERTS)  {
	  (*ppoctTree)->m_ppVerts[(*ppoctTree)->m_uiNumVerts++] = pVtx;
	  return(pVtx);
	}
	// otherwise, give it children and redistribute its vertices 
	else{
	  poctNewTree = new COctTree;
	  
	  for (v = 0; v < (*ppoctTree)->m_uiNumVerts; v++) {
		if ((*ppoctTree)->m_ppVerts[v]->GetPoint()(X) > halfx) 
		  i = 1; 
		else 
		  i = 0;
		if ((*ppoctTree)->m_ppVerts[v]->GetPoint()(Y) > halfy) 
		  j = 1; 
		else 
		  j = 0;
		if ((*ppoctTree)->m_ppVerts[v]->GetPoint()(Z) > halfz) 
		  k = 1; 
		else 
		  k = 0;

		if (poctNewTree->m_ppppoctDeeper[i][j][k] == NULL) {
		  poctNewTree->m_ppppoctDeeper[i][j][k] = new COctTree;
		}

		poctNewTree->m_ppppoctDeeper[i][j][k]->
		  m_ppVerts[poctNewTree->m_ppppoctDeeper[i][j][k]->m_uiNumVerts++] =
			(*ppoctTree)->m_ppVerts[v];
	  }
	  delete (*ppoctTree);
	  (*ppoctTree) = poctNewTree;
	}

  }// if it's a leaf

  // Only if insert==NO_INSERT (we are looking for a match), 
  //do we search everything.
  if (insert == NO_INSERT) {
	for (i = 0; i < 2; i++) {
	  if (i == 0) {
		if (x > (halfx+m_fEpsilon)) continue;
		newlowx = lowx;
		newhighx=halfx;
	  } else { 
		if (x <= (halfx-m_fEpsilon)) continue;
		newlowx = halfx;
		newhighx = highx;
	  }
	  for (j = 0; j < 2; j++) {
		if (j == 0) {
		  if (y > (halfy+m_fEpsilon)) continue;
		  newlowy = lowy;
		  newhighy = halfy;
		} else { 
		  if (y<=halfy-m_fEpsilon) continue;
		  newlowy = halfy;
		  newhighy = highy;
		}
		for (k = 0; k < 2; k++) {
		  if (k == 0) { 
			if (z>halfz+m_fEpsilon) continue;
			newlowz = lowz;
			newhighz = halfz;
		  } else {
			if (z<=halfz-m_fEpsilon) continue;
			newlowz = halfz;
			newhighz = highz;
		  }
		  answer = StorePoint(pVtx,
							  &((*ppoctTree)->m_ppppoctDeeper[i][j][k]),
							 newlowx, newlowy, newlowz, 
							 newhighx, newhighy, newhighz, NO_INSERT);
		  if (answer) return(answer);
		}
	  }
	}
	// We didn't find a match 
	return(NULL);
  }

  // we'll only get here if insert is true 
  if (x>halfx) {
	i = 1;
	newlowx = halfx;
	newhighx = highx;
  } else {
	i = 0;
	newlowx = lowx;
	newhighx = halfx;
  }
  if (y>halfy) {
	j = 1;
	newlowy = halfy;
	newhighy = highy;
  } else {
	j = 0;
	newlowy = lowy;
	newhighy = halfy;
  }
  if (z>halfz) {
	k = 1;
	newlowz = halfz;
	newhighz = highz;
  } else {
	k = 0;
	newlowz = lowz;
	newhighz = halfz;
  }
  // insert into the appropriate subtree 
  COctTree **ppoctNewTree = &((*ppoctTree)->m_ppppoctDeeper[i][j][k]);
  return(StorePoint(pVtx,
					ppoctNewTree,
					newlowx, newlowy, newlowz, 
					newhighx, newhighy, newhighz, INSERT));
}


UINT_32 CEpsilonVtxMerger::ClosePoints(const Point& Pt1, const Point& Pt2)
{
  float x1,y1,z1,x2,y2,z2;

  x1 = Pt1(X);
  y1 = Pt1(Y);
  z1 = Pt1(Z);
  x2 = Pt2(X);
  y2 = Pt2(Y);
  z2 = Pt2(Z);
  
  // use <= to make it work with epsilon = 0 
  if (x1-x2 <= m_fEpsilon && x2-x1 <= m_fEpsilon &&
	  y1-y2 <= m_fEpsilon && y2-y1 <= m_fEpsilon &&
	  z1-z2 <= m_fEpsilon && z2-z1 <= m_fEpsilon) 
	return (TRUE);

  return (FALSE);
}