// TriMeshSDFAlgorithm.cpp: implementation of the TriMeshSDFAlgorithm class.
//
//////////////////////////////////////////////////////////////////////

#include "Utils.h"
#include "TriMeshSdfAlgorithm.h"
using namespace SDF;

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

TriMeshSdfAlgorithm::TriMeshSdfAlgorithm()
{

}

TriMeshSdfAlgorithm::~TriMeshSdfAlgorithm()
{

}

int maxInd (Vector l);
Vector proj (Vector A, Vector B);

/*
Vector proj (Vector A, Vector B) {
	// Projection of A onto B
	return (A.Dot(B) / B.Dot(B)) * B;
}
*/

double Dot(const Vector &A, const Vector &b) {
	return A.Dot(b);
}

/*
int maxInd (Vector l) {
	// Returns the index of the vector component with the largest magnitude
	float value = Utils::abs_d(l[0]); // Assume its x
	int index = 0;
	
	if (value < Utils::abs_d(l[1])) { // But maybe its y
		value = Utils::abs_d(l[1]);
		index = 1;
	}
	
	if (value < Utils::abs_d(l[2])) { // Or maybe its z
		index = 2;
	}
	
	return index;
}
*/

/* (Steve) This is simple and easy..but not robust at all.  In cases where A and B have equal
components, this breaks down due to round off error

bool pt_between_but_not_on(const Point &p, const Point &A, const Point &B) {
	return
		// Make sure p is not EQUAL to any of the two pts
		p != A && p != B
		// Make sure the point is between A and B..
		// We need <= here!!  Because if one component is the same for all,
		// we still consider p "in between".  If we used <, then that component
		// would these two tests to fail
		&& ((A <= p && p <= B) || (B <= p && p <= A));
}
*/

//---------------------------------
//	(Steve) 2005-12-13 (02-12)
//	This version is based on Greg's maxInd code.
//---------------------------------
bool edge_projed_pt_within_boundary(const Point &p, const Point &A, const Vector &lineAB)
{
	// Greg's code
	unsigned int i = maxInd(lineAB);			// This prevents a division be zero error in the next line
	double s = (p(i) - A(i)) / lineAB(i);		// To be used to check if projection is between the vertices

	return (s > 0.0f) && (s < 1.0f);

}

PointOnTriangle MeshTriangle::closest_point_from(const Point &p)
{
	//---------------------------------
	//	2005-12-12 (22-53) (Steve)
	//	A lot of this code is Greg's original minDistTri function
	//	However, I found a few problems and bugs with it, so I re-wrote a lot of it.
	//
	//	This ain't pretty - but hopefully it works fine and is clear.
	//---------------------------------

	Point Pc;	// The closest point
	const Point Pt(p);	// The test point
	PointOnTriangle::POTType PcType = PointOnTriangle::POT_UNKNOWN;

	// If cp_type is POT_EDGE, then these specify the edge verts
	unsigned int e1 = 3;
	unsigned int e2 = 3;	

	// Save some typing
	const Point P0 = (this->pts[0]);
	const Point P1 = (this->pts[1]);
	const Point P2 = (this->pts[2]);
	const Vector N = this->norm;
	
	// Edge vectors
	const Vector L01( P1 - P0 );   //edge 1
	const Vector L12( P2 - P1 );   //edge 2
	const Vector L20( P0 - P2 );   //edge 3

	//---------------------------------
	//	The inward edge normals
	//	(Steve) These point in towards the center of the triangle, if
	//	you add them to the mid pts of the edges
	//---------------------------------
	const Vector N01( L01 * N );
	const Vector N12( L12 * N );
	const Vector N20( L20 * N );

	//---------------------------------
	//	The test point projected onto the tri's plane
	//	proj((P0 - Pt), N) gives a vector perpendicular to the plane.
	//	Its magnitude is also Pt's distance from the plane.  Thus, when we
	//	add it to Pt, we will get its projection onto the plane.
	//---------------------------------
	const Point Pproj( Pt + proj((P0 - Pt), N) );

	//---------------------------------
	//	Vectors from the plane projection to the triangle vertices
	//---------------------------------
	const Vector V0(P0 - Pproj);
	const Vector V1(P1 - Pproj);
	const Vector V2(P2 - Pproj);


	//---------------------------------
	//	These are dot products which tell us where the projected point is
	//	relative to each edge - either "in front" or "behind"
	//---------------------------------
	const double proj_dot_edge01 = Dot(V0, N01);
	const double proj_dot_edge12 = Dot(V1, N12);
	const double proj_dot_edge20 = Dot(V2, N20);

	//---------------------------------
	//	Is the projected point in the triangle, and not touching an edge/vertex?
	//---------------------------------
	if( proj_dot_edge01 > 0.0f
		&& proj_dot_edge12 > 0.0f
		&& proj_dot_edge20 > 0.0f )
	{
		// Yay - we're done.
		Pc = Pproj;
		PcType = PointOnTriangle::POT_FACE;
	}
	else
	{
		PcType = PointOnTriangle::POT_UNKNOWN;

		//---------------------------------
		//	Go through all edges and see if the point projects onto any of them
		//	Keep trying when necessary
		//	It's quite important that these are NOT else-if's!
		//---------------------------------
		
		if( proj_dot_edge01 <= 0 )
		{
			const Point proj01 = Pproj + proj(V0, N01);
			if( edge_projed_pt_within_boundary(proj01, P0, L01) )
			{
				// Good - we got an edge!
				Pc = proj01;
				PcType = PointOnTriangle::POT_EDGE;
				e1 = 0;
				e2 = 1;
			}
		}

		//	It's quite important that these are NOT else-if's!

		if( PcType == PointOnTriangle::POT_UNKNOWN
			&& proj_dot_edge12 <= 0 )
		{
			const Point proj12 = Pproj + proj(V1, N12);
			if( edge_projed_pt_within_boundary(proj12, P1, L12) )
			{
				Pc = proj12;
				PcType = PointOnTriangle::POT_EDGE;
				e1 = 1;
				e2 = 2;
			}
		}

		if( PcType == PointOnTriangle::POT_UNKNOWN
			&& proj_dot_edge20 <= 0 )
		{
			const Point proj20 = Pproj + proj(V2, N20);
			if( edge_projed_pt_within_boundary(proj20, P2, L20) )
			{
				Pc = proj20;
				PcType = PointOnTriangle::POT_EDGE;
				e1 = 2;
				e2 = 0;
			}
		}


		if( PcType == PointOnTriangle::POT_UNKNOWN )
		{
			//---------------------------------
			//	At this point, the only options we have are the vertices
			//	Just find the one that gives us the minimal distance!
			//---------------------------------

			unsigned int imin = 0;
			double min_dist = (Pt - this->pts[0]).Magnitude();
			
			for(int i = 1; i < 3; i++) {
				double curr_dist = (Pt - this->pts[i]).Magnitude();
				if(curr_dist < min_dist) {
					imin = i;
					min_dist = curr_dist;
				}
			}

			Pc = this->pts[imin];
			PcType = PointOnTriangle::POT_VERTEX;
		}
	}

	assert(PcType != PointOnTriangle::POT_UNKNOWN);

	return PointOnTriangle(*this, PcType, Pc, e1, e2);
}

