/*
** cordic -- program to calculate various functions
**           (e.g. trigonometric) using CORDIC techniques
**
**           original author unknown
**           modified by Joseph B. Evans, 11/15/91
**                       evans@shannon.tisl.ukans.edu
*/

#include "cordic.h"

static long arctantab[BITS] = {
# ifdef DEGREES		/* MS 10 integral bits */
	266065460, 188743680, 111421900, 58872272, 29884485, 15000234, 7507429,
	3754631, 1877430, 938729, 469366, 234683, 117342, 58671, 29335, 14668,
	7334, 3667, 1833, 917, 458, 229, 115, 57, 29, 14, 7, 4, 2, 1, 0, 0, 
# else
# ifdef RADIANS	/* MS 4 integral bits */
	297197971, 210828714, 124459457, 65760959, 33381290, 16755422, 8385879,
	4193963, 2097109, 1048571, 524287, 262144, 131072, 65536, 32768, 16384,
	8192, 4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1, 0, 0, 
# else	/* BRADS - MS 2 integral bits */
	756808418, 536870912, 316933406, 167458907, 85004756, 42667331,
	21354465, 10679838, 5340245, 2670163, 1335087, 667544, 333772, 166886,
	83443, 41722, 20861, 10430, 5215, 2608, 1304, 652, 326, 163, 81, 41,
	20, 10, 5, 3, 1, 1, 
# endif
# endif
};

/*
** pseudorotate - performs CORDIC rotations
**
** To rotate a vector through an angle of theta, we calculate:
**
**	x' = x cos(theta) - y sin(theta)
**	y' = x sin(theta) + y cos(theta)
**
** The rotate() routine performs multiple rotations of the form:
**
**	x[i+1] = cos(theta[i]) { x[i] - y[i] tan(theta[i]) }
**	y[i+1] = cos(theta[i]) { y[i] + x[i] tan(theta[i]) }
**
** with the constraint that tan(theta[i]) = pow(2, -i), which can be
** implemented by shifting. We always shift by either a positive or
** negative angle, so the convergence has the ringing property. Since the
** cosine is always positive for positive and negative angles between -90
** and 90 degrees, a predictable magnitude scaling occurs at each step,
** and can be compensated for instead at the end of the iterations by a
** composite scaling of the product of all the cos(theta[i])'s.
*/
static
pseudorotate(px, py, theta)
long *px, *py;
register long theta;
{
	register int i;
	register long x, y, xtemp;
	register long *arctanptr;

	x = *px;
	y = *py;

	/* Get angle between -90 and 90 degrees */
	while (theta < -QUARTER) {
	    x = -x;
	    y = -y;
	    theta += 2 * QUARTER;
	}
	while (theta > QUARTER) {
	    x = -x;
	    y = -y;
	    theta -= 2 * QUARTER;
	}

	/* Initial pseudorotation, with left shift */
	arctanptr = arctantab;
	if (theta < 0) {
	    xtemp = x + (y << 1);
	    y     = y - (x << 1);
	    x = xtemp;
	    theta += *arctanptr++;
	}
	else {
	    xtemp = x - (y << 1);
	    y     = y + (x << 1);
	    x = xtemp;
	    theta -= *arctanptr++;
	}

	/* Subsequent pseudorotations, with right shifts */
	for (i = 0; i <= (BITS-4); i++) {
	    if (theta < 0) {
		xtemp = x + (y >> i);
		y     = y - (x >> i);
		x = xtemp;
		theta += *arctanptr++;
	    }
	    else {
		xtemp = x - (y >> i);
		y     = y + (x >> i);
		x = xtemp;
		theta -= *arctanptr++;
	    }
	}

	*px = x;
	*py = y;
}


/*
** pseudopolarize - convert from rectangular to polar using CORDIC
**                  for CORDIC rotations
*/
static
pseudopolarize(argx, argy)
long *argx, *argy;
{
	register long theta;
	register long yi, i;
	register long x, y;
	register long *arctanptr;

	x = *argx;
	y = *argy;

	/* Get the vector into the right half plane */
	theta = 0;
	if (x < 0) {
	    x = -x;
	    y = -y;
	    theta = 2 * QUARTER;
	}

	if (y > 0)
	    theta = - theta;
	
	arctanptr = arctantab;
	if (y < 0) {	/* Rotate positive */
		yi = y + (x << 1);
		x = x - (y << 1);
		y = yi;
		theta -= *arctanptr++;	/* Subtract angle */
	}
	else {		/* Rotate negative */
		yi = y - (x << 1);
		x = x + (y << 1);
		y = yi;
		theta += *arctanptr++;	/* Add angle */
	}

	for (i = 0; i <= (BITS-4); i++) {
	    if (y < 0) {	/* Rotate positive */
		yi = y + (x >> i);
		x = x - (y >> i);
		y = yi;
		theta -= *arctanptr++;
	    }
	    else {		/* Rotate negative */
		yi = y - (x >> i);
		x = x + (y >> i);
		y = yi;
		theta += *arctanptr++;
	    }
	}

	*argx = x;
	*argy = theta;
}


