// ConvexSkirtTriMeshSdfAlgorithm.cpp: implementation of the ConvexUmbrellaTriMeshSdfAlgorithm class.
//
//////////////////////////////////////////////////////////////////////

#include "ConvexUmbrellaTriMeshSdfAlgorithm.h"
using namespace SDF;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

ConvexUmbrellaTriMeshSdfAlgorithm::~ConvexUmbrellaTriMeshSdfAlgorithm()
{
	// Clear tri sets
	list<ClosestTriSet*>::iterator i;
	for( i = this->tri_sets.begin(); i != this->tri_sets.end(); ++i) {
		delete (*i);
	}
	this->tri_sets.clear();
}

void ConvexUmbrellaTriMeshSdfAlgorithm::start_new_computation(const Point &test_point)
{
	__DBa("**** new SDf comp *** test pt: " << test_point);

	// Clear tri sets
	list<ClosestTriSet*>::iterator i;
	for( i = this->tri_sets.begin(); i != this->tri_sets.end(); ++i) {
		delete (*i);
	}
	this->tri_sets.clear();

	this->test_point = test_point;
}


void ConvexUmbrellaTriMeshSdfAlgorithm::feed_triangle(const MeshTriangle &tri) {

	// Make sure to make a copy!  I had a mean pointer/object error here.
	MeshTriangle *mytri = new MeshTriangle(tri);
	__DBa("----------------");
	__DBa("eating tri: " << *mytri);

	// Get the SDF from just this triangle
	PointOnTriangle curr_pc( mytri->closest_point_from( this->test_point ) );
	__DBa("PC: " << curr_pc);

	// Calculate the distance of this point from test_point
	Vector pc2tp = (this->test_point - curr_pc.getPoint());
	//pc2tp[3] = 0.0f;	// (Steve) This isn't really a hack - we're just setting it zero..which it should be PRECISELY anyway
	assert(pc2tp(3) == 0.0f);
	const double curr_dist = pc2tp.Magnitude();
	__DBa("dist: " << curr_dist);

	list<ClosestTriSet*>::iterator i;

	//-----------------------
	//	Go through our tri sets and see if this one is obviously worse than one of them.
	//	Obviously worse meaning, it's at least some reasonable units further.
	//	If it is, we don't need to bother with this triangle.
	//	NOTE: If our list is empty, then this loop will never iterate, so we're good.
	//-----------------------
	for( i = this->tri_sets.begin(); i != this->tri_sets.end(); ++i) {
		if(curr_dist > ((*i)->abs_dist + REASONABLE_DISTANCE_CUT_OFF)) {
			// Ok, this triangle is obviously useless.
			// It has no chance of being chosen as the base set.
			// Just ignore it.
			delete mytri;	// Free up this useless triangle copy
			__DBa("\tdropping");
			return;
		}
	}

	//-----------------------
	//	Go through our tri sets and see if this triangle should be put
	//	in one of the sets.
	//-----------------------
	bool triAddedToExisting = false;
	
	for( i = this->tri_sets.begin(); i != this->tri_sets.end(); ++i) {
		if( (*i)->p.equal_to(curr_pc) ) {
			// Yep, this triangle's closest point is the same as some others.
			// It belongs to an existing set.
			// Add it.
			(*i)->tris.push_back(mytri);
			triAddedToExisting = true;

			//-----------------------
			//	Some checks
			//-----------------------
			assert(curr_pc.getType() != PointOnTriangle::POT_FACE);
			if(curr_pc.getType() == PointOnTriangle::POT_EDGE) {
				// We just added to an edge set.
				// We must have just 2 triangles now.
				assert((*i)->tris.size() == 2);
			}

			__DBa("\tadded tri to existing set: " << *i);
			break;
		}
	}

	if( !triAddedToExisting ) {
		// We couldn't find one to put this in.
		// Create one on its own.
		ClosestTriSet *set = new ClosestTriSet(curr_dist, curr_pc);
		set->tris.push_back(mytri);

		// Add to our sets!
		this->tri_sets.push_back(set);

		__DBa("\tcreating new set: " << set );
	}

}

