/*
 * CORDIC algorithms.
 * The original code was published in Docter Dobbs Journal issue ddj9010.
 * The ddj version can be obtained using FTP from SIMTEL and other places.
 *
 * Converted to ANSI-C ( with prototypes ) by P. Knoppers, 13-Apr-1992.
 *
 * The main advantage of the CORDIC algorithms is that all commonly used math
 * functions ( [a]sin[h] [a]cos[h] [a]tan[h] atah[h] ( y/x ) ln exp sqrt ) are
 * implemented using only shift, add, subtract and compare. All values are
 * treated as integers. Actually they are fixed point floating point values.
 * The position of the fixed point is a compile time constant ( fractionBits ).
 * I don't believe that this can be easily fixed...
 *
 * Some initialization of internal tables and constants is necessary before
 * all functions can be used. The constant "HalfPi" must be determined before
 * compile time, all others are computed during run-time -see main() below.
 *
 * Of course, any serious implementation of these functions should probably
 * have _all_ constants determined sometime before run-time and most
 * functions might be written in assembler.
 *
 *
 * The layout of the code is adapted to my personal preferences.
 * PK.
 *
 * Reformatted again by D. Corbit.  Also, the code has been tweaked
 * to favor shifts over multiplication by two or adding a quantity
 * to itsself.
 */


/* Internal Prototypes: */


void WriteVarious( long n );
void WriteFraction( long n );
long Reciprocal( unsigned n, unsigned k );
long Poly2( int log, unsigned n );
void Circular( long x, long y, long z );
void WriteRegisters( void );
long ScaledReciprocal( long n, unsigned k );
void InvertCircular( long x, long y, long z );
void Hyperbolic( long x, long y, long z );
void Linear( long x, long y, long z );
void InvertHyperbolic( long x, long y, long z );


/*
 * _IMPLEMENTING CORDIC ALGORITHMS_
 * by Pitts Jarvis
 *
 *
 * [LISTING ONE]
 */


/*
 * cordicC.c --J. Pitts Jarvis, III
 *
 * cordicC.c computes CORDIC constants and exercises the basic algorithms.
 * Represents all numbers in fixed point notation. 1 bit sign,
 * longBits-1-n bit integral part, and n bit fractional part. n=29 lets us
 * represent numbers in the interval [-4, 4 ) in 32 bit long. Two's
 * complement arithmetic is operative here.
 */


#define fractionBits 29
#define longBits 32
#define One ( 010000000000L>>1 )
#define HalfPi ( 014441766521L>>1 )
#define TwoToTheMinusTwentyNinthPower 1.86264514923095703125e-9
/*
 * Cordic algorithm identities for circular functions, starting with [x, y, z]
 * and then driving z to 0 gives:
 *    [P* ( x*cos ( z )-y*sin ( z ) ), P* ( y*cos ( z )+x*sin ( z ) ), 0]
 * Driving y to 0 gives:
 *    [P*sqrt ( x^2+y^2 ), 0, z+atan ( y/x )]
 *    where K = 1/P = sqrt ( 1+1 )* . . . *sqrt ( 1+ ( 2^ ( -2*i ) ) )
 * Special cases which compute interesting functions:
 *    sin, cos [K, 0, a] -> [cos ( a ), sin ( a ), 0]
 *    atan [1, a, 0] -> [sqrt ( 1+a^2 )/K, 0, atan ( a )]
 *    [x, y, 0] -> [sqrt ( x^2+y^2 )/K, 0, atan ( y/x )]
 * For hyperbolic functions, starting with [x, y, z] and then
 * driving z to 0 gives:
 *    [P* ( x*cosh ( z )+y*sinh ( z ) ), P* ( y*cosh ( z )+x*sinh ( z ) ), 0]
 * Driving y to 0 gives:
 *    [P*sqrt ( x^2-y^2 ), 0, z+atanh ( y/x )]
 * Where K = 1/P = sqrt ( 1-( 1/2 )^2 )* . . . *sqrt ( 1-( 2^ ( -2*i ) ) )
 *    sinh, cosh [K, 0, a] -> [cosh ( a ), sinh ( a ), 0]
 *    exponential [K, K, a] -> [e^a, e^a, 0]
 *    atanh [1, a, 0] -> [sqrt ( 1-a^2 )/K, 0, atanh ( a )]
 *    [x, y, 0] -> [sqrt ( x^2-y^2 )/K, 0, atanh ( y/x )]
 *    ln [a+1, a-1, 0] -> [2*sqrt ( a )/K, 0, ln ( a )/2]
 *    sqrt [a+ ( K/2 )^2, a-( K/2 )^2, 0] -> [sqrt ( a ), 0, ln ( a* ( 2/K )^2 )/2]
 *    sqrt, ln [a+ ( K/2 )^2, a-( K/2 )^2, -ln ( K/2 )] -> [sqrt ( a ), 0, ln ( a )/2]
 * For linear functions, starting with [x, y, z] and then
 * driving z to 0 gives:
 *    [x, y+x*z, 0]
 * Driving y to 0 gives:
 *    [x, 0, z+y/x]
 */


