// OctreeADF.cpp: implementation of the COctreeADF class.
//
//////////////////////////////////////////////////////////////////////

#include "OctreeADF.h"

// These shouldn't be here.  an OctreeADF shouldn't even be handling the itnerpolation of itself..
#include "CoonsSdfInterp.h"	//TEMP
#include "TrilinearSdfInterp.h" // TEMP

/* (Steve) moved to CoonsSdfInterp.cpp

float evalInterpolationSurface(float* d,float u,float v){
	float eval = (4*d[0] - 8*d[3] + 4*d[6] - 8*d[1] + 4*d[2] - 8*d[5] + 16*d[4] - 8*d[7] + 4*d[8])*u*u*v*v +
	     		 (-6*d[0] + 8*d[3] - 2*d[6] + 12*d[1]- 6*d[2] + 8*d[5] - 16*d[4] + 4*d[7] - 2*d[8])*u*u*v +
				 (-6*d[0] + 12*d[3] - 6*d[6] + 8*d[1] - 2*d[2] + 4*d[5] - 16*d[4] + 8*d[7] - 2*d[8])*u*v*v +
			     (9*d[0] - 12*d[3] + 3*d[6] - 12*d[1] + 3*d[2] - 4*d[5] + 16*d[4] - 4*d[7] + d[8])*u*v +
				 (2*d[0] - 4*d[1] + 2*d[2])*u*u + (2*d[0] - 4*d[3] + 2*d[6])*v*v  + (-3*d[0] + 4*d[1] - 1*d[2])*u + (-3*d[0] + 4*d[3] - 1*d[6])*v + d[0];  	
	return eval;
}
*/

octreeNode::octreeNode()
{
	octreeNode* newNode = this;
	
	if (newNode == NULL) {
		cerr << "Memory For New Octree Node Could Not Be Allocated.";
	}
	
	newNode->m_edgeLength = 0;
	newNode->inOnOut = 0;
	newNode->m_octant = Octant::ROOT;
	newNode->m_parent = NULL;
	newNode->m_depth = 0;

	newNode->m_c000id = 0;
	newNode->m_c001id = 0;
	newNode->m_c010id = 0;
	newNode->m_c011id = 0;
	newNode->m_c100id = 0;
	newNode->m_c101id = 0;
	newNode->m_c110id = 0;
	newNode->m_c111id = 0;
	
	for (int i = 0; i < 2; i += 1) {
		for (int j = 0; j < 2; j += 1) {
			for (int k = 0; k < 2; k += 1) {
				newNode->vtxArray[i][j][k] = NULL;
				newNode->subnodeArray[i][j][k] = NULL;
			}
		}
	}
}

octreeNode::~octreeNode(){

}

/* (Steve) See CoonsSdfInterp

///////////////////////////////////////////////////////////////////
// auxiliary evaluation functions
float octreeNode::evalR0(float u, float v){
	return evalInterpolationSurface(m_R0,u,v);
}

float octreeNode::evalR1(float u, float v){
	return evalInterpolationSurface(m_R1,u,v);
}

float octreeNode::evalP0(float v, float w){
	return evalInterpolationSurface(m_P0,v,w);
}

float octreeNode::evalP1(float v, float w){
	return evalInterpolationSurface(m_P1,v,w);
}

float octreeNode::evalQ0(float u, float w){
	return evalInterpolationSurface(m_Q0,u,w);
}

float octreeNode::evalQ1(float u, float w){
	return evalInterpolationSurface(m_Q1,u,w);
}

*/