SDF::SdfValue ConvexUmbrellaTriMeshSdfAlgorithm::compute_sdf_result()
{
	//-----------------------
	//	Preconditions
	//-----------------------
	if(this->tri_sets.size() == 0) {
		__ERROR__("ConvexUmbrellaTriMeshSdfAlgorithm::compute_sdf_result was called, but no triangles were given!");
		return SdfValue(0, Point(0,0,0), Vector(0,0,0), DONT_KNOW);
	}

	//-----------------------
	//	Find the min-dist set
	//-----------------------
	list<ClosestTriSet*>::iterator it;
	ClosestTriSet *classifying_set = NULL;
	double best_dist = 0.0;
	for(it = this->tri_sets.begin(); it != this->tri_sets.end(); ++it) {
		if( classifying_set == NULL || (*it)->abs_dist < best_dist)
		{
			// Got a better one
			classifying_set = *it;
			best_dist = (*it)->abs_dist;
		}
	}

	//__DBc("Using classifying set: " << *classifying_set);

	// Save some typing
	const ClosestTriSet *set = classifying_set;

	//---------------------------------
	//	Now _try_ to use this set to classify the point.
	//---------------------------------
	PointClass clazz = DONT_KNOW;

	if(set->abs_dist == 0.0f) {
		//-----------------------
		//	The distance is zero
		//	Just assume interior - it doesn't matter really
		//-----------------------
		clazz = INTERIOR;
	}
	else if(set->p.getType() == PointOnTriangle::POT_FACE) {
		//--------------------------------
		// The simple case where Pc is on the face of a triangle - not an edge or vertex
		//--------------------------------

		// We could just return DK if this happens..but this shouldn't with
		// the way we're currently comparing POTs
		assert(set->tris.size() == 1);

		/*
		if(set->p.getType() != PointOnTriangle::POT_FACE) {
			__DBb("triangle was in its own set, but was not a FACE point! " << 
				"\t set: " << (*set) <<
				"\t test pt: " << this->test_point <<
				"\t # of sets: " << this->tri_sets.size() );
				list<ClosestTriSet*>::iterator set_i = this->tri_sets.begin();
			__DBb("set 0: " << *(*set_i));
			set_i++;
			__DBb("set 1: " << *(*set_i));
		}
		// TODO this still fails a lot..
		assert(set->p.getType() == PointOnTriangle::POT_FACE);
		*/

		__DBa("Classifying by one tri (in its own face-set)");
		clazz = this->classify_pt_by_single_best_triangle(*set->tris.at(0), set->p.getPoint());

		// Classifying by a single tri should never fail
		assert(clazz != DONT_KNOW);
	}
	else if(set->p.getType() == PointOnTriangle::POT_EDGE) {
		//--------------------------------
		// Also a pretty simple case where Pc is on a shared edge
		//--------------------------------

		// But, we may have lost some triangles
		// If we did, we can't classify!
		if(set->tris.size() != 2) {
			__DBa("Co-edge set only had " << set->tris.size() << " triangles - assuming this is due to round-off error.  Failing classification.");
			clazz = DONT_KNOW;
		}
		else {
			// Otherwise, use it!
			__DBa("Classifying with a co-edge set (a pair of tris)");
			clazz = this->classify_pt_by_two_best_triangles(*set);
			assert(clazz != DONT_KNOW);	// This should never fail with a valid edge set
		}
	}
	else {
		// The Co-vertex case
		assert(set->p.getType() == PointOnTriangle::POT_VERTEX);

		//--------------------------------
		// The interesting case - Pc is a vertex shared by many triangles.
		// We shall call these triangles the umbrella triangles.
		//--------------------------------

		//--------------------------------
		// Check if any triangles are aligned with the perpendicular plane
		//	TODO - move this into the umbrella algo
		//--------------------------------
		Vector plane_norm = this->test_point - set->p.getPoint();
		const BOOL normSuccess = plane_norm.Normalize();
		assert(normSuccess);

		// TODO i'd like to use iterators here instead of index'ing..but it won't let me :(
		unsigned int tri_i;
		//for( tri_i = set->tris.begin(); tri_i != set->tris.end(); ++tri_i) {
		for(tri_i = 0; tri_i < set->tris.size(); ++tri_i) {
			//const double dotp = plane_norm.Dot((*tri_i)->norm);
			MeshTriangle* tri = set->tris.at(tri_i);
			const double dotp = plane_norm.Dot( tri->norm );
			if(dotp == 1.0f) {
				// Got one.
				// It's positive, so assume exterior
				clazz = EXTERIOR;
				break;
			}
			else if( dotp == -1.0f ) {
				clazz = INTERIOR;
				break;
			}
		}

		if(clazz == DONT_KNOW) {
			__DBa("We found no p-perp aligned tris.  Trying with the umbrella algo..");
			clazz =	classify_pt_by_umbrella_tris(
				set->tris,
				set->p.getPoint(),
				this->test_point );

			// The umbrella algo may return DK, if the vertex set is incomplete (ie. missing some tris)
		}
		else {
			__DBa("A Co-vertex set had a p-perp aligned triangle - used that to classify pt.");
		}
	}

	//--------------------------------
	// Now return a signed distance field according to the class
	//--------------------------------

	Vector grad = this->calc_gradient(*classifying_set);
	SdfValue sdf(classifying_set->abs_dist, classifying_set->p.getPoint(), grad, clazz);

	if(clazz != EXTERIOR && clazz != INTERIOR) {
		assert(sdf.dist_class == DONT_KNOW);
		//__LOG__("Could not classify point as in or out!  Blame round-off error - u should try again with a jittered pt.  Test point: " << this->test_point);
	}
	
	return sdf;
}

