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

#include "Utils.h"

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

Utils::Utils()
{

}

Utils::~Utils()
{

}

char* Utils::STL_to_ADF_filename(char *stlFilename, float tolerance, int subdivLevel, bool using_sphere_tree)
{
	char* ADFFilename = new char[100];

	// Convert the subdiv level to a string
	char* subdivLevelStr = new char[8];
	itoa(subdivLevel, subdivLevelStr, 10);

	// Similarly
	// Truncate the error
	int tolerTrunc = (int)tolerance;
	char* tolerStr = new char[32];
	itoa(tolerTrunc, tolerStr, 10);

	
	strcpy(ADFFilename, stlFilename);
	// Tack on tolerance
	strcat(ADFFilename, ".");
	strcat(ADFFilename, tolerStr);
	// Tack on the max sub div level
	strcat(ADFFilename, ".");
	strcat(ADFFilename, subdivLevelStr);

	// Tack on the sphere flag
	if(using_sphere_tree)
	{
		strcat(ADFFilename, ".spheretree");
	}

	strcat(ADFFilename, ".adf");

	return ADFFilename;
}

double Utils::abs_d(double num)
{
	if (num >= 0) {
		return num;
	}
	else {
		return -num;
	}
}

bool Utils::approx_equals(double a, double b, double epsilon)
{
	return (a-epsilon) <= b && b <= (a+epsilon);
}

bool Utils::points_approx_equal(const Point &a, const Point &b, double epsilon)
{
	return approx_equals(a(0), b(0), epsilon)
		&& approx_equals(a(1), b(1), epsilon)
		&& approx_equals(a(2), b(2), epsilon);
}

Point Utils::octnode_corner_pt(const Point &c000, double edgelength, int x, int y, int z)
{
	return Point( c000(0) + x*edgelength,
		c000(1) + y*edgelength,
		c000(2) + z*edgelength );
}

//--------------------------------
// General util for debugging prints.
// If you actually want to show output, u shouldn't use this.
// This is here so we can easily turn off all debugging prints.
//--------------------------------
void Utils::shout(const char *fmt, ...)
{
	va_list		argptr;
	char		text[1024];

	va_start( argptr, fmt );
	vsprintf( text, fmt, argptr );
	va_end( argptr );

	fprintf(stdout, text);
}

//--------------------------------
// If result is false, this will halt the program (by using getchar()..heh)
// and print out the given msg.
// So, the message should be the failing case, explaning what went unexpectedly.
//--------------------------------
void Utils::Assert(bool result, const char *fmt, ...)
{
	if(result == FALSE) {
		//--------------------------------
		// Print the msg
		//--------------------------------

		va_list		argptr;
		char		text[1024];

		va_start( argptr, fmt );
		vsprintf( text, fmt, argptr );
		va_end( argptr );

		fprintf(stdout, text);

		//--------------------------------
		// Halt the program
		//--------------------------------
		assert(FALSE);
	}
}


//--------------------------------
// NOTE: This assumes the ray is NOT parallel with the plane!!
// If u don't satisfy that, you will get a div by 0!!
// Also assumes that plane is normaled, meaning the directional components have magnitude 1!!
//--------------------------------
Point Utils::intersect_ray_with_plane(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
	const double p_dot_start = plane.Dot(start);
	const double intxDist = -1 * p_dot_start / p_dot_n;

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

	return intxPt;
}

void Utils::Test_plane_ray_intx()
{
	//--------------------------------
	// 
	//--------------------------------

	Plane p(0, 1, 0, -1);
	Point s(0, -1, 0);
	Point t(0, 1, 0);

	Point expected_intx(0, 1, 0);
	Point actual_intx = intersect_ray_with_plane(p, s, t);

	//printf("acual %f %f %f", actual_intx(0), actual_intx(1), actual_intx(2));

	Utils::Assert((actual_intx == expected_intx) == TRUE);
}

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

	Test_plane_ray_intx();

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

}