/*****************************************************************************
 * $Id: orient.cc,v 1.1 2006/10/21 00:19:08 devel Exp $
 * @file
 * @author	Jim E. Brooks  http://www.jimbrooks.org
 * @brief	Test of matrix orient functions (these functions formed basis of OrientMatrix_*() ).
 *//*
 * LEGAL:	COPYRIGHT (C) 2006 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE (GPL).
 * @verbatim
 * Jim Brooks  2006/05
 * Idea is to yaw and pitch a matrix to orient its Z axis to point towards a target.
 *
 * The trick is that the calculations of the angles to yaw/pitch are asymmetric.
 * The yaw angle is calculated from the X coordinate vs. the 2D distance of the X,Z coordinates.
 * The pitch angle is calculated from the Y coordinate vs. the full 3D distance.
 *
 * When target is at (1,1,1), this is the result.
 * 0.5777^2 = 0.333  (1,1,1), when normalized, is (0.333,0.333,0.333)
 * Xxyzw:   0.707107  -0.408248  0.577350
 * Yxyzw:   0.000000   0.816497  0.577350
 * Zxyzw:  -0.707107  -0.408248  0.577350
 * Oxyzw:   0.000000   0.000000  0.000000
 * yaw, pitch = 45.000000, 35.264389
 * CheckAngle3D() = 0.000000
 *
 * The code, I have to admit, is based on trial-and-error.
 * The first problem: I originally tried calculating angles in 2D for yaw which failed.
 * Then tried in 3D, which I thought would fail, but it succeeded.
 * The second problem: The sign of the degrees.  By trial-and-error.
 * @endverbatim
 *****************************************************************************/

#define TESTING 1
#define DEBUG 1
#include "gfx.hh"
#include "gfx_math.hh"
#include "gfx_random.hh"
using namespace gfx;

/*****************************************************************************
 * @param	m
 *          matrix (in/out)
 * @param	targetLocal
 *			Target translated relative to matrix origin.
 *****************************************************************************/
fp
CalcAngleYaw( const Matrix& m, const Vector3& targetLocal )
{
	// Calculate angle between YZ plane and X coordinate.
	fp dist = sqrtf( SQUARE(targetLocal.x)
                   + SQUARE(targetLocal.z) );
	if ( dist < 0.1 ) return 0.0;  // prevent divide error
	fp norm = targetLocal.x / dist;
	fp rad = asin( norm );
	fp deg = rad2deg( rad );

	// Change sign of degree according to X,Z coordinate.
	// This affects and depends on the way RotateMatrixLocal() rotates.
	if ( targetLocal.z < 0 )
		deg += 180.0;
	if ( (targetLocal.x < 0 && targetLocal.z < 0) || (targetLocal.x > 0 && targetLocal.z < 0) )
		deg = -deg;

	return deg;
}

/*****************************************************************************
 * @param	m
 *          matrix (in/out)
 * @param	targetLocal
 *			Target translated relative to matrix origin.
 *****************************************************************************/
fp
CalcAnglePitch( const Matrix& m, const Vector3& targetLocal )
{
	// Calculate angle between Y coordinate and 3D distance.
	// Visualize the 3D distance as point the Y axis,
	// and the target is some 3D point elsewhere.
	// The angle is between the target's Y coordinate and the point on the Y axis.
	fp dist = sqrtf( SQUARE(targetLocal.x)
                   + SQUARE(targetLocal.y)
                   + SQUARE(targetLocal.z) );
	if ( dist < 0.1 ) return 0.0;  // prevent divide error
	fp norm = targetLocal.y / dist;
	fp rad = asin( norm );
	fp deg = rad2deg( rad );
	return deg;
}

/*****************************************************************************
 * Check 3D angle between matrix Z axis and target.
 *****************************************************************************/
bool
CheckAngle3d( const Matrix& m, const Vector3& targetLocal )
{
	// Z axis coordinates are taken from Xz/Yz/Zz column (not Zx/Zy/Zx row).
	// For example, for target at (1,1,1), the right column corresponds to (1,1,1).
	// BTW 0.5777350^2 = 0.33333333
	// Xxyzw:  0.707107  -0.408248  0.577350
	// Yxyzw:  0.000000   0.816497  0.577350
	// Zxyzw:  -0.707107 -0.408248  0.577350
	Vector3 z;
	z.x = m[Xz];
	z.y = m[Yz];
	z.z = m[Zz];
	fp rad = Angle( z, targetLocal );
	fp deg = rad2deg( rad );
	cout << "CheckAngle3D() = " << deg << endl;
	return ABS(deg) < 0.1;
}