/*
	NOTE - this is the old, VERY Expensive gradient calc.
	This was thrown away in favor of the pseudo-normal approx.

Vector *StlSdfAble::calc_gradient(const Point &from) const
{
	Vector *grad = new Vector;

	// The two distance values for the nudged points
	double da, db;
	const double eps = this->gradient_calc_epsilon;
	const double double_eps = 2.0 * eps;

	Point nudged(from);

	// Nudge p in each dimension
	for(int i = 0; i < 3; i++) {
		
		// Nudge negative
		nudged[i] -= eps;
		da = this->calc_sdf_from_pt(nudged).getSignedDistance();

		// Nudge positive
		nudged[i] += double_eps;
		db = this->calc_sdf_from_pt(nudged).getSignedDistance();

		// Calculate the gradient
		(*grad)[i] = (db - da) / double_eps;

		// Un-nudge
		nudged[i] -= eps;
	}

	return grad;
}
*/


// Returns verts.size() if it couldn't find it
unsigned int find_vert( const VertexList &verts, const Point &v ) {
	unsigned int size = (unsigned int)verts.size();
	for(unsigned int i = 0; i < size; i++) {
		Point *p = verts[i];
		if( *p == v ) {
			return i;
		}
	}

	// Can't find it!
	return size;
}


SortedUmbrella *ConvexUmbrellaTriMeshSdfAlgorithm::
//--------------------------------
// T - the list of MeshTriangle's
// x - the point they ALL share.  This is assumed.
//--------------------------------
Sort_by_umbrella_verts(const TriangleList &T, const Point &x) {

	VertexList *verts = new VertexList;
	TriangleListList *pairs = new TriangleListList;

	// For each triangle..
	for(unsigned int it = 0; it < T.size(); it++) {
		MeshTriangle *t = T.at(it);

		// For each of its verts..
		for(int iv = 0; iv < 3; iv++ ) {
			Point vert = t->pts[iv];

			// (Steve) If two vert's are equal, they should be EXACTLY equal!
			// This is due to how STLs are loaded and what not.
			if( vert == x ) {
				// This is the shared vert.  Don't need to add to our sorted list.
				continue;
			}
			else {
				// This vertex is an outskirt vert
				// Is this vert already in our list?
				unsigned int vert_pos = find_vert(*verts, vert);

				if (vert_pos == verts->size()) {
					// It's not in the list yet.
					// Add this to the back.
					verts->push_back(new Point(vert));
					// Set the current vert position to the last element we just added
					vert_pos = verts->size() - 1;

					// Also create the incident triangles list
					pairs->push_back(new TriangleList);
				}

				// Now add this tri to the corresponding tri pair
				(*pairs)[vert_pos]->push_back(t);

				//--------------------------------
				// Check the size of the current tri list
				// If there are more than 2 tris..we have a bad situation!
				//--------------------------------
				if((*pairs)[vert_pos]->size() > 2) {
					__ERROR__("We found an umbrella where more than two triangles share an edge.	\
						This is currently not handled by our generator, so the ADF may not be accurate.  See the log for more info.  The program is in a unsafe state!!");
					return NULL;
				}
			}
		}
	}

	SortedUmbrella *out = new SortedUmbrella;
	out->SetFirst(verts);
	out->SetSecond(pairs);
	return out;
}