long X0C, X0H, X0R; /* seed for circular, hyperbolic, and square root */
long OneOverE, E;   /* the base of natural logarithms */
long HalfLnX0R;     /* constant used in simultanous sqrt, ln computation */


/*
 * compute atan ( x ) and atanh ( x ) using infinite series
 * atan ( x ) = x -x^3/3 + x^5/5 -x^7/7 + . . . for x^2 < 1
 * atanh ( x ) = x + x^3/3 + x^5/5 + x^7/7 + . . . for x^2 < 1
 * To calcuate these functions to 32 bits of precision, pick
 * terms[i] s.t. ( ( 2^-i )^ ( terms[i] ) )/ ( terms[i] ) < 2^-32
 * For x <= 2^(-11), atan ( x ) = atanh ( x ) = x with 32 bits of accuracy
 */


static unsigned terms[11] = {
   0, 27, 14, 9, 7, 5, 4, 4, 3, 3, 3
};
static long a[fractionBits -1];
static long latan[fractionBits + 1];
static long latanh[fractionBits + 1];
static long X;
static long Y;
static long Z;


#include <stdio.h> /* putchar is a marco for some */


#define abs( n ) ( n >= 0 ) ? ( n ) : -( n )


/*
 * Reciprocal, calculate reciprocol of n to k bits of precision
 * a and r form integer and fractional parts of the dividend respectively
 */
long Reciprocal( unsigned n, unsigned k )
{
   unsigned i;
   unsigned a = 1;
   long r = 0;


   for ( i = 0; i <= k; ++i )
   {
      r <<= 1;
      if ( a >= n )
      {
         r++;
         a -= n;
      };
      a <<= 1;
   }
   return ( a >= n ? r + 1 : r );/* round result */
}


/* ScaledReciprocal, n comes in funny fixed point fraction representation */
long ScaledReciprocal( long n, unsigned k )
{
   long a;
   long r = 0L;
   unsigned i;


   a = 1L << k;
   for ( i = 0; i <= k; ++i )
   {
      r <<= 1;
      if ( a >= n )
      {
         r++;
         a -= n;
      };
      a <<= 1;
   };
   return ( a >= n ? r + 1 : r );/* round result */
}


/*
 * Poly2 calculates polynomial where the variable is an integral power of 2,
 * log is the power of 2 of the variable
 * n is the order of the polynomial
 * coefficients are in the array a[]
 */
long Poly2( int log, unsigned n )
{
   long r = 0;
   int i;


   for ( i = n; i >= 0; --i )
      r = ( log < 0 ? r >> -log : r << log ) + a[i];
   return ( r );
}


void WriteFraction( long n )
{
   unsigned short i;
   unsigned short low;
   unsigned short digit;
   unsigned long k;


   putchar ( n < 0 ? '-' : ' ' );
   n = abs( n );
   putchar ( ( n >> fractionBits ) + '0' );
   putchar ( '.' );
   low = k = n << ( longBits -fractionBits ); /* align octal point at left */
   k >>= 4; /* shift to make room for a decimal digit */
   for ( i = 1; i <= 8; ++i )
   {
      digit = ( k *= 10L ) >> ( longBits -4 );
      low = ( low & 0xf ) * 10;
      k += ( ( unsigned long ) ( low >> 4 ) ) -
          ( ( unsigned long ) digit << ( longBits -4 ) );
      putchar ( digit + '0' );
   }
}


