#include "StlSdfAble.h"
using namespace SDF;

SdfValue calc_sdf_from_point_to_STL(CLEDSGeometry *STL, const Point &pt, TriMeshSdfAlgorithm *algo);

SdfValue StlSdfAble::calc_sdf_from_pt( const Point &from ) const
{
	// Calculate the closest pt and SDF
	SDF::SdfValue sdf = calc_sdf_from_point_to_STL(
		this->stl,
		from,
		this->sdf_algo);

	return sdf;
}

StlSdfAble::~StlSdfAble() {
	if(this->sdf_algo != NULL) {
		delete this->sdf_algo;
	}
}

//--------------------------------
// (Steve) This is my modified version of Greg's original minDistSTL function.
// It uses a loop to go through all triangles of the given STL,
// but then feeds these triangles into the given SDF algorithm object.
// It then returns the result of the algorithm computation.
//--------------------------------
SdfValue calc_sdf_from_point_to_STL(CLEDSGeometry *STL, const Point &pt, TriMeshSdfAlgorithm *algo)
{
	// Reset the algorithm
	algo->start_new_computation(pt);
	
	// Now loop through all the triangles in the STL
	// using Greg's code

	//This is the old brute force method
	CListIter<CLEDSliteFace *, UINT_8> CListIterIntFace;
	CLEDSliteFace *pFace;
	CLEDSliteFaceIter FaceIter;
	CLEDSliteVertex *p1stVtx, *pVtx;
	Point P,Q,R,S;
	//octreeVtx *VClosest = NULL, *VTemp = NULL;
	int index=0,numTetra=0;
	
	//Initialize face list iterator
	CListIterIntFace.Init(STL->GetCListFaces());
	
	//Pull out the first face
	pFace = CListIterIntFace.PeekFirst();
	
	while (pFace)
	{
		// Initialize face iterator
		FaceIter.Init(pFace);
		
		// Get first vertex in face
		pVtx = p1stVtx = (CLEDSVertex *)FaceIter.PeekFirstCVertex();
		
		// (Steve) Iterate through all tris and find the closest
		// of all distances

		do {
			index++;
			if (index == 1) {
				Q = pVtx->GetPoint();
			}
			else if (index == 2) {
				R = pVtx->GetPoint();
			}
			else {
				S = pVtx->GetPoint();
				index = 0;
				numTetra++;

				// QRS is now a triangle
				// Create a new MeshTriangle

				//---------------------------------
				//	Sometimes, we can get colinear triangles for some reason..
				//---------------------------------
				Vector L1 = R-Q;   //edge 1
				Vector L2 = S-R;   //edge 1
				Vector norm = L1 * L2;

				if(norm.Magnitude() == 0.0) {
					//__WARN__("colinear tri! skipping.  Pts: " << Q << R << S);
				}
				else {
					// We're good.  Input it.
					MeshTriangle tri(Q, R, S);

					// Feed it into the algorithm
					algo->feed_triangle(tri);
				}
			}
			// Get next vertex in face
			pVtx = (CLEDSVertex *)FaceIter.PeekNextCVertex();
		}
		while (pVtx != p1stVtx); // Until you're back to the start
			
		//Get next face
		pFace = CListIterIntFace.PeekNext();
	}

	//--------------------------------
	// Now return the algo's results
	//--------------------------------

	const SDF::SdfValue sdf = algo->compute_sdf_result();
	return sdf;
}

//---------------------------------
//	Dumpster for old code
//---------------------------------
/*
	// Use minDistSTL to get the octree vert info
	octreeVtx *vert = minDistSTL(STL, &temp);
	// then extract and return the info from that.
	SDF::SdfValue sdf(
		abs((double)vert->dist),
		*(vert->Pc),
		(vert->dist >= 0.0f ? SDF::INTERIOR : SDF::EXTERIOR));

*/