//-----------------------
//	This will return the ray (start, thru) intersection point with the plane.
//	If there is none (ie. the ray is || with the plane) this will return NULL.
//-----------------------
Point* calc_positive_ray_plane_intx(const Plane &plane, const Point &start, const Point &thru) {

	// Calculate the ray's direction
	Vector rayDir;
	rayDir = (thru - start);
	rayDir.Normalize();

	const double p_dot_n = plane.Dot(rayDir);	// Assuming this isn't 0

	if(p_dot_n == 0.0f) {
		// ray || to plane - no intersection
		return NULL;
	}

	// Calculate intx pt
	const double p_dot_start = plane.Dot(start);
	const double intxDist = -1 * p_dot_start / p_dot_n;

	//-----------------------
	//	Only return POSITIVE intx's
	//-----------------------
	if( intxDist < 0.0f ) {
		return NULL;
	}

	// Now calculate the intx pt
	Point* intxPt = new Point(start + (rayDir * intxDist));

	return intxPt;
}

VertexList* ConvexUmbrellaTriMeshSdfAlgorithm::
//--------------------------------
//	x - the point all umbrella-edges should start from
//	verts - the second pt's of each umbrella edge.  s, for-all v in verts, xv is an umbrella edge.
//	This returns a list projed, such that projed[i] is the intersection pt
//	of the ray (x, verts[i]) with plane.
//	IMPORTANT:  This will calculate only POSITIVE intersections!  And the rays it will do it for 
//	start from x through verts[i]!!  So if it does get an intx pt, the pts will be ordered
//	like so: x -> verts[i] -> intxPt
//--------------------------------
Project_umbrella_rays(const Point &x, const VertexList &verts, const Plane &plane)
{
	VertexList *projed = new VertexList;

	for(unsigned int iv = 0; iv < verts.size(); ++iv) {

		Point *v = verts[iv];
		Point *intx = NULL;

		// Find the intersection with the plane for this umbrella edge
		// The edge-ray starts from the umbrella origin vert
		intx = calc_positive_ray_plane_intx(plane, x, *v);
		
		// Add the point to the list
		projed->push_back(intx);
	}

	return projed;
}

ConvexUmbrellaTriMeshSdfAlgorithm::FoldType
ConvexUmbrellaTriMeshSdfAlgorithm::calc_fold_type(const MeshTriangle &a, const MeshTriangle &b)
{
	if( a.norm == b.norm ) {
		return FLAT;
	}
	else if(a.is_behind(b)) {
		return CONVEX;
	}
	else {
		return CONCAVE;
	}
}

SDF::PointClass
ConvexUmbrellaTriMeshSdfAlgorithm::
classify_pt_by_fold(const MeshTriangle &a, const MeshTriangle &b)
{
	FoldType type = calc_fold_type(a,b);
	switch( type ){
	case FLAT:
		// This shouldn't really happen, since anyone who calls this
		// should guarantee that the two tri's are not a flat fold.
		// But, this may happen just due to round-off error.  So, just return DONT_KNOW and let jitter.
		__DBc("CU giving DK for flat fold");
		return DONT_KNOW;
	case CONVEX: return EXTERIOR;
	case CONCAVE: return INTERIOR;
	default:
		// This should never happen with our umbrella algorithm
		cerr << "Unknown fold type: " << type << "\n";
		return DONT_KNOW;
	}
}

bool vert_less_than(const Point &a, const Point &b) {
	//-----------------------
	//	I could've written this as a short OR expression,
	//	but that would rely on short-circuiting.  Better to make it
	//	explicit in the code.
	//-----------------------
	if( a(X) < b(X) ){
		return true;
	} else if( a(X) > b(X) ) {
		return false;
	// X's are equal - try Y
	} else if( a(Y) < b(Y) ) {
		return true;
	} else if( a(Y) > b(Y) ) {
		return false;
	// Y's are equal - try Z!
	} else if( a(Z) < b(Z) ) {
		return true;
	} else if( a(Z) > b(Z) ) {
		return false;
	} else {
		// They're equal
		// So return false - a is not less than b
		return false;
	}
}

//-----------------------
//	Given a list of vertices, this returns the index of the minimum vertex.
//	By minimum, we mean the vertex that is vert_less_than all others.
//-----------------------
unsigned int find_min_vert(const VertexList &verts) {
	int ibest = 0;
	Point *best = verts.at(ibest);
	for(unsigned int i = 0; i < verts.size(); ++i) {
		Point *curr = verts.at(i);
		__DBa("find_min_vert: comparing " << *curr << " to current champ: " << *best);
		if( vert_less_than(*curr, *best) ) {
			// We got a better one!
			ibest = i;
			best = curr;
			__DBa(*best << " is the NEW champ!");
		}
		else {
			__DBa(*best << " is still the champ..");
		}
	}

	return ibest;
}