/*
These two functions are defined in ADFCreator.c
*/
double dist_to_plane( const Vector &norm, const Point &on_plane, const Point &from_pt );
bool point_behind_plane( const Vector &norm, const Point &on_plane, const Point &test_pt);

bool tri_A_behind_B( const Point &Q, const Point &R, const Point &S,
					const Point &T, const Vector &tri_norm)
{
	// First, calculate the average pt of tri A.  We'll use this point to
	// determine tri A's relative position to B.

	Point avgA = (Q + R + S) * (float)(1.0/3.0);

	bool behind = point_behind_plane(tri_norm, T, avgA);

	return behind;
}

bool MeshTriangle::is_behind(const MeshTriangle &tri) const
{
	return tri_A_behind_B(
		this->pts[0], this->pts[1], this->pts[2], 
		tri.pts[0], tri.norm );
}

ostream& SDF::operator<< (ostream &os, const SDF::MeshTriangle &tri)
{
  return os <<
	  "Point" << tri.pts[0] << "\t" <<
	  "Point" << tri.pts[1] << "\t" <<
	  "Point" << tri.pts[2] << "\t" <<
	  "Normal" << tri.norm;
}

//-----------------------
//	
//-----------------------

Point PointOnTriangle::getEdgeStart() const
{
	assert(this->getType() == POT_EDGE);
	assert(this->e1 >= 0);
	assert(this->e1 < 3);

	return this->tri.pts[e1];
}

Point PointOnTriangle::getEdgeEnd() const
{
	assert(this->getType() == POT_EDGE);
	assert(this->e2 >= 0);
	assert(this->e2 < 3);

	return this->tri.pts[e2];
}

ostream& SDF::operator<< (ostream &os, const PointOnTriangle &pot)
{
	if(pot.getType() == PointOnTriangle::POT_EDGE) {
		return os <<
			"pot{" <<
			"\tpoint: " << pot.p <<
			"\ttype: " << pot.type <<
			"\ttri: " << pot.tri <<
			"\te1: " << pot.getEdgeStart() <<
			"\te2: " << pot.getEdgeEnd() <<
			"\t}";
	}
	else {
		return os <<
			"pot{" <<
			"\tpoint: " << pot.p <<
			"\ttype: " << pot.type <<
			"\ttri: " << pot.tri <<
			"\t}";
	}
		
}

ostream& SDF::operator<< (ostream &os, const SDF::PointOnTriangle::POTType &type) {
	if(type == PointOnTriangle::POT_EDGE) {
		return os << "EDGE";
	}
	else if(type == PointOnTriangle::POT_FACE ) {
		return os << "FACE";
	}
	else if(type == PointOnTriangle::POT_VERTEX ) {
		return os << "VERTEX";
	}
	else if(type == PointOnTriangle::POT_UNKNOWN ) {
		return os << "UNKNOWN";
	}
	else {
		return os << "** invalid POT type!! **";
	}
}