/*****************************************************************************
 * Orient matrix towards a target vertex.
 * @return 0 if successful (for exit code).
 *****************************************************************************/
int
Orient( void )
{
	// Init matrix.
	Matrix m;
	IdentityMatrix( m );

	// Init target.
	WorldVertex target;
	target.x = 0.0;
	target.y = 0.0;
	target.z = 0.0;

	// Test every sign combination.
	for ( int x1 = -1; x1 <= 1; x1 += 2 )
	for ( int y1 = -1; y1 <= 1; y1 += 2 )
	for ( int z1 = -1; z1 <= 1; z1 += 2 )
	for ( int x2 = -1; x2 <= 1; x2 += 2 )
	for ( int y2 = -1; y2 <= 1; y2 += 2 )
	for ( int z2 = -1; z2 <= 1; z2 += 2 )
	{
		cout << "------------------------\n";
		cout << "x1,y1,z1 = " << x1 << ',' << y1 << ',' << z1 << "  "
		     << "x2,y2,z2 = " << x2 << ',' << y2 << ',' << z2 << endl;

		// Reset matrix axis coefficients (leave matrix origin unchanged).
		m[Xx] = 1.0; m[Xy] = 0.0; m[Xz] = 0.0;
		m[Yx] = 0.0; m[Yy] = 1.0; m[Yz] = 0.0;
		m[Zx] = 0.0; m[Zy] = 0.0; m[Zz] = 1.0;

		// Random coordinates (on first pass).
		static int once = 0;
		if ( once++ == 0 )
		{
			randomize();
			const fp n = 1000.0;
			m[Ox]    = MAX(random_f( n ), 30.0);
			m[Oy]    = MAX(random_f( n ), 30.0);
			m[Oz]    = MAX(random_f( n ), 30.0);
			target.x = MAX(random_f( n ), 30.0);
			target.y = MAX(random_f( n ), 30.0);
			target.z = MAX(random_f( n ), 30.0);
		}

		// Change sign of coordinates.
		m[Ox]    = ABS(m[Ox]) * x1;
		m[Oy]    = ABS(m[Oy]) * y1;
		m[Oz]    = ABS(m[Oz]) * z1;
		target.x = ABS(target.x) * x2;
		target.y = ABS(target.y) * y2;
		target.z = ABS(target.z) * z2;

#if 0
		m[Ox] =    x1 * 1110.0;
		m[Oy] =    y1 * 2033.0;
		m[Oz] =    z1 * 33330.0;
		target.x = x2 * 3331.0;
		target.y = y2 * 111231.0;
		target.z = z2 * 134431341.0;
#endif

		// Target translated relative to matrix origin.
		Vector3 targetLocal;
		targetLocal.x = target.x - m[Ox];
		targetLocal.y = target.y - m[Oy];
		targetLocal.z = target.z - m[Oz];

		// Orient matrix's Z axis towards target.
		fp degYaw   = CalcAngleYaw(   m, targetLocal );
		fp degPitch = CalcAnglePitch( m, targetLocal );
		RotateMatrixLocalDegree( AXIS_Y, degYaw,   m );
		RotateMatrixLocalDegree( AXIS_X, degPitch, m );

		PrintMatrix( m );
		cout << "yaw, pitch = " << degYaw << ", " << degPitch << endl;

		// Check that angle is approx 0.
		if ( ! CheckAngle3d( m, targetLocal ) )
		{
			cerr << "ERROR[CheckAngle3d]: angle != 0 \n";
			cerr << "mOxyz = " << m[Ox] << ',' << m[Oy] << ',' << m[Oz] << endl;
			cerr << "targetLocal = " << targetLocal << endl;
			return 1;  // error
		}
	}
	return 0;  // ok
}

int main( int argc, char** argv )
{
	GFX_INIT( argv[0] );
	int res = Orient();
	GFX_CLEANUP();
	if ( res == 0 ) cerr << "\nPASSED.\n"; else cerr << "\nFAILED\n";
	exit( res );
}