unsigned octreeNode::reflect(Direction dir, Octant o){
	unsigned ret = octreeNode::ROOT;

	switch (dir) {
	case octreeNode::L:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::RFD;
			break;
		case octreeNode::RFD:
			ret = octreeNode::LFD;
			break;
		case octreeNode::LBD:
			ret = octreeNode::RBD;
			break;
		case octreeNode::RBD:
			ret = octreeNode::LBD;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::RFU;
			break;
		case octreeNode::RFU:
			ret = octreeNode::LFU;
			break;
		case octreeNode::LBU:
			ret = octreeNode::RBU;
			break;
		case octreeNode::RBU:
			ret = octreeNode::LBU;
			break;
		}
		break;

	case octreeNode::R:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::RFD;
			break;
		case octreeNode::RFD:
			ret = octreeNode::LFD;
			break;
		case octreeNode::LBD:
			ret = octreeNode::RBD;
			break;
		case octreeNode::RBD:
			ret = octreeNode::LBD;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::RFU;
			break;
		case octreeNode::RFU:
			ret = octreeNode::LFU;
			break;
		case octreeNode::LBU:
			ret = octreeNode::RBU;
			break;
		case octreeNode::RBU:
			ret = octreeNode::LBU;
			break;
		}
		break;
	
	case octreeNode::F:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::LBD;
			break;
		case octreeNode::RFD:
			ret = octreeNode::RBD;
			break;
		case octreeNode::LBD:
			ret = octreeNode::LFD;
			break;
		case octreeNode::RBD:
			ret = octreeNode::RFD;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::LBU;
			break;
		case octreeNode::RFU:
			ret = octreeNode::RBU;
			break;
		case octreeNode::LBU:
			ret = octreeNode::LFU;
			break;
		case octreeNode::RBU:
			ret = octreeNode::RFU;
			break;
		}
		break;

	case octreeNode::B:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::LBD;
			break;
		case octreeNode::RFD:
			ret = octreeNode::RBD;
			break;
		case octreeNode::LBD:
			ret = octreeNode::LFD;
			break;
		case octreeNode::RBD:
			ret = octreeNode::RFD;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::LBU;
			break;
		case octreeNode::RFU:
			ret = octreeNode::RBU;
			break;
		case octreeNode::LBU:
			ret = octreeNode::LFU;
			break;
		case octreeNode::RBU:
			ret = octreeNode::RFU;
			break;
		}
		break;

	case octreeNode::U:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::LFU;
			break;
		case octreeNode::RFD:
			ret = octreeNode::RFU;
			break;
		case octreeNode::LBD:
			ret = octreeNode::LBU;
			break;
		case octreeNode::RBD:
			ret = octreeNode::RBU;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::LFD;
			break;
		case octreeNode::RFU:
			ret = octreeNode::RFD;
			break;
		case octreeNode::LBU:
			ret = octreeNode::LBD;
			break;
		case octreeNode::RBU:
			ret = octreeNode::RBD;
			break;
		}
		break;

	case octreeNode::D:
		switch (o) {
		
		case octreeNode::LFD:
			ret = octreeNode::LFU;
			break;
		case octreeNode::RFD:
			ret = octreeNode::RFU;
			break;
		case octreeNode::LBD:
			ret = octreeNode::LBU;
			break;
		case octreeNode::RBD:
			ret = octreeNode::RBU;
			break;
	
		case octreeNode::LFU:
			ret = octreeNode::LFD;
			break;
		case octreeNode::RFU:
			ret = octreeNode::RFD;
			break;
		case octreeNode::LBU:
			ret = octreeNode::LBD;
			break;
		case octreeNode::RBU:
			ret = octreeNode::RBD;
			break;
		}
	break;
	default:
		ASSERT(FALSE);
		printf("Octree_node::reflect should never be here\n");
		break;
	}

	return ret;
}


