/* 
 * Utilities for extended precision arithmetic, called by qflt.c.
 * These should all be written in machine language for speed.
 *
 * addm( x, y )  add significand of x to that of y
 * shdn1( x )  shift significand of x down 1 bit
 * shdn8( x )  shift significand of x down 8 bits
 * shdn16( x )  shift significand of x down 16 bits
 * shup1( x )  shift significand of x up 1 bit
 * shup8( x )  shift significand of x up 8 bits
 * shup16( x )  shift significand of x up 16 bits
 * divm( a, b )  divide significand of a into b
 * mulm( a, b )  multiply significands, result in b
 * mdnorm( x )  normalize and round off
 *
 * Copyright (c) 1984 - 1996 by Stephen L. Moshier.  All rights reserved.
 */

#include "qhead.h"

#define N (NQ-2)
#if WORDSIZE != 16
 Error qfltb.c works only with WORDSIZE 16.
#endif

#ifdef ANSIPROT
int mdnorm ( QELT *);
static int mulv(QELT *, QELT *, QELT *, int);
static int squarev(QELT *, QELT *, int);
#else
static int mulv(), squarev();
int mdnorm();
#endif

/*
; Shift mantissa down by 1 bit
*/

int shdn1(x)
register unsigned short *x;
{
register short bits;
int i;

x += 2; /* point to mantissa area */

bits = 0;
for( i=0; i<NQ-1; i++ )
 {
 if( *x & 1 )
  bits |= 1;
 *x >>= 1;
 if( bits & 2 )
  *x |= 0x8000;
 bits <<= 1;
 ++x;
 } 
return 0;
}

/*
; Shift mantissa up by 1 bit
*/
int shup1(x)
register unsigned short *x;
{
register short bits;
int i;

x += NQ;
bits = 0;

for( i=0; i<NQ-1; i++ )
 {
 if( *x & 0x8000 )
  bits |= 1;
 *x <<= 1;
 if( bits & 2 )
  *x |= 1;
 bits <<= 1;
 --x;
 }
return 0;
}



/*
; Shift mantissa down by 8 bits
*/

int shdn8(x)
register unsigned short *x;
{
register unsigned short newbyt, oldbyt;
int i;

x += 2;
oldbyt = 0;
for( i=0; i<NQ-1; i++ )
 {
 newbyt = *x << 8;
 *x >>= 8;
 *x |= oldbyt;
 oldbyt = newbyt;
 ++x;
 }
return 0;
}

/*
; Shift mantissa up by 8 bits
*/

int shup8(x)
register unsigned short *x;
{
int i;
register unsigned short newbyt, oldbyt;

x += NQ;
oldbyt = 0;

for( i=0; i<NQ-1; i++ )
 {
 newbyt = *x >> 8;
 *x <<= 8;
 *x |= oldbyt;
 oldbyt = newbyt;
 --x;
 }
return 0;
}



/*
; Shift mantissa up by 16 bits
*/

int shup16(x)
register short *x;
{
int i;
register short *p;

p = x+2;
x += 3;

for( i=0; i<NQ-2; i++ )
 *p++ = *x++;

*p = 0;
return 0;
}

/*
; Shift mantissa down by 16 bits
*/

int shdn16(x)
register unsigned short *x;
{
int i;
register unsigned short *p;

x += NQ;
p = x+1;

for( i=0; i<NQ-2; i++ )
 *(--p) = *(--x);

*(--p) = 0;
return 0;
}



/*
; add mantissas
; x + y replaces y
*/