bool sorted_umb_is_complete(SortedUmbrella *umb) {
	TriangleListList *umb_tri_lists = umb->GetSecond();

	// Each entry should have exactly two tris
	TriangleListList::iterator i;
	for(i = umb_tri_lists->begin(); i != umb_tri_lists->end(); ++i) {
		if( (*i)->size() != 2) {
			return false;
		}
	}

	return true;
}

int furthest_from(const VertexList &verts, const Point &from) {
	int ibest = 0;
	FLOAT best_dist = 0.0;
	
	ibest = 0;
	best_dist = Distance(*verts.at(ibest), from);

	for(unsigned int i = 1; i < verts.size(); ++i) {
		FLOAT curr_dist = Distance(*verts.at(i), from);

		if( curr_dist > best_dist ) {
			// We got a better one!
			ibest = i;
			best_dist = curr_dist;
		}
		else {
		}
	}

	return ibest;
}

template <typename Pointed>
void safeDeletePointersInVector(vector<Pointed*> &v) {
	vector<Pointed>::iterator() itr;
	for(itr = v.begin(); itr != v.end(); ++itr) {
		Pointed *ptr = *itr;
		if(ptr != NULL) {
			delete ptr;
		}
	}
}

void deleteVertexList(VertexList *v) {
	VertexList::iterator itr;
	for(itr = v->begin(); itr != v->end(); ++itr) {
		Point *ptr = *itr;
		if(ptr != NULL) {
			delete ptr;
		}
	}

	delete v;
}

void deleteTriangleList(TriangleList *v) {
	TriangleList::iterator itr;
	for(itr = v->begin(); itr != v->end(); ++itr) {
		MeshTriangle *ptr = *itr;
		if(ptr != NULL) {
			delete ptr;
		}
	}

	delete v;
}

void deleteSortedUmbrella(SortedUmbrella *umb) {
	VertexList *umb_verts = umb->GetFirst();
	TriangleListList *umb_tri_lists = umb->GetSecond();

	deleteVertexList(umb_verts);

	// Go through and free each tri list
	TriangleListList::iterator tllItr;
	for(tllItr = umb_tri_lists->begin();
		tllItr != umb_tri_lists->end();
		++tllItr)
	{
		TriangleList *l = *tllItr;
		if(l != NULL) {
			// Note - don't delete the triangles
			// We don't own them.
			delete l;
		}
	}

	delete umb_tri_lists;

	delete umb;
}