bool octreeNode::adj(Direction d, Octant o)
{
	// For a given face in direction d and
	// a given octant, this returns true
	// if the octant is on that face.
	bool ret = false;

	switch (d) {

	case octreeNode::L:
		switch (o) {
		case octreeNode::LFD:
		case octreeNode::LBD:
		case octreeNode::LBU:
		case octreeNode::LFU:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;

	case octreeNode::R:
		switch (o) {
		case octreeNode::RFD:
		case octreeNode::RBD:
		case octreeNode::RBU:
		case octreeNode::RFU:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;

	case octreeNode::D:
		switch (o) {
		case octreeNode::LFD:
		case octreeNode::LBD:
		case octreeNode::RFD:
		case octreeNode::RBD:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;

	case octreeNode::U:
		switch (o) {
		case octreeNode::LFU:
		case octreeNode::RFU:
		case octreeNode::LBU:
		case octreeNode::RBU:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;

	case octreeNode::F:
		switch (o) {
		case octreeNode::LFU:
		case octreeNode::RFU:
		case octreeNode::LFD:
		case octreeNode::RFD:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;
	
	case octreeNode::B:
		switch (o) {
		case octreeNode::LBU:
		case octreeNode::RBU:
		case octreeNode::LBD:
		case octreeNode::RBD:
			ret = true;
			break;
		default:
			ret = false;
			break;
		}
		break;
	}
	return ret;
}

octreeNode *octreeNode::findNeighbor(octreeNode::Direction dir)
{
	std::vector<Octant> path;
	octreeNode *o = this;

	while (1) {
		ASSERT(o != NULL);
		path.push_back(o->m_octant);
		if (o->m_parent == NULL) {
			// o is the root node - it has no neighbors
			return NULL;
		} else {
			if (!adj(dir, o->m_octant)) {
				o = o->m_parent;
				break;
			} else {
				o = o->m_parent;
			}
		}
	}
	ASSERT(path.size() > 0);
	for (int i = path.size() - 1; i > -1; i--) {
		unsigned ro = reflect(dir, path[i]);
	//	printf("stange ro = %d\n",ro);
		ASSERT(ro < 8);
		ASSERT(o != NULL);
		unsigned x_i,y_i,z_i;

		// (Steve) I redid these
		x_i = (ro & 1) >> 0;
		y_i = (ro & 4) >> 2;
		z_i = 1 - ((ro & 2)>>1);
		/*
		Young's version:
		z_i = (ro & 4)>>2;
		y_i = (ro & 2)>>1;
		x_i = (ro & 1)>>0;
		*/
		if (o->m_depth == m_depth) {
			break;
		}
		if (o->subnodeArray[x_i][y_i][z_i] != NULL) {
			o = o->subnodeArray[x_i][y_i][z_i];
		} else {
			return o;
		}
	}
	return o;
}

/*
float octreeNode::findCOONSInterpDist(float u, float v, float w){
	float term1 = (1-u)*evalP0(v,w)+ u*evalP1(v,w)+ (1-v)*evalQ0(u,w)+ v*evalQ1(u,w) + (1-w)*evalR0(u,v)+ w*evalR1(u,v); 
	float term2 = (
			((this->vtxArray)[0][0][0])->dist * (1 - u) * (1 - v) * (1 - w) +
			((this->vtxArray)[1][0][0])->dist * u * (1 - v) * (1 - w) +
			((this->vtxArray)[0][1][0])->dist * (1 - u) * v * (1 - w) +
			((this->vtxArray)[0][0][1])->dist * (1 - u) * (1 - v) * w +
			((this->vtxArray)[1][0][1])->dist * u * (1 - v) * w +
			((this->vtxArray)[0][1][1])->dist * (1 - u) * v * w +
			((this->vtxArray)[1][1][0])->dist * u * v * (1 - w) +
			((this->vtxArray)[1][1][1])->dist * u * v * w
		   );
	return term1-2.0*term2;
}


void octreeNode::buildBoundarySurfaces()
{
	octreeNode* neighbor = findNeighbor(octreeNode::U);
	if(neighbor->isLeafNode() == FALSE){ //neighbor has children
		m_R1[0] = m_d001; m_R1[2] = m_d101; m_R1[6] = m_d011; m_R1[8] = m_d111;
		octreeNode* auxNode1 = neighbor->subnodeArray[0][0][0];
		octreeNode* auxNode2 = neighbor->subnodeArray[1][1][0];
		m_R1[1]= auxNode1->m_d100; m_R1[3] = auxNode1->m_d010; m_R1[4] = auxNode1->m_d110;
		m_R1[5]= auxNode2->m_d100; m_R1[7] = auxNode2->m_d010; 
	}

	else if(neighbor->m_edgeLength > m_edgeLength){ //neighbor is bigger
		octreeNode* auxNode1 = NULL;
		m_R1[0] = m_d001;
		switch (m_octant){
		case LFU:
			auxNode1 = m_parent->subnodeArray[1][1][1];
			m_R1[0] = neighbor->m_d000;   
			m_R1[1] = m_d101;
			m_R1[2]= neighbor->m_d100;
			m_R1[3] = m_d011; 
			m_R1[4] = m_d111; 
			m_R1[5] = auxNode1->m_d101;
			m_R1[6] = neighbor->m_d010;
			m_R1[7] = auxNode1->m_d011; 
			m_R1[8] = neighbor->m_d110;
			break;
		case RFU:
			auxNode1 = m_parent->subnodeArray[0][1][1];
			m_R1[0] = neighbor->m_d000;   
			m_R1[1] = m_d001; 
			m_R1[2]= neighbor->m_d100;
			m_R1[3] = auxNode1->m_d101;
			m_R1[4] = m_d101; 
			m_R1[5] = m_d111; 
			m_R1[6] = neighbor->m_d010;
			m_R1[7] = auxNode1->m_d111;
			m_R1[8] = neighbor->m_d110;
			break;
		case LBU:
			auxNode1 = m_parent->subnodeArray[1][0][1];
			m_R1[0] = neighbor->m_d000;   
			m_R1[1] = auxNode1->m_d001; 
			m_R1[2] = neighbor->m_d100;
			m_R1[3] = m_d001;
			m_R1[4] = m_d101; 
			m_R1[5] = auxNode1->m_d111; 
			m_R1[6] = neighbor->m_d010;
			m_R1[7] = m_d111;
			m_R1[8] = neighbor->m_d110;
			break;
		case RBU:
			auxNode1 = m_parent->subnodeArray[0][0][1];
			m_R1[0] = neighbor->m_d000;   
			m_R1[1] = auxNode1->m_d101; 
			m_R1[2] = neighbor->m_d100;
			m_R1[3] = auxNode1->m_d011;
			m_R1[4] = m_d001; 
			m_R1[5] = m_d101; 
			m_R1[6] = neighbor->m_d010;
			m_R1[7] = m_d011;
			m_R1[8] = neighbor->m_d110;
			break;
		default:
			break;
		}
	}

	else { //same size
		if(neighbor->m_parent == m_parent){
		m_R1[0] = m_d001; 
		m_R1[1] = (m_d001+ m_d101)/2.0;
		m_R1[2] = m_d101;
		m_R1[3] = (m_d101+m_d011)/2.0;
		m_R1[4] = (m_d001+m_d011+m_d101+m_d111)/4.0;
		m_R1[5] = (m_d101+m_d111)/2.0;
		m_R1[6] = m_d011;
		m_R1[7] = (m_d011+ m_d111)/2.0;
		m_R1[8] = m_d111;
		}




	}
}

*/
void octreeNode::render(float r, float g, float b,bool solid) {
	double halfLength = this->m_edgeLength / 2.0;
	Vector halfPoint;
	halfPoint.Set(halfLength, halfLength, halfLength);
	Point center = m_v + halfPoint;
	glPushMatrix();
	if(solid == false){ //draw a wire cube
		glPushAttrib(GL_LIGHTING_BIT);
		glDisable(GL_LIGHTING);
		glColor3f (r, g, b);
		glTranslatef ((GLfloat) center[0], (GLfloat) center[1], (GLfloat) center[2]);
		glutWireCube((GLdouble) this->m_edgeLength);
		glPopMatrix();
		glPopAttrib();
	}
	else{ //draw a solid cube
		glPushAttrib(GL_LIGHTING_BIT);
		glEnable(GL_LIGHTING);
		glTranslatef ((GLfloat) center[0], (GLfloat) center[1], (GLfloat) center[2]);
		glutSolidCube((GLdouble) this->m_edgeLength);
		glPopMatrix();
		glPopAttrib();
	}
}

bool octreeNode::isLeafNode()
{
	return this->subnodeArray[0][0][0] == NULL;
//	return (m_c000id == 0);
}

void octreeNode::subdivide_for_ADF(octreeVtx *child_corner_verts[3][3][3], const ISDF &reference_model)
{
	if( !this->isLeafNode() ) {
		printf(" ** Subdiving an already divided internal node..bad\n" );
		return;
	}

	octreeNode *node = this;

	// Were corner verts given?
	if( child_corner_verts == NULL )
	{
		child_corner_verts = new octreeVtx*[3][3][3];
		// Calculate em.
		octreeNode::calculate_child_corner_verts(node, reference_model, child_corner_verts);
	}

	// Create, init, and attach child nodes
	for(int i = 0; i < 2; i++)
	for(int j = 0; j < 2; j++)
	for(int k = 0; k < 2; k++)
	{
		octreeNode* child = new octreeNode;

		// Set the verts, using the computed ones given
		for(int l = 0; l < 2; l++)
		for(int m = 0; m < 2; m++)
		for(int n = 0; n < 2; n++)
		{
			child->vtxArray[l][m][n] = child_corner_verts[i + l][j + m][k + n];
		}
		
		child->m_edgeLength = (node->m_edgeLength)/2;
		child->m_octant = get_octant(i, j, k);
		child->m_parent = node;
		child->m_depth = node->m_depth + 1;

		// Attach the child to its parent
		node->subnodeArray[i][j][k] = child;
	}
}

void octreeNode::calculate_child_corner_verts(octreeNode *node, const ISDF &reference_model, octreeVtx *child_corner_verts[3][3][3] )
{
	int i, j, k;

	// Null the whole array out
	if( child_corner_verts == NULL )
	{
		ASSERT(false);
	}

	for( i = 0; i < 3; i++)
	for( j = 0; j < 3; j++)
	for( k = 0; k < 3; k++)
	{
		child_corner_verts[i][j][k] = NULL;
	}

	// First, re-use the outer corner pos from the parent node
	for( i = 0; i < 2; i++)
	for( j = 0; j < 2; j++)
	for( k = 0; k < 2; k++)
	{
		child_corner_verts[2 * i][2 * j][2 * k] = (node->vtxArray)[i][j][k];
	}

	// Now, for the remaining, set them to new ADF octree vertices
	Point *node_origin = ((node->vtxArray)[0][0][0])->v;
	const double halfLength = (node->m_edgeLength) / 2;
	for( i = 0; i < 3; i++)
	for( j = 0; j < 3; j++)
	for( k = 0; k < 3; k++)
	{
		if( child_corner_verts[i][j][k] == NULL )
		{
			// Figure out the position of this corner
			// Gotta be careful with the pointer arithmetic here..
			Vector corner_to_child(
				i * halfLength,
				j * halfLength,
				k * halfLength);
			Point *pt = new Point;
			(*pt) = (*node_origin) + corner_to_child;

			// Create an ADF octreeVtx for this point, based on
			// the reference object
			child_corner_verts[i][j][k] =
				octreeNode::create_ADF_octree_vert( pt, reference_model );
		}
	}

}

// Statics

void octreeNode::subdivide_octnode(octreeNode *node, octreeVtx *child_corner_verts[3][3][3], const ISDF &reference_model)
{
	printf(" ******** DONT USE THIS FUNCTION ******" );

	// Were corner verts given?
	if( child_corner_verts == NULL )
	{
		// Calculate em.
		octreeNode::calculate_child_corner_verts(node, reference_model, child_corner_verts);
	}

	// Create, init, and attach child nodes
	for(int i = 0; i < 2; i++)
	for(int j = 0; j < 2; j++)
	for(int k = 0; k < 2; k++)
	{
		octreeNode* child = new octreeNode;

		// Set the verts, using the computed ones given
		for(int l = 0; l < 2; l++)
		for(int m = 0; m < 2; m++)
		for(int n = 0; n < 2; n++)
		{
			child->vtxArray[l][m][n] = child_corner_verts[i + l][j + m][k + n];
		}
		
		child->m_edgeLength = (node->m_edgeLength)/2;

		// Attach the child to its parent
		node->subnodeArray[i][j][k] = child;
	}
}

octreeVtx* octreeNode::create_ADF_octree_vert(Point *p, const ISDF &reference_model)
{
	octreeVtx *vert = new octreeVtx;

	SDF::SdfValue sdf( reference_model.calc_sdf_from_pt(*p) );
	vert->dist = sdf.getSignedDistance();
	vert->v = p;
	vert->Pc = new Point(sdf.closest_point);
	vert->grad = new Vector(sdf.gradient);

	return vert;
}

bool octreeNode::all_child_corner_verts_meet_tolerance(octreeVtx *corners[3][3][3], octreeNode *parent )
{
	int i, j, k;
	for( i = 0; i < 3; i++)
	for( j = 0; j < 3; j++)
	for( k = 0; k < 3; k++)
	{
		if( FALSE == corner_vert_meets_tolerance( corners[i][j][k], parent ) )
		{
			return false;
		}
	}

	return true;
}

bool octreeNode::corner_vert_meets_tolerance(const octreeVtx *vert, octreeNode *node_to_interp_with)
{
	const double interped_dist =
		interped_distance(node_to_interp_with, vert->v);

#if USE_FIXED_DISTANCE_TOLERANCE
	return octreeNode::distance_meets_distance_tolerance(interped_dist, vert->dist);
#else
	return octreeNode::distance_meets_error_tolerance( interped_dist, vert->dist );
#endif

}

double octreeNode::interped_distance(octreeNode *node, const Point *pt)
{
	// TEMP - TODO - again, this should be abstracted out into an AdfGen algorithm class
	static CoonsSdfInterp interp;
	//static TrilinearSdfInterp interp;
	interp.setNode(node);
	return interp.interpolate(*pt);
}

int sign(float x);

bool octreeNode::distance_meets_error_tolerance(double interped_dist, double actual_dist)
{
	double deviation = Utils::abs_d( Utils::abs_d(actual_dist) - Utils::abs_d(interped_dist) );
	
	return (deviation < (octreeNode::adf_error_limit * Utils::abs_d(actual_dist)));
}

double octreeNode::adf_error_limit;
double octreeNode::adf_distance_tolerance;

octreeNode::Direction octreeNode::next_direction(Direction d)
{
	switch(d)
	{
	case L: return R;
	case R: return F;
	case F: return B;
	case B: return U;
	case U: return D;
	case D: return DIR_MAX;
	default: return L;
	}
}

double octreeNode::longest_diagonal_dist()
{
	Point p000 = *(this->vtxArray[0][0][0]->v);
	Point p111 = *(this->vtxArray[1][1][1]->v);
	
	// (Steve) this is kind of hacky..
	// We shouldn't need this..
	// UPDATE: Fixed our calculations, so we don't need this hack anymore
	/*	
	p000[3] = 1.0;
	p111[3] = 1.0;
	*/

	return (p111 - p000).Magnitude();
}

bool octreeNode::contains_surface()
{
	double max = this->longest_diagonal_dist();

	// If any vertex has a dist field of greater than the diagonal,
	// then we know there must be no surface within the object.
	for(int i = 0; i < 2; i++)
	for(int j = 0; j < 2; j++)
	for(int k = 0; k < 2; k++)
	{
		octreeVtx *v = this->vtxArray[i][j][k];

		// If this vertex has a distance field > diagonal..
		if( Utils::abs_d(v->dist) > max ) {
			return FALSE;
		}
	}

	// We didn't find any too-long vertices
	return TRUE;
}

void octreeNode::get_directions_for_diagonal(int i, Direction &da, Direction &db)
{
	switch(i)
	{
	case 0: da = U; db = L; break;
	case 1: da = U; db = R; break;
	case 2: da = U; db = F; break;
	case 3: da = U; db = B; break;
	case 4: da = D; db = L; break;
	case 5: da = D; db = R; break;
	case 6: da = D; db = F; break;
	case 7: da = D; db = B; break;
	case 8: da = R; db = F; break;
	case 9: da = R; db = B; break;
	case 10: da = L; db = F; break;
	case 11: da = L; db = B; break;
	default: return;
	}
}

octreeNode* octreeNode::get_edge_nbor_if_larger(Direction da, Direction db, octreeNode *a, octreeNode *b)
{
	// If either face nbor doesn't actually exist, then there is no
	// edge nbor.
	if( a == NULL || b == NULL ) return NULL;

	// If either face nbor is subdivided to a level
	// smaller than me, then the edge nbor cannot possibly
	// be larger.
	if( !a->isLeafNode()	// Is subdivided..
		&& a->subnodeArray[0][0][0]->m_edgeLength < this->m_edgeLength) {
		return NULL;
	}
	else if( !b->isLeafNode()	// Is subdivided..
		&& b->subnodeArray[0][0][0]->m_edgeLength < this->m_edgeLength) {
		return NULL;
	}
	else {
		// OK, it's possible that the edge nbor is larger.

		// From the face nbors, go in the appropriate direction
		// to look for the edge nbor in between.
		// The appropriate direction is always the direction of the
		// other face nbor.
		octreeNode *a_adj = a->findNeighbor(db);
		octreeNode *b_adj = b->findNeighbor(da);

		// If both yielded the same thing, it could be larger
		if( a_adj != NULL && a_adj == b_adj ) {
			// Only return it if it actually is larger
			if( a_adj->m_edgeLength > this->m_edgeLength ) {
				if (a_adj->isLeafNode() ) return a_adj;
			}
		}
		// The two did not match.
		// This means the edge node is actually split well,
		// so a and b found different child nodes.
		// So, it can't possibly be larger.
		// Fall thru
	}

	return NULL;
}

octreeNode* octreeNode::get_edge_nbor_if_larger(int diag_index, octreeNode *face_nbors[DIR_MAX])
{
	// Get the directions for the diagonal
	Direction da, db;
	get_directions_for_diagonal(diag_index, da, db);

	// Pass these to the real method
	return this->get_edge_nbor_if_larger(da, db, face_nbors[da], face_nbors[db]);
}

void octreeNode::get_face_nbors(octreeNode *face_nbors[DIR_MAX])
{
	// This doesn't quite work..this just returns the root node - why?

	for( Direction d = first_direction(); d != DIR_MAX; d = next_direction(d) )
	{
		face_nbors[d] = this->findNeighbor(d);
		octreeNode *nbor = face_nbors[d];

		// Check that the face nbor's length is no more than
		// twice our length.
		// Note: this is only true if we're balancing..so i'm just gonna print a warning
		// instead of crashing on assertion.
		if( face_nbors[d] != NULL) {
			if( FALSE == (face_nbors[d]->m_edgeLength <= this->m_edgeLength*2.5) )
			{
				printf("a neighbor is not balanced!\n");
			}
			// ASSERT(face_nbors[d]->m_edgeLength <= this->m_edgeLength*2.5);
		}
	}
}

octreeNode::Direction octreeNode::first_direction()
{
	return L;
}

void octreeNode::subdivide_for_balanced_ADF(octreeVtx *child_corner_verts[3][3][3], const ISDF &reference_model)
{	
	if( !this->isLeafNode() ) {
		printf(" ** Subdiving an already divided internal node..bad\n" );
		return;
	}

	// Get each face nbor
	octreeNode *face_nbors[DIR_MAX];
	for( Direction d = first_direction(); d != DIR_MAX; d = next_direction(d) )
		face_nbors[d] = NULL;

	this->get_face_nbors(face_nbors);

	// Using the face nbors, subdivide each larger edge nbor
	for( int edge = 0; edge < 12; edge++ )
	{
		octreeNode *larger_edge_nbor = this->get_edge_nbor_if_larger(edge, face_nbors);
		if( larger_edge_nbor != NULL && larger_edge_nbor->isLeafNode()) {
			// Subdivide it
			// Give it a NULL for corners, since we don't know
			larger_edge_nbor->subdivide_for_balanced_ADF(NULL, reference_model);

			ASSERT(larger_edge_nbor->m_edgeLength <= this->m_edgeLength*2.5);
		}
	}

	// Now, check each face nbor, subdividing them if they're
	// larger.
	for( d = first_direction(); d != DIR_MAX; d = next_direction(d) )
	{
		octreeNode *face_nbor = face_nbors[d];
		if( face_nbor != NULL
			&& face_nbor->isLeafNode()
			&& face_nbor->m_edgeLength > this->m_edgeLength) {
			// Subdivide it
			// Give it a NULL for corners, since we don't know
			face_nbor->subdivide_for_balanced_ADF(NULL, reference_model);
		}
	}

	// Now, actually subdivide this node
	this->subdivide_for_ADF(child_corner_verts, reference_model);
}


octreeNode::Octant octreeNode::get_octant(int i, int j, int k)
{
	if( i == 0 && j == 0 && k == 0 ) return Octant::LBD;
	else if( i == 0 && j == 0 && k == 1 ) return Octant::LFD;
	else if( i == 0 && j == 1 && k == 0 ) return Octant::LBU;
	else if( i == 0 && j == 1 && k == 1 ) return Octant::LFU;
	else if( i == 1 && j == 0 && k == 0 ) return Octant::RBD;
	else if( i == 1 && j == 0 && k == 1 ) return Octant::RFD;
	else if( i == 1 && j == 1 && k == 0 ) return Octant::RBU;
	else if( i == 1 && j == 1 && k == 1 ) return Octant::RFU;
	else {
		printf(" ** INVALID i,j,k given to get_octant: %d %d %d\n", i, j, k);
		return Octant::ROOT;
	}
}

octreeNode* octreeNode::create_child_node(int i, int j, int k)
{
	// TODO
	return NULL;
}

unsigned int octreeNode::count_offspring()
{
	if( this->isLeafNode() ) return 0;

	unsigned int num_offs = 8;	// We have 8 immediate children

	// Now add their offspring
	for(int i = 0; i < 2; i++)
	for(int j = 0; j < 2; j++)
	for(int k = 0; k < 2; k++)
	{
		// How many offspring does this child have?
		// Add it to our offspring count
		num_offs += this->subnodeArray[i][j][k]->count_offspring();
	}

	return num_offs;
}

bool octreeNode::is_balanced_with(octreeNode *other)
{
	// Returns true if this node's edge length is within
	// a factor of two of the other's.
	if( other->m_edgeLength < this->m_edgeLength )
	{
		return( this->m_edgeLength < other->m_edgeLength * 2.5 );
	}
	else if( other->m_edgeLength > this->m_edgeLength )
	{
		return( other->m_edgeLength < this->m_edgeLength * 2.5 );
	}
	else
	{
		// I guess they're equal..return true.
		return TRUE;
	}
}

octreeNode* octreeNode::find_edge_nbor_by_brute_force(int diag_index)
{
	Direction da, db;
	octreeNode::get_directions_for_diagonal(diag_index, da, db);

	// Try to find the two nbors that should surround the
	// desired edge nbor
	octreeNode *na = this->findNeighbor(da);
	octreeNode *nb = this->findNeighbor(db);

	// If either face nbor DNE, we have no edge nbor
	if( na == NULL || nb == NULL ) return NULL;

	// Now, we know we have the two face nbors
	ASSERT( na != NULL && nb != NULL );

	// For each face nbor, go in the direction of the edge nbor.
	// Which is, just the other face nbor's direction.
	octreeNode *adj_a = na->findNeighbor(db);
	octreeNode *adj_b = nb->findNeighbor(da);

	// Now, if the two face nbor's agree about the edge nbor,
	// we have one.  Otherwise, we don't really have one.
	// (B/c, the face nbor is actually spanning the edge)
	if( adj_a == adj_b ) return adj_a;
	else return NULL;

}

bool octreeNode::is_balanced_with_all_nbors()
{
	// If we're not balanced with any nbor,
	// return false.

	ASSERT(this->isLeafNode());	// This check should only be invoked for leaf nodes.

	// for each face nbor, check balance
	// Get each face nbor
	octreeNode *face_nbors[DIR_MAX];
	for( Direction d = first_direction(); d != DIR_MAX; d = next_direction(d) )
		face_nbors[d] = NULL;

	this->get_face_nbors(face_nbors);

	// Now, check each face nbor
	for( d = first_direction(); d != DIR_MAX; d = next_direction(d) )
	{
		octreeNode *face_nbor = face_nbors[d];
		if( face_nbor != NULL
			&& !this->is_balanced_with(face_nbor)) {
			// Not balanced!
			return FALSE;
		}
	}

	// for each edge nbor, check balance
	for( int edge = 0; edge < 12; edge++ )
	{
		octreeNode *edge_nbor = this->find_edge_nbor_by_brute_force(edge);
		if( edge_nbor != NULL
			&& !this->is_balanced_with(edge_nbor))
		{
			// Not balanced!
			return FALSE;
		}
	}

	return TRUE;

}

bool octreeNode::subtree_is_balanced()
{
	if( this->isLeafNode() )
	{
		// Base case.
		return this->is_balanced_with_all_nbors();
	}

	// This is an internal node.
	// If all its children are balanced, return true.
	// If any one aren't, return FALSE.
	for(int i = 0; i < 2; i++)
	for(int j = 0; j < 2; j++)
	for(int k = 0; k < 2; k++)
	{
		if( FALSE == this->subnodeArray[i][j][k]->subtree_is_balanced() )
			return FALSE;
	}

	return TRUE;
}

bool octreeNode::distance_meets_distance_tolerance(double interped_dist, double actual_dist)
{
	return Utils::abs_d(interped_dist - actual_dist) < octreeNode::adf_distance_tolerance;
}

octreeNode::octreeNode(const Point &origin, double edge_len)
{
	// TODO - not implemented yet
}


int octreeNode::calc_max_depth()
{
	int max_depth = 0;

	if( this->isLeafNode() ) {
		max_depth = 0;
	}
	else {

		// Find the max depth of all children
		int max_child_depth = -1;

		octreeNode *c = NULL;
		for (int i = 0; i < 2; i += 1)
		for (int j = 0; j < 2; j += 1)
		for (int k = 0; k < 2; k += 1)
		{
			c = this->subnodeArray[i][j][k];

			int curr_child_depth = c->calc_max_depth();

			if( curr_child_depth > max_child_depth )
				max_child_depth = curr_child_depth;
		}

		max_depth = max_child_depth + 1;
	}

	return max_depth;
}

bool octreeNode::check_corner_pts()
{
	// Just check if all corner pts have w = 1
	for (int i = 0; i < 2; i += 1)
	for (int j = 0; j < 2; j += 1)
	for (int k = 0; k < 2; k += 1)
	{
		Point *v = this->vtxArray[i][j][k]->v;
		if( (*v)(3) != 1.0 ) {

			printf("w was not 1.0: %f\n", (*v)(3));
			return FALSE;
		}
	}
			
	// Now check all children
	if( FALSE == this->isLeafNode() ) {
		for (int i = 0; i < 2; i += 1)
		for (int j = 0; j < 2; j += 1)
		for (int k = 0; k < 2; k += 1)
		{
			octreeNode *c = this->subnodeArray[i][j][k];
			if( FALSE == c->check_corner_pts() ) return FALSE;
		}
	}
			
	// All passed
	return TRUE;
}