void WriteRegisters( void )
{
   printf ( " X: " );
   WriteVarious( X );
   printf ( " Y: " );
   WriteVarious( Y );
   printf ( " Z: " );
   WriteVarious( Z );
}


void WriteVarious( long n )
{
   WriteFraction( n );
   printf ( " 0x%08lx 0%011lo %.8f\n", n, n, n*TwoToTheMinusTwentyNinthPower );
}


void Circular( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;


   for ( i = 0; i <= fractionBits; ++i )
   {
      x = X >> i;
      y = Y >> i;
      z = latan[i];
      if ( Z >= 0 )
      {
         X -= y;
         Y += x;
         Z -= z;
      }
      else
      {
         X += y;
         Y -= x;
         Z += z;
      }
   }
}


void InvertCircular( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;


   for ( i = 0; i <= fractionBits; ++i )
   {
      x = X >> i;
      y = Y >> i;
      z = latan[i];
      if ( Y < 0 )
      {
         X -= y;
         Z -= z;
         Y += x;
      }
      else
      {
         X += y;
         Z += z;
         Y -= x;
      }
   }
}


void Hyperbolic( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;


   for ( i = 1; i <= fractionBits; ++i )
   {
      x = X >> i;
      y = Y >> i;
      z = latanh[i];
      if ( Z >= 0 )
      {
         X += y;
         Y += x;
         Z -= z;
      }
      else
      {
         X -= y;
         Y -= x;
         Z += z;
      }
      if ( ( i == 4 ) || ( i == 13 ) )
      {
         x = X >> i;
         y = Y >> i;
         z = latanh[i];
         if ( Z >= 0 )
         {
            X += y;
            Y += x;
            Z -= z;
         }
         else
         {
            X -= y;
            Y -= x;
            Z += z;
         }
      }
   }
}


void InvertHyperbolic( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;
   for ( i = 1; i <= fractionBits; ++i )
   {
      x = X >> i;
      y = Y >> i;
      z = latanh[i];
      if ( Y < 0 )
      {
         X += y;
         Z -= z;
         Y += x;
      }
      else
      {
         X -= y;
         Z += z;
         Y -= x;
      }
      if ( ( i == 4 ) || ( i == 13 ) )
      {
         x = X >> i;
         y = Y >> i;
         z = latanh[i];
         if ( Y < 0 )
         {
            X += y;
            Z -= z;
            Y += x;
         }
         else
         {
            X -= y;
            Z += z;
            Y -= x;
         }
      }
   }
}


void Linear( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;
   z = One;


   for ( i = 1; i <= fractionBits; ++i )
   {
      x >>= 1;
      z >>= 1;
      if ( Z >= 0 )
      {
         Y += x;
         Z -= z;
      }
      else
      {
         Y -= x;
         Z += z;
      }
   }
}


void InvertLinear( long x, long y, long z )
{
   int i;


   X = x;
   Y = y;
   Z = z;
   z = One;


   for ( i = 1; i <= fractionBits; ++i )
   {
      z >>= 1;
      x >>= 1;
      if ( Y < 0 )
      {
         Z -= z;
         Y += x;
      }
      else
      {
         Z += z;
         Y -= x;
      }
   }
}


/*********************************************************/