//--------------------------------
// x - the closest point on the surface
// p - the pt to classify
// tris - the tris that share that closest pt
//--------------------------------
SDF::PointClass
ConvexUmbrellaTriMeshSdfAlgorithm::
classify_pt_by_umbrella_tris(const TriangleList &tris, const Point &x, const Point &p)
{
	//--------------------------------
	// Calculate the plane to project onto - p perp
	//--------------------------------
	Plane pPerp;
	Vector x2p = p - x;
	BOOL normSuccess = x2p.Normalize();
	assert(normSuccess);

	// First find our umbrella verts by sorting\
	// Note - we must free up this structure later!
	SortedUmbrella *umb = Sort_by_umbrella_verts(tris, x);

	if(umb == NULL) {
		__ERROR__("sort_by_umbrella_verts returned NULL..not good.");
		return DONT_KNOW;
	}

	//---------------------------------
	//	Also check that the sorted umbrella is complete
	//	IE. every umbrella vert has exactly 2 tris attached to it.
	//---------------------------------
	if( !sorted_umb_is_complete(umb) ) {
		__DBc("Sorted umbrella was not complete - return DONT_KNOW.  We should try again with a jittered point.");
		deleteSortedUmbrella(umb);
		return DONT_KNOW;
	}

	//debug_print_sorted_umb(*umb);

	// Get the sorted lists
	VertexList *umb_verts = umb->GetFirst();
	TriangleListList *umb_tri_lists = umb->GetSecond();

	//---------------------------------
	//	Calculate pPerp
	//---------------------------------
	// Make pPerp face p and be positioned reasonably far from x.
	// Plane.Set takes the normal, and then a point on the plane.

	// Take the umbrella vertex that's furthest from x.
	int i_furthest = furthest_from(*umb_verts, x);
	pPerp.Set(x2p, *umb_verts->at(i_furthest));

	__DBa("p-perp: " << pPerp);

	//--------------------------------
	// Now project all umbrella edges onto pPerp
	//--------------------------------
	VertexList *projed = Project_umbrella_rays(x, *umb_verts, pPerp);

	//--------------------------------
	// Now look for e* and classify!
	//--------------------------------

	// The pair/fold corresponding to e*
	TriangleList *classifyingPair = NULL;

	// The following state machine goes through each projected umbrella
	// point, looking for a fold which we can use to classify the point.
	//	A flat fold can never be used, so it ignores all flat folds.
	//	Also, an edge aligned with pPerp is automatically used if encountered.
	//	In general, it finds the minimum vertex in component-sorted order,
	//	and stores the corresponding fold in classifyingPair.
	enum {ESS_NO_CAND, ESS_HAVE_CAND} e_star_state = ESS_NO_CAND;

	VertexList::iterator projItr;
	TriangleListList::iterator trisItr;
	Point *minProjedPoint = NULL;

	for(projItr = projed->begin(),	trisItr = umb_tri_lists->begin();
		projItr != projed->end() &&	trisItr != umb_tri_lists->end();
		++projItr,					++trisItr)
	{
		// Calculate input signals
		const bool pPerpAligned = (*projItr == NULL);

		TriangleList *pair = *trisItr;
		MeshTriangle *a = pair->at(0);
		MeshTriangle *b = pair->at(1);

		bool flatFold = (a->norm == b->norm);

		switch(e_star_state) {
		case ESS_NO_CAND:
			if(!flatFold) {
				if(pPerpAligned) {
					// We got a good one.  Just use it.
					classifyingPair = pair;
					break;
				}
				else {
					// This is our first possible candidate for the min-vert
					minProjedPoint = *projItr;
					classifyingPair = pair;
					// update state
					e_star_state = ESS_HAVE_CAND;
				}
			}
			// If it's a flat fold, ignore it no matter what
			break;

		case ESS_HAVE_CAND:
			if(!flatFold) {
				if(pPerpAligned) {
					// We got a good one.  Use it.
					classifyingPair = pair;
					break;
				}
				else {
					// We got another possible min vert.  Test it
					Point *curr = *projItr;
					if(vert_less_than(*curr, *minProjedPoint)) {
						// This one is better!
						minProjedPoint = curr;
						classifyingPair = pair;
					}			
				}
			}
			break;
		}
	}

	// Make sure we got something!
	if(classifyingPair == NULL) {
		__ERROR__("We couldn't find a classifying pair/fold for the test point: " << p << ".  This means we couldn't find an extreme pt, or all the folds were flat!");
		// TODO - print all state info here, like umb tris/verts etc.
	}



	// This block spits out everything about the current computation state.
	// Uncomment if needed for debugging.
	/*
	{
		__DBa("all umb tris:");
		int dbi;
		for(dbi = 0; dbi < tris.size(); dbi++ ) {
			__DBa("\t" << *tris.at(dbi));
		}

		__DBa("all projed pts / umb verts:");
		for(dbi = 0; dbi < projed->size(); dbi++ ) {
			Point* projd = projed->at(dbi);
			if(projd != NULL) {
				__DBa("\t" << *projed->at(dbi) << "\t" << *umb_verts->at(dbi));
			}
			else {
				__DBa("\t" << "NULL" << "\t" << *umb_verts->at(dbi));
			}
			__DBa("\t\ttris on this umb_vert:");
			TriangleList *sharingTris = umb_tri_lists->at(dbi);
			for(int iTri = 0; iTri < sharingTris->size(); iTri++ ) {
				__DBa("\t\t" << *sharingTris->at(iTri));
			}
		}
	}
	*/

	PointClass clazz;

	if(classifyingPair->size() != 2)
	{
		//-----------------------
		//	This is not a valid edge-set.  It must've lost some triangles to round-off error.
		//	We can't use this to accurately classify the point.
		//	Just return DONT_KNOW
		//-----------------------

		__DBc("found an umbrella vert, but it had " << classifyingPair->size() << " tris sharing it - this isn't a valid co-vertex tri set");
		clazz = DONT_KNOW;
	}
	else {
		// Classify!
		clazz = classify_pt_by_fold(
			*classifyingPair->at(0),
			*classifyingPair->at(1));

		__DBa("classified by two tris:");
		__DBa("tri A:");
		__DBa(*classifyingPair->at(0));
		__DBa("tri B:");
		__DBa(*classifyingPair->at(1));

		// If 
		if(clazz == DONT_KNOW) {
			__DBa("We classified by a fold but got DONT_KNOW.  So the fold was flat - probably due to round-off error.  jitter and try again."
				<< " Test pt: " << p );

			unsigned int dbi;
			__DBa("  all umb tris:");
			for(dbi = 0; dbi < tris.size(); ++dbi) {
				__DBa(*tris.at(dbi));
			}

			__DBa("  umb verts:");
			for(dbi = 0; dbi < umb_verts->size(); dbi++ ) {
				__DBa(*umb_verts->at(dbi));
			}

			__DBa("all projed pts / umb verts:");
			for(dbi = 0; dbi < projed->size(); dbi++ ) {
				Point* projd = projed->at(dbi);
				if(projd != NULL) {
					__DBa("\t" << *projed->at(dbi) << "\t" << *umb_verts->at(dbi));
				}
				else {
					__DBa("\t" << "NULL" << "\t" << *umb_verts->at(dbi));
				}
				__DBa("\t\ttris on this umb_vert:");
				TriangleList *sharingTris = umb_tri_lists->at(dbi);
				for(unsigned int iTri = 0; iTri < sharingTris->size(); ++iTri) {
					__DBa("\t\t" << *sharingTris->at(iTri));
				}
			}
		}
	}

	// De-init
	deleteVertexList(projed);
	deleteSortedUmbrella(umb);

	return clazz;
}

