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

#include <math.h>
#include "cordic.h"

long arctantab[BITS];
long coscale;
long quarter;

/*
** 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 pseudorotate 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 void
pseudorotate(px, py, theta, bits)
long *px, *py;
register long theta;
int bits;
{
	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 void
pseudopolarize(argx, argy, bits)
long *argx, *argy;
int bits;
{
	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, bits)
long *argx, *argy;
int bits;
{
	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
*/
void
fxunitvec(cos, sin, theta, bits)
long *cos, *sin, theta;
int bits;
{
	*cos = coscale >> 1;	/* Save a place for the integer bit */
	*sin = 0;
	pseudorotate(cos, sin, theta, bits);

	/* 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.
*/
void
fxrotate(argx, argy, theta, bits)
long *argx, *argy;
long theta;
int bits;
{
	register long x, y;
	int shiftexp;

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

	/* Prenormalize vector */
	shiftexp = fxprenorm(argx, argy, bits);

	/* Perform CORDIC pseudorotation */
	pseudorotate(argx, argy, theta, bits);

	/* 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
*/
long
fxatan2(x, y, bits)
long x, y;
int bits;
{
	if ((x || y) == 0)
	    return(0);
	/* Prenormalize vector for maximum precision */
	(void) fxprenorm(&x,&y,bits);

	/* Convert to polar coordinates and return */
	pseudopolarize(&x,&y,bits);
	return(y);
}

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

	if ((*argx || *argy) == 0) {
	    *argx = 0;	/* radius */
	    *argy = 0;	/* angle */
	    return;
	}

	/* Prenormalize vector for maximum precision */
	shiftexp = fxprenorm(argx, argy, bits);

	/* Perform CORDIC conversion to polar coordinates */
	pseudopolarize(argx, argy, bits);

	/* Scale radius to undo pseudorotation enlargement factor */
	*argx = frmul(*argx, coscale);

	/* Denormalize radius */
	*argx = (shiftexp < 0) ? (*argx >> -shiftexp) : (*argx << shiftexp);
}

/*
** frmul - this is a hack, just for testing;
** 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));
}

/*
** cordic - initializes tables and constants for the CORDIC algorithm
*/
void
cordic_init(bits, msbits)
long bits, msbits;
{
	double scale;
	int i;

#ifdef DEGREES
	quarter = (90 << (bits-msbits));
	arctantab[0] = (1<<bits-msbits) * (180 / M_PI) * atan(2.0);
	arctantab[1] = (1<<bits-msbits) * (180 / M_PI) * atan(1.0);
	for (i = 2; i < bits; i++) {
		arctantab[i] = (1<<bits-msbits) * (180 / M_PI)
			* atan(1.0/(double)(1<<(i-1)));
	}
#endif
#ifdef RADIANS
	quarter = ((int)(3.141592654 / 2.0 * (1 << (bits-msbits))));
	arctantab[0] = (1<<bits-msbits) * atan(2.0);
	arctantab[1] = (1<<bits-msbits) * atan(1.0);
	for (i = 2; i < bits; i++) {
		arctantab[i] = (1<<bits-msbits) * atan(1.0/(double)(1<<(i-1)));
	}
#endif
#ifdef BRADS
	quarter = (1 << (bits-msbits));
	arctantab[0] = (1<<bits-msbits) * (2.0 / M_PI) * atan(2.0);
	arctantab[1] = (1<<bits-msbits) * (2.0 / M_PI) * atan(1.0);
	for (i = 2; i < bits; i++) {
		arctantab[i] = (1<<bits-msbits) * (2.0 / M_PI)
			* atan(1.0/(double)(1<<(i-1)));
	}
#endif

	/* coscale = (long)(0.271571768 * (1<<(bits-1))); */
	for (i = 1, scale = 1.0; i <= bits; i++)
                scale *= sqrt(1.0 + pow(2.0,(double)(-2*i)));
        coscale = (long)(scale * (1<<(bits-3)));
	printf("scale = %g\tcoscale = %d\n",scale,coscale);

#ifdef DEBUG
	for (i = 0; i < bits; i++) printf("theta[%d] = %d\n",i,arctantab[i]);
#endif
}