/* cordic.c Extended Range Cordic Algorithm */

/* These routine carry out extended range forward and backward CORDIC

   forward CORDIC :
   void CordicRT2XY ( float r, float theta, float *x, float *y, float *z ) ;
   Inputs:  r, theta - polar coordinates ( r>=0, -Pi <= theta <= Pi )
   Outputs: x, y - cartesian coordinates,  and z - the error

   backward CORDIC :
   void CordicXY2RT ( float x, float y, float *r, float *theta, float *err ) ;
   Inputs:  x, y - cartesian coordinates
   Outputs: r, theta - polar coordinates,  and err - the error

   For further details see DSP-CSP Section 16.5   */

/* Jonathan (Y) Stein */


#include <math.h>

/* globals for CORDIC sine and cosine procedures */
#define Pi 3.141592653
#define NumberOfBitsP3  34
float tp[NumberOfBitsP3], atantp[NumberOfBitsP3], K ;

void SetUpCordic ( void ) {
int  i ;
    tp[0] = 1 ; atantp[0] = Pi/4.0 ;
    tp[1] = 1 ; atantp[1] = Pi/4.0 ;
    tp[2] = 1 ; atantp[2] = Pi/4.0 ;
    K = sqrt(2.0) / 4.0 ;
    for (i=3; i<NumberOfBitsP3; i++) {
        tp[i] = tp[i-1] / 2.0;
        atantp[i] = atan(tp[i]) ;
        K = K * cos(atantp[i]) ;
    }
}

void CordicXY2RT ( float x, float y, float *r, float *theta, float *err ) {
int i ;
float z, tx ;
    z = 0.0 ;
    for (i=0; i<NumberOfBitsP3; i++) {
        tx = x ;
        if (y < 0) {
            x -= y * tp[i] ;
            y += tx * tp[i] ;
            z -= atantp[i] ;
        } else {
            x += y * tp[i] ;
            y -= tx * tp[i] ;
            z += atantp[i] ;
        }
    }
    *r = K * x ;
    *theta = z  ;
    *err = y ;
}

void CordicRT2XY ( float r, float theta, float *x, float *y, float *z ) {
int i, flag ;
float tx ;
    *x = r*K ;
    *y = 0 ;
    *z = theta ;
    for (i=0; i<NumberOfBitsP3; i++) {
        tx = *x ;
        if (*z < 0) {
            *x += *y * tp[i] ;
            *y -= tx * tp[i] ;
            *z += atantp[i] ;
        } else {
            *x -= *y * tp[i] ;
            *y += tx * tp[i] ;
            *z -= atantp[i] ;
        }
    }
}

#ifdef TEST
/* to compile for test:   cc -DTEST cordic.c -o cordtest.exe  */


#include <stdio.h>
#include <stdlib.h>

void main ( void ) {
int i ;
float x, y, r, theta, err ;
float tx, ty, tr, ttheta ;

    SetUpCordic() ;

    printf ( "FORWARD (polar to cartesian) CORDIC TEST\n" ) ;
    printf ( "    r      theta       x     cordic x     y     cordic y\n"  ) ;
    for (i=0; i<=20; i++) {
        r = sqrt(2)*(float)rand()/RAND_MAX ;
        theta = 2*Pi*((float)rand()/RAND_MAX-0.5) ;
        tx = r * cos(theta) ;
        ty = r * sin(theta) ;
        CordicRT2XY ( r, theta, &x, &y, &err ) ;
        printf ( "%8.4f %8.4f  %8.4f %8.4f  %8.4f %8.4f\n",
                  r,    theta, tx,   x,     ty,   y  ) ;
    }

    printf ( "\n\nBACKWARD (cartesian to polar) CORDIC TEST\n" ) ;
    printf ( "    x        y        r     cordic r   theta   cordic theta\n"  ) ;
    for (i=0; i<=20; i++) {
        x = 2*((float)rand()/RAND_MAX)-1 ;
        y = 2*((float)rand()/RAND_MAX)-1 ;
        tr = sqrt ( x*x + y*y ) ;
        ttheta = atan2 ( y, x ) ;
        CordicXY2RT ( x, y, &r, &theta, &err ) ;
        printf ( "%8.4f %8.4f %8.4f %8.4f  %8.4f %8.4f\n",
                  x,    y,    tr,   r,     ttheta, theta  ) ;
    }
}
#endif