void ConvexUmbrellaTriMeshSdfAlgorithm::Test_find_vert()
{
	Utils::shout("---- Running Test_find_vert --\n");

	VertexList *verts = new VertexList();
	verts->push_back(new Point(0, 0, 2));
	verts->push_back(new Point(-2, 2, -2));
	verts->push_back(new Point(2, -2, 1));
	verts->push_back(new Point(0, 0, 0));
	verts->push_back(new Point(0, 0, 0));
	verts->push_back(new Point(0, 0, 0));

	unsigned int found = -1;

	found = find_vert(*verts, Point(2, -2, 1));
	Utils::Assert(found == 2);

	found = find_vert(*verts, Point(-2, 2, -2));
	Utils::Assert(found == 1);

	// Utils::shout("-- OK Test_find_vert passed --\n");
}

void ConvexUmbrellaTriMeshSdfAlgorithm::Test_sort_umbrella()
{
	Utils::shout("---- Running Test_sort_umbrella --\n");

	Point shared(0, 10, 0);
	TriangleList tris;
	tris.push_back(
		MeshTriangle::create_test_tri(
			shared,
			1, 0, 1,
			1, 0, -1));
	tris.push_back(
		MeshTriangle::create_test_tri(
			shared,
			1, 0, -1,
			-1, 0, -1));
	tris.push_back(
		MeshTriangle::create_test_tri(
			shared,
			-1, 0, -1,
			-1, 0, 1));
	tris.push_back(
		MeshTriangle::create_test_tri(
			shared,
			-1, 0, 1,
			1, 0, 1));

	SortedUmbrella *umb = Sort_by_umbrella_verts(tris, shared);

	VertexList umbVerts = *umb->GetFirst();
	TriangleListList triLists = *umb->GetSecond();
	Utils::Assert(umbVerts.size() == 4);
	Utils::Assert(triLists.size() == 4);

	for(unsigned int i = 0; i < triLists.size(); ++i) {
		Utils::Assert(triLists.at(i)->size() == 2);
	}
}

void ConvexUmbrellaTriMeshSdfAlgorithm::Test_all()
{
	Utils::shout("-- Running all ConvexUmbrellaTriMeshSdfAlgorithm unit tests -- \n");

	Test_sort_umbrella();
	Test_find_vert();

	Utils::shout("-- OK All ConvexUmbrellaTriMeshSdfAlgorithm unit tests passed! \n");
}



SDF::PointClass
ConvexUmbrellaTriMeshSdfAlgorithm::classify_pt_by_single_best_triangle(const MeshTriangle &tri, const Point &pc )
{
	// There is only one triangle in the list.
	// So, just look at the t-x vector compared to its normal.
	Vector x2t = this->test_point - pc;
	assert(x2t(3) == 0.0f);

	double dotprod = tri.norm.Dot(x2t);

	// NOTE - if it's on the plane, we consider it interior
	if(dotprod > 0.0) {
		return EXTERIOR;
	}
	else {
		return INTERIOR;
	}
}

