/****************************************************************************/
/*	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_cordicr( int x, int y, int z );

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

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

/**
*	Rotate a vector by an angle.
*
*	This function rotates a vector by a given angle. The angle is represented
*	by a signed fixpoint number, where the radix point is after the 4<sup>th
*	</sup> most significant bit. The angle is expressed in radians. Any angle
*	representable in the 4.28 format is allowed.
*
*	The vector components are signed integers (or fixpoint numbers, but the
*	position of the radix point is up to you). Up to 28 significant bits are
*	allowed, that is, the top 4 bits must be either all 0s or all 1s.
*	If that's not the case, there will be internal overflows and you will get
*	garbage in the result.
*
*	You can use this function to rotate a vector by a given angle, or,
*	if you set x to the length of the vector and y to 0, to transform
*	from polar to Cartesian representation of a vector or, by choosing
*	the initial vector of (1,0) in your preferred representation, to
*	simultaneously calculate the sine and the cosine of an angle.
*
*	In effect, the function calculates:
*
*	@code
*	x_out = x * cos( a ) - y * sin( a );
*	y_out = x * sin( a ) + y * cos( a );
*	@endcode
*
*	The returned X and Y valueas are packed into a 64-bit integer. To
*	decompose the result into its components, the following code fragment
*	can be used:
*
*	@code
*	int x, y, a;
*	long long t;
*
*	t = vrot32( x, y, a );
*	x = t;
*	y = t >> 32;
*	@endcode
*
*	The result is accurate to 27 bits.
*
*	@param	x		The X coordinate of the vector to be rotated
*	@param	y		The Y coordinate of the vector to be rotated
*	@param	a		Rotational angle, in radians, multiplied by 2<sup>28</sup>.
*					This format allows you to represent angles in (-8,+8),
*					which is rougly -458 to +485 degrees, that is, more than
*					a full circle in each direction. The angular resolution
*					is 2<sup>-28</sup> radians, that is, about 0.0008 seconds
*					of arc.
*
*	@return			A 64-bit integer that contains the rotated vector's X
*					coordinate on its lower 32 bits and the Y coordinate on
*					its upper 32 bits.
*/

long long	vrot32( int x, int y, int a )
{
long long	r;
int			q;
int			t;

	q = 0;
	t = 0x6487ed50; 	// 2*PI

	// The underlying CORDIC core can handle angles between +/- 0.555 * PI,
	// If the original angle is negative, we add 2PI. It may leave a small
	// negative angle (which can be processed as it is) or a positive up to
	// 2PI.

	if ( a < 0 ) a += t;

	// Now we do a ranage reduction of the positive angles

	if ( a >= t ) {

		a -= t;
	}
	else {

		// The angle is either small negative or positive but less than 2*PI

		t >>= 1;		// PI

		if ( a >= t ) {

			a -= t;
			q += 2;
		}

		t >>= 1;		// PI / 2

		if ( a >= t ) {

			a -= t;
			q += 1;
		}
	}

	// The angle is such that it can be processed directly and q is the
	// number of extra PI/2 rotations that have to be done. The CORDIC core
	// returns a long long, the lower 32 bits are x and the upper 32 bits
	// represent y.

	r = _blm_cordicr( x, y, a );
	y = r >> 32;
	x = r;

	// Now do the extra PI/2 rotations, they are simply signum changes.

	switch ( q ) {

		case 1:

			x = -x;
			break;

		case 2:

			x = -x;
			y = -y;
			break;

		case 3:

			y = -y;
			break;
	}

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

/** @} */