/*
** fxprenorm - block normalizes the arguments to a magnitude suitable
**             for CORDIC pseudorotations.  The returned value is the
**              block exponent.
*/
static int
fxprenorm(argx, argy)
long *argx, *argy;
{
	register long x, y;
	register int shiftexp;
	int signx, signy;

	shiftexp = 0;		/* Block normalization exponent */
	signx = signy = 1;

	if ((x = *argx) < 0) {
	    x = -x; signx = -signx;
	}
	if ((y = *argy) < 0) {
	    y = -y; signy = -signy;
	}
	/* Prenormalize vector for maximum precision */
	if (x < y) {	/* |y| > |x| */
	    while (y < (1 << (BITS-5))) {
		x <<= 1; y <<= 1; shiftexp--;
	    }
	    while (y > (1 << (BITS-4))) {
		x >>= 1; y >>= 1; shiftexp++;
	    }
	}
	else {		/* |x| > |y| */
	    while (x < (1 << (BITS-5))) {
		x <<= 1; y <<= 1; shiftexp--;
	    }
	    while (x > (1 << (BITS-4))) {
		x >>= 1; y >>= 1; shiftexp++;
	    }
	}

	*argx = (signx < 0) ? -x : x;
	*argy = (signy < 0) ? -y : y;
	return(shiftexp);
}


/*
** fxunitvec - return a unit vector corresponding to theta, where
**             sin and cos (the x and y coordinates) are fixed-point
**             fractions
*/
fxunitvec(cos, sin, theta)
long *cos, *sin, theta;
{
	*cos = COSCALE >> 1;	/* Save a place for the integer bit */
	*sin = 0;
	pseudorotate(cos, sin, theta);

	/* Saturate results to fractions less than 1, shift out integer bit */
	if (*cos >= (1 << (BITS-2))) 
	    *cos = 0x7FFFFFFF;		/* Just shy of 1 */
	else if (*cos <= -(1 << (BITS-2)))
	    *cos = 0x80000001;		/* Just shy of -1 */
	else
	    *cos <<= 1;

	if (*sin >= (1 << (BITS-2)))
	    *sin = 0x7FFFFFFF;		/* Just shy of 1 */
	else if (*sin <= -(1 << (BITS-2)))
	    *sin = 0x80000001;		/* Just shy of -1 */
	else
	    *sin <<= 1;
}


/*
** fxrotate - rotates the vector (argx, argy) by the angle theta.
*/
fxrotate(argx, argy, theta)
long *argx, *argy;
long theta;
{
	register long x, y;
	int shiftexp;
	long frmul();

	if (((*argx || *argy) == 0) || (theta == 0)) {
	    *argx = 0; *argy = 0;
	    return;
	}

	/* Prenormalize vector */
	shiftexp = fxprenorm(argx, argy);
	/* Perform CORDIC pseudorotation */
	pseudorotate(argx, argy, theta);
	/* Compensate for CORDIC enlargement */
	x = frmul(*argx, COSCALE);
	y = frmul(*argy, COSCALE);
	/* Denormalize vector */
	if (shiftexp < 0) {
	    *argx = x >> -shiftexp;
	    *argy = y >> -shiftexp;
	}
	else {
	    *argx = x << shiftexp;
	    *argy = y << shiftexp;
	}
}


/*
** fxatan2 - take a vector described by rectangular coordinates
**           and return the arctangent of the angle from the + x-axis
*/
fxatan2(x, y)
long x, y;
{
	if ((x || y) == 0)
	    return(0);
	fxprenorm(&x, &y);	/* Prenormalize vector for maximum precision */
	pseudopolarize(&x, &y);	/* Convert to polar coordinates */
	return(y);
}

/*
** fxpolarize - take a vector described by rectangular coordinates
**              and replace it by the polar coordinates, after performing
**              various scaling operations
*/
fxpolarize(argx, argy)
long *argx, *argy;
{
	int shiftexp;
	long frmul();

	if ((*argx || *argy) == 0) {
	    *argx = 0;	/* Radius */
	    *argy = 0;	/* Angle */
	    return;
	}

	/* Prenormalize vector for maximum precision */
	shiftexp = fxprenorm(argx, argy);
	/* Perform CORDIC conversion to polar coordinates */
	pseudopolarize(argx, argy);
	/* Scale radius to undo pseudorotation enlargement factor */
	*argx = frmul(*argx, COSCALE);
	/* Denormalize radius */
	*argx = (shiftexp < 0) ? (*argx >> -shiftexp) : (*argx << shiftexp);
}

/*
** frmul - Just for testing. This is not portable.
** This routine should be written in assembler to calculate the
** high part of the product, i.e. the product when the operands
** are considered fractions.
*/
long
frmul(a, b)
long a, b;
{
	return((a >> 16) * (b >> 15));
}