PointClass
ConvexUmbrellaTriMeshSdfAlgorithm::classify_pt_by_two_best_triangles(const ClosestTriSet &set)
{
	/** This is my method. I shouldn't be relying on classify by fold - it's logically different
	classify by fold should ONLY be used in the umbrella case, when we have a fold that's on the
	convex umbrella

	assert(this->best_tris.size() == 2);

	PointClass clazz = classify_pt_by_fold(
		*this->best_tris.at(0),
		*this->best_tris.at(1));

	if( clazz == DONT_KNOW ) {
		// The fold must've been flat.
		// Then just classify by any one of the tris!
		// After all, these are the two best - meaning they share an edge
		// and the closest pt is on that edge.
		return classify_pt_by_single_best_triangle( *this->best_tris.at(0) );
	}
	else {
		return clazz;
	}
	*/

	/*
	This uses the average pseudo-normal.  it's one way to do it.
	*/

	const Vector x2t = this->test_point - set.p.getPoint();
	MeshTriangle *a = set.tris.at(0);
	MeshTriangle *b = set.tris.at(1);
	Vector avgNorm = (a->norm + b->norm) * 0.5;
	double dotprod = avgNorm.Dot(x2t);

	// NOTE - if it's on the plane, we consider it interior
	if(dotprod > 0.0) {
		return EXTERIOR;
	}
	else {
		return INTERIOR;
	}
}

//---------------------------------
//	For a given triangle, this returns the angle at the given vertex 'at'.
//	So for a right triangle, one of the angles would be 90.
//	Returns angle in radians
//---------------------------------
double incident_angle(const MeshTriangle &tri, const Point &at)
{
	enum {START, HAVE_FIRST, HAVE_SECOND_DONE} state = START;
	Vector l1, l2;
	int i = 0;
	for(i = 0; i < 3; i++) {
		if(at == tri.pts[i]) {
			continue;
		}

		// It's a leg vector
		if(state == START) {
			// This is the first leg vector
			l1 = tri.pts[i] - at;
			state = HAVE_FIRST;
		}
		else if(state == HAVE_FIRST) {
			// This must be the second leg vector
			l2 = tri.pts[i] - at;

			// We're done!
			state = HAVE_SECOND_DONE;
			break;
		}
	}

	assert(state == HAVE_SECOND_DONE);

	return AngleBetween(l1, l2);
}

Vector ConvexUmbrellaTriMeshSdfAlgorithm::calc_angle_weighted_pseudo_normal(const ClosestTriSet &set)
{
	Vector pnorm(0, 0, 0, 0);

	if(set.tris.size() == 1) {
		pnorm = set.tris[0]->norm;
	}
	else if(set.tris.size() == 2) {
		// just add the two
		pnorm = set.tris[0]->norm + set.tris[1]->norm;
	}
	else if(set.tris.size() >= 3) {
		// Go through each tri, looking for its incident angle to the set's common pt
		TriangleList::const_iterator i;
		for(i = set.tris.begin(); i != set.tris.end(); ++i) {
			MeshTriangle *tri = *i;
			pnorm += tri->norm * incident_angle(*tri, set.p.getPoint());
		}
	}
	else {
		__ERROR__("calc_angle_weighted_pseudo_normal called with an empty tri set!");
		exit(0);
	}

	return pnorm;
}

Vector ConvexUmbrellaTriMeshSdfAlgorithm::calc_gradient(const ClosestTriSet &set)
{
	return this->pseudo_normal_to_gradient(
		this->calc_angle_weighted_pseudo_normal(set));
}

Vector ConvexUmbrellaTriMeshSdfAlgorithm::pseudo_normal_to_gradient(const Vector &pnorm)
{
	// This approximates gradient by getting the angle-weighted pseudo normal, negating it, and then
	// normalizing.
	Vector grad = pnorm * -1.0;
	grad.Normalize();
	return grad;
}

ostream& SDF::operator<< (ostream &os, const ClosestTriSet &set) {
	return os << "triSet{" <<
		"\tabs_dist: " << set.abs_dist <<
		"\tpot: " << set.p <<
		"\ttris.size: " << set.tris.size();
}

//-----------------------
//	Code snippet dumpster - useless now, may be useful later!
//-----------------------
/*
	
	__DBa(" after sorting tri-sets:");
	for( i = this->tri_sets.begin(); i != this->tri_sets.end(); ++i) {
		__DBa((*i)->abs_dist);
	}
*/

		// Uncomment below if u want to display debug info for this case.
		/*
		int dbi;
		__ERROR__("------  found an umbrella vert, but only one vertex was attached to it-------------");
		__ERROR__("  all umb tris:");
		for(dbi = 0; dbi < tris.size(); dbi++ ) {
			__ERROR__(*tris.at(dbi));
		}

		__ERROR__("  umb verts:");
		for(dbi = 0; dbi < umb_verts->size(); dbi++ ) {
			__ERROR__(*umb_verts->at(dbi));
		}

		__ERROR__("  lonely vert index: " << e_star);
		__ERROR__("  closest pt: " << x);
		*/