int main ( int argc, char *argv[] )
{
   int i;
   long r;


   for ( i = 0; i <= 13; ++i )
   {
      a[2 * i] = 0;
      a[2 * i + 1] = Reciprocal( 2 * i + 1, fractionBits );
   }
   for ( i = 0; i <= 10; ++i )
      latanh[i] = Poly2( -i, terms[i] );
   latan[0] = HalfPi / 2; /* latan ( 2^0 )= pi / 4 */
   for ( i = 1; i <= 7; ++i )
      a[4 * i -1] = -a[4 * i -1];
   for ( i = 1; i <= 10; ++i )
      latan[i] = Poly2( -i, terms[i] );
   for ( i = 11; i <= fractionBits; ++i )
      latan[i] = latanh[i] = 1L << ( fractionBits -i );


   printf ( "\nlatanh ( 2^-n )\n" );
   for ( i = 1; i <= 10; ++i )
   {
      printf ( "%2d ", i );
      WriteVarious( latanh[i] );
   }


   r = 0;
   for ( i = 1; i <= fractionBits; ++i )
      r += latanh[i];
   r += latanh[4] + latanh[13];
   printf ( "radius of convergence" );
   WriteFraction( r );
   printf ( "\n\nlatan ( 2^-n )\n" );
   for ( i = 0; i <= 10; ++i )
   {
      printf ( "%2d ", i );
      WriteVarious( latan[i] );
   }


   r = 0;
   for ( i = 0; i <= fractionBits; ++i )
      r += latan[i];
   printf ( "radius of convergence" );
   WriteFraction( r );


   /* all the results reported in the printfs are calculated with my HP-41C */
   printf ( "\n\n--------------------circular functions--------------------\n" );


   printf ( "Grinding on [1, 0, 0]\n" );
   Circular( One, 0L, 0L );
   WriteRegisters();
   printf ( "\n K: " );
   WriteVarious( X0C = ScaledReciprocal( X, fractionBits ) );


   printf ( "\nGrinding on [K, 0, 0]\n" );
   Circular( X0C, 0L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [K, 0, pi/6] -> [0.86602540, 0.50000000, 0]\n" );
   Circular( X0C, 0L, HalfPi / 3L );
   WriteRegisters();


   printf ( "\nGrinding on [K, 0, pi/4] -> [0.70710678, 0.70710678, 0]\n" );
   Circular( X0C, 0L, HalfPi / 2L );
   WriteRegisters();


   printf ( "\nGrinding on [K, 0, pi/3] -> [0.50000000, 0.86602540, 0]\n" );
   Circular( X0C, 0L, 2L * ( HalfPi / 3L ) );
   WriteRegisters();


   printf ( "\n------Inverse functions------\n" );


   printf ( "Grinding on [1, 0, 0]\n" );
   InvertCircular( One, 0L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [1, 1/2, 0] -> [1.84113394, 0, 0.46364761]\n" );
   InvertCircular( One, One / 2L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [2, 1, 0] -> [3.68226788, 0, 0.46364761]\n" );
   InvertCircular( One * 2L, One, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [1, 5/8, 0] -> [1.94193815, 0, 0.55859932]\n" );
   InvertCircular( One, 5L * ( One / 8L ), 0L );
   WriteRegisters();


   printf ( "\nGrinding on [1, 1, 0] -> [2.32887069, 0, 0.78539816]\n" );
   InvertCircular( One, One, 0L );
   WriteRegisters();


   printf ( "\n--------------------hyperbolic functions--------------------\n" );


   printf ( "Grinding on [1, 0, 0]\n" );
   Hyperbolic( One, 0L, 0L );
   WriteRegisters();
   printf ( "\n K: " );
   WriteVarious( X0H = ScaledReciprocal( X, fractionBits ) );
   printf ( " R: " );
   X0R = X0H >> 1;
   Linear( X0R, 0L, X0R );
   WriteVarious( X0R = Y );


   printf ( "\nGrinding on [K, 0, 0]\n" );
   Hyperbolic( X0H, 0L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [K, 0, 1] -> [1.54308064, 1.17520119, 0]\n" );
   Hyperbolic( X0H, 0L, One );
   WriteRegisters();


   printf ( "\nGrinding on [K, K, -1] -> [0.36787944, 0.36787944, 0]\n" );
   Hyperbolic( X0H, X0H, -One );
   WriteRegisters();
   OneOverE = X; /* save value ln ( 1/e ) = -1 */


   printf ( "\nGrinding on [K, K, 1] -> [2.71828183, 2.71828183, 0]\n" );
   Hyperbolic( X0H, X0H, One );
   WriteRegisters();
   E = X; /* save value ln ( e ) = 1 */


   printf ( "\n------Inverse functions------\n" );


   printf ( "Grinding on [1, 0, 0]\n" );
   InvertHyperbolic( One, 0L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [1/e + 1, 1/e -1, 0] -> [1.00460806, 0, -0.50000000]\n" );
   InvertHyperbolic( OneOverE + One, OneOverE -One, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [e + 1, e -1, 0] -> [2.73080784, 0, 0.50000000]\n" );
   InvertHyperbolic( E + One, E -One, 0L );
   WriteRegisters();


   printf ( "\nGrinding on ( 1/2 )*ln ( 3 ) -> [0.71720703, 0, 0.54930614]\n" );
   InvertHyperbolic( One, One / 2L, 0L );
   WriteRegisters();


   printf ( "\nGrinding on [3/2, -1/2, 0] -> [1.17119417, 0, -0.34657359]\n" );
   InvertHyperbolic( One + ( One / 2L ), -( One / 2L ), 0L );
   WriteRegisters();


   printf ( "\nGrinding on sqrt ( 1/2 ) -> [0.70710678, 0, 0.15802389]\n" );
   InvertHyperbolic( One / 2L + X0R, One / 2L -X0R, 0L );
   WriteRegisters();


   printf ( "\nGrinding on sqrt ( 1 ) -> [1.00000000, 0, 0.50449748]\n" );
   InvertHyperbolic( One + X0R, One -X0R, 0L );
   WriteRegisters();
   HalfLnX0R = Z;


   printf ( "\nGrinding on sqrt ( 2 ) -> [1.41421356, 0, 0.85117107]\n" );
   InvertHyperbolic( One * 2L + X0R, One * 2L -X0R, 0L );
   WriteRegisters();


   printf ( "\nGrinding on sqrt ( 1/2 ), ln ( 1/2 )/2 -> [0.70710678, 0, -0.34657359]\n" );
   InvertHyperbolic( One / 2L + X0R, One / 2L -X0R, -HalfLnX0R );
   WriteRegisters();


   printf ( "\nGrinding on sqrt ( 3 )/2, ln ( 3/4 )/2 -> [0.86602540, 0, -0.14384104]\n" );
   InvertHyperbolic( ( 3L * One / 4L ) + X0R, ( 3L * One / 4L ) -X0R, -HalfLnX0R );
   WriteRegisters();


   printf ( "\nGrinding on sqrt ( 2 ), ln ( 2 )/2 -> [1.41421356, 0, 0.34657359]\n" );
   InvertHyperbolic( One * 2L + X0R, One * 2L -X0R, -HalfLnX0R );
   WriteRegisters();
   return ( 0 );
}



/*
[LISTING TWO]


latanh ( 2^-n )


1 0.54930614 0x1193ea7a 002144765172
2 0.25541281 0x082c577d 001013053575
3 0.12565721 0x04056247 000401261107
4 0.06258157 0x0200ab11 000200125421
5 0.03126017 0x01001558 000100012530
6 0.01562627 0x008002aa 000040001252
7 0.00781265 0x00400055 000020000125
8 0.00390626 0x0020000a 000010000012
9 0.00195312 0x00100001 000004000001
10 0.00097656 0x00080000 000002000000


radius of convergence 1.11817300


latan ( 2^-n )


0 0.78539816 0x1921fb54 003110375524
1 0.46364760 0x0ed63382 001665431602
2 0.24497866 0x07d6dd7e 000765556576
3 0.12435499 0x03fab753 000376533523
4 0.06241880 0x01ff55bb 000177652673
5 0.03123983 0x00ffeaad 000077765255
6 0.01562372 0x007ffd55 000037776525
7 0.00781233 0x003fffaa 000017777652
8 0.00390622 0x001ffff5 000007777765
9 0.00195312 0x000ffffe 000003777776
10 0.00097656 0x0007ffff 000001777777


radius of convergence 1.74328660
*/