/****************************************************************************/
/*	Copyright (c) 2009, Bendor Research Pty. Ltd. All rights reserved.		*/
/*	This software is released under the GNU General Public License, version	*/
/*	3, with the exemptions that 1) the header files installed by this 	 	*/
/*	package can be included in closed-source code and 2) the libraries		*/
/*	built by this package can be statically linked into a closed-source 	*/
/*	binary. See the file LICENSE for the details.							*/
/*	This software is provided "AS IS", without any warranty or guarantee.	*/
/*	By using this software you acknowledge that neither Bendor Research nor	*/
/*	any of its personnel or affiliates can be held liable for any damages,	*/
/*	including, but not limited to, loss of business, loss of money, loss of */
/*	life, loss of reputation, arising out of this software being faulty or	*/
/*	not performing as advertised, even if Bendor Research was aware of such	*/
/*	faults in the software. You use this software at your own risk, any and */
/*	all liabilities that arise out of your using this software are strictly	*/
/*	yours.																	*/
/****************************************************************************/

#include <fixpoint.h>

/****************************************************************************/
/*					Local prototypes										*/
/****************************************************************************/

long long _blm_cordicv( int x, int y, int z );

/****************************************************************************/
/*					Public API functions									*/
/****************************************************************************/

/** @addtogroup fixp */
/** @{ */

/**
*	Calculate the length and angle of a vector.
*
*	This function processes a vector presented in Cartesian format and returns
*	its length and angle. That is, it transforms the Cartesian represenation to
*	polar form. The X and Y coordinates can be integers or fixpoint numbers
*	with the radix point at any place; the only restriction is that they must
*	have at most 28 significant bits. Thus, the most significant 4 bits must
*	be all 0s or all 1s. If that criterion is not met, then the returned values
*	will be garbage.
*
*	The function returns the length of the vector, in the same format as X and
*	Y were presented. The length is always positive. Furthermore, the function
*	returns the angle of the vector. The returned angle is a fix-point number
*	which represents the angle in radians, multiplied by 2<sup>28</sup>. That
*	is, the radix point is after the 4<sup>th</sup> most significant bit of the
*	32 bit integer. The returned value is in [-PI,+PI].
*
*	In effect, the function calculates:
*
*	@code
*	length = sqrt( x*x + y*y );	// Length of the vector
*	angle  = atan2( y, x );		// Qaudrant correct angle of the vector
*	@endcode
*
*	The two returned values are concatenated into a 64-bit integer, in that
*	way the use of a structure can be avoided. To decompose the result the
*	following simple code fragment can be used:
*
*	@code
*	int x, y;				// The vector
*	int angle, length;	 	// The returned values
*	long long temp;			// A temporary variable
*
*	temp = vabs32( x, y );
*	angle = temp >> 32;		// The angle is in the upper 32 bits
*	length = temp;			// The length is in the lower 32 bits
*	@endcode
*
*	The result is accurate to 27 bits.
*
*	@param	x		The X coordinate of the vector
*	@param	y		The Y coordinate of the vector
*
*	@return			A 64-bit integer that in its upper 32 bits contains
*					the angle of the vector, in signed 4.28 format and
*					the lower 32 bits is the length of the vector, in
*					the same format as the X and Y coordinates were. The
*					length of the vector is always positive.
*/

long long	vabs32( int x, int y )
{
long long	retv;
int			quad;
int			temp;
int			angl;

	temp = x;
	angl = y;

	// Special cases

	if ( ! angl ) {

		if ( temp < 0 ) {

			x = -angl;
			y = 0x3243f6a8;			// PI
		}
		else {

			y = 0;
		}

		return( (long long ) y << 32 | x );
	}

	if ( ! temp ) {

		if ( angl < 0 ) {

			x = -angl;
			y = -0x1921fb54;		// -PI/2
		}
		else {

			x = angl;
			y = 0x1921fb54;			// PI/2
		}

		return( (long long ) y << 32 | x );
	}

	// Work out the quadrant

	if ( temp < 0 ) {

		quad = 1;
		temp = -temp;

		if ( angl < 0 ) {

			angl = -angl;
			quad = 2;
		}
	}
	else {

		quad = 0;

		if ( angl < 0 ) {

			angl = -angl;
			quad = 3;
		}
	}

	// Calculate the length and the angle for the 0th quadrant

	retv = _blm_cordicv( temp, angl, 0 );
	temp = retv;
	angl = retv >> 32;

	// Make the angle qaudrant correct

	switch ( quad ) {

		case 1:

			angl = 0x3243f6a8 - angl;		// PI - angle
			break;

		case 2:

			angl = angl - 0x3243f6a8;		// angle - PI
			break;

		case 3:

			angl = -angl;
			break;
	}

	// We're done

	return( (long long ) angl << 32 | temp );
}

/** @} */