int addm( x, y )
unsigned short *x, *y;
{
register unsigned long a;
int i;
unsigned int carry;

x += NQ;
y += NQ;
carry = 0;
for( i=0; i<NQ-1; i++ )
 {
 a = (unsigned long )(*x) + (unsigned long )(*y) + carry;
 if( a & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *y = a;
 --x;
 --y;
 }
return 0;
}

/*
; subtract mantissas
; y - x replaces y
*/

int subm( x, y )
unsigned short *x, *y;
{
register unsigned long a;
int i;
unsigned int carry;

x += NQ;
y += NQ;
carry = 0;
for( i=0; i<NQ-1; i++ )
 {
 a = (unsigned long )(*y) - (unsigned long )(*x) - carry;
 if( a & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *y = a;
 --x;
 --y;
 }
return 0;
}


int divm( a, b )
unsigned short a[], b[];
{
unsigned short sqr[NQ+2], prod[NQ+2], quot[NQ+2];
int i, prec, k;
unsigned short d, qu, *p;
unsigned long u;

/* Test if denominator has only 16 bits of significance. */
p = &a[4];
i = NQ-4;
do
 {
 if( *p++ != 0 )
  goto longdiv;
 }
while( --i );

/* Do single precision divides if so. */
qmov( b, prod );
prod[NQ] = 0;
prod[NQ+1] = 0;
shdn1( prod );
shdn1( prod );
d = a[3];
u = ((unsigned long )prod[3] << 16) | prod[4];
for( i=3; i<NQ; i++ )
 {
 qu = u / d;
 prod[i] = qu;
 u = ((u - (unsigned long )d * qu) << 16) | prod[i+2];
 }
prod[NQ] = u / d;
goto divdon;


longdiv:

/* Slower procedure is required */
qclear(quot);
quot[NQ] = 0;
qclear(prod);
qclear(sqr);
quot[3] = ((unsigned long )0x40000000) / (unsigned long )a[3];
prec = 2;
k = 1;
while( prec < NQ-2 )
 {
 k = 2 * k;
 if( k > NQ-2 )
  prec = NQ - 2;
 else
  prec = k;
 squarev( quot, sqr, prec );
 mulv( a, sqr, prod, prec );
 subm( prod, quot );
 shup1( quot );
 }
mulv( quot, b, prod, NQ-2 );
prod[0] = b[0];
prod[1] = b[1];

divdon:

mdnorm( prod );
qmov( prod, b );
return 0;
}

/*
prtemp( s, z )
char *s;
unsigned short z[];
{
int i;

printf( "%s ", s );
for( i=0; i<8; i++ )
 printf( "%04x ", z[i+2] );
printf( "\n" );
}
*/


/* Variable precision multiply of significands.
 * c must not be in the same location as either a or b.
 */
static int mulv( a, b, c, prec )
unsigned short a[], b[], c[];
int prec;
{
register unsigned short *p, *q, *r;
register unsigned long u, lp;
unsigned short carry;
int k, i;

k = prec+2;
p = &c[2];
do
 *p++ = 0;
while( --k );

r = &c[prec+3];
for( k=prec+2; k>=3; k-- )
{
q = &b[3];
p = &a[k];
for( i=k; i>=3; i-- )
 {
 if( (*p == 0) || (*q == 0) )
  {
  --p;
  ++q;
  continue;
  }
 lp = (unsigned long )(*p--) * (unsigned long )(*q++);
 u = (unsigned long )(*r) + (lp & 0xffff);
 if( u & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *r-- = u;
 u = (unsigned long )(*r) + ((lp >> 16) & 0xffff) + carry;
 *r-- = u;
 if( u & 0x10000 )
  *r += 1;
 r += 2;
 }
--r;
}
return 0;
}



/* Variable precision square.
 * b must be in a different location from a.
 */
static int squarev( a, b, prec )
unsigned short a[], b[];
int prec;
{
register unsigned short *p, *q, *r;
register unsigned long u, lp;
unsigned short carry;
int k;

k = prec+2;
p = &b[2];
do
 *p++ = 0;
while( --k );

r = &b[prec+3];
for( k=prec+2; k>=3; k-- )
{
q = &a[3];
p = &a[k];
while( p >= q ) 
 {
 if( (*p == 0) || (*q == 0) )
  {
  --p;
  ++q;
  continue;
  }
/* printf( "%d %d %d\n", p - &a[3], q - &a[3], r - &b[3] );*/
 lp = (unsigned long )(*p) * (unsigned long )(*q);
 if( p != q )
  {
  if( lp & 0x80000000 )
   *(r-2) += 1;
  lp <<= 1;
  }
 --p;
 ++q;
 u = (unsigned long )(*r) + (lp & 0xffff);
 if( u & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *r-- = u;
 u = (unsigned long )(*r) + ((lp >> 16) & 0xffff) + carry;
 *r-- = u;
 if( u & 0x10000 )
  *r += 1;
 r += 2;
 }
--r;
}
shup1(b);
return 0;
}





int mulm( b, ac3 )
unsigned short b[], ac3[];
{
register unsigned short *p, *q;
unsigned short act[NQ+2];
unsigned short carry, *r;
unsigned long lp, a;
int i, k, m, o;

qclear( act );
act[0] = ac3[0];
act[1] = ac3[1];
act[NQ] = 0;
act[NQ+1] = 0;
r = &act[NQ+1];
for( k=NQ; k>=3; k-- )
{
if( k == NQ )
 {
 m = NQ-1;
 o = 4;
 }
else
 {
 m = k;
 o = 3;
 }
q = &ac3[o];
p = &b[m];

for( i=m; i>=o; i-- )
 {
 if( (*p == 0) || (*q == 0) )
  {
  --p;
  ++q;
  continue;
  }
 lp = (unsigned long )(*p--) * (unsigned long )(*q++);
 a = (unsigned long )(*r) + (lp & 0xffff);
 if( a & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *r-- = a;
 a = (unsigned long )(*r) + ((lp >> 16) & 0xffff) + carry;
 *r-- = a;
 if( a & 0x10000 )
  *r += 1;
 r += 2;
 }
--r;
}
mdnorm( act );
qmov( act, ac3 );
return 0;
}




int mulin( b, ac3 )
unsigned short b[], ac3[];
{
register unsigned short *p, *r;
unsigned short act[NQ+1];
unsigned short carry, y;
unsigned long lp, a;
int i;

qclear( act );
act[0] = ac3[0];
act[1] = ac3[1];
act[NQ] = 0;
r = &act[NQ];
y = b[3];
p = &ac3[NQ-1];
for( i=NQ-1; i>=3; i-- )
 {
 if( *p == 0 )
  {
  --p;
  --r;
  continue;
  }
 lp = (unsigned long )(*p--) * y;
 a = (unsigned long )(*r) + (lp & 0xffff);
 if( a & 0x10000 )
  carry = 1;
 else
  carry = 0;
 *r-- = a;
 a = (unsigned long )(*r) + ((lp >> 16) & 0xffff) + carry;
 *r-- = a;
 if( a & 0x10000 )
  *r += 1;
 ++r;
 }
mdnorm( act );
qmov( act, ac3 );
return 0;
}


unsigned short rndbit[NQ+1];
short rndset = 0;

int mdnorm( x )
unsigned short x[];
{
int i;
register unsigned short *p;

if( rndset == 0 )
 {
 qclear( rndbit );
 rndbit[NQ-1] = 1;
 rndbit[NQ] = 0;
 rndset = 1;
 }
p = &x[1];
for( i=0; i<3; i++ )
 {
 if( x[2] == 0 )
  break;
 shdn1(x);
 if( *p < MAXEXP )
  *p += 1;
 else
  {
  *p = MAXEXP; 
  mtherr("mdnorm", OVERFLOW);
  }
 }
for( i=0; i<3; i++ )
 {
 if( x[3] & 0x8000 )
  break;
 /* Prevent exponent underflow.
   Rounding may be incorrect when this happens.  */
 if( *p >= 1 )
  {
  shup1(x);
  *p -= 1;
  }
 else
  mtherr("mdnorm", UNDERFLOW);
 }

if( x[NQ] & 0x8000 )
 {
/*
 if(  ((x[NQ] & 0x8000) == 0x8000) && ((x[NQ-1] & 1) == 0)  )
  goto nornd;
*/
 addm( rndbit, x );
 }
if( x[2] )
 {
 shdn1( x );
 if( *p < MAXEXP )
  *p += 1;
 else
  {
  *p = MAXEXP; 
  mtherr("mdnorm", OVERFLOW);
  }
 }
/* nornd: */
x[NQ] = 0;
return 0;
}

/*
; move a to b
;
; short a[NQ], b[NQ];
; qmov( a, b );
*/

int qmov( a, b )
register unsigned short *a, *b;
{
register int i;

i = NQ;
do
 {
 *b++ = *a++;
 }
while( --i );
return 0;
}


int qmovz( a, b )
register unsigned short *a, *b;
{
register int i;

i = NQ;
do
 {
 *b++ = *a++;
 }
while( --i );
*b++ = 0;
return 0;
}


/*
; Clear out entire number, including sign and exponent, pointed
; to by x
;
; short x[];
; qclear( x );
*/

int qclear( x )
register unsigned short *x;
{
register int i;

for( i=0; i<NQ; i++ )
 *x++ = 0;
return 0;
}