/* 
 * 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 - 1988 by Stephen L. Moshier.  All rights reserved.
 */

#include "qhead.h"
#define N (NQ-2)

/*
;	Shift mantissa down by 1 bit
*/

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;
	}	
}

/*
;	Shift mantissa up by 1 bit
*/
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;
	}
}



/*
;	Shift mantissa down by 8 bits
*/

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;
	}
}

/*
;	Shift mantissa up by 8 bits
*/

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;
	}
}



/*
;	Shift mantissa up by 16 bits
*/

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;
}

/*
;	Shift mantissa down by 16 bits
*/

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;
}



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

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;
	}
}

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

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;
	}
}


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, *q, *r;
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 );
}

/*
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;
}
}



/* Variable precision square.
 * b must be in a different location from a.
 */
static 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, i;

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);
}





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, y;
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 );
}




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 );
}


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

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

if( rndset == 0 )
	{
	qclear( rndbit );
	rndbit[NQ-1] = 1;
	rndbit[NQ] = 0;
	rndset = 1;
	}
p = (short *)&x[1];
for( i=0; i<3; i++ )
	{
	if( x[2] == 0 )
		break;
	shdn1(x);
	*p += 1;
	if( *p < 0 )
		*p = 32767; 
	}
for( i=0; i<3; i++ )
	{
	if( x[3] & 0x8000 )
		break;
	shup1(x);
	*p -= 1;
	if( *p < 0 )
		*p = 0;
	}

if( x[NQ] & 0x8000 )
	{
/*
	if(  ((x[NQ] & 0x8000) == 0x8000) && ((x[NQ-1] & 1) == 0)  )
		goto nornd;
*/
	addm( rndbit, x );
	}
if( x[2] )
	{
	shdn1( x );
	*p += 1;
	if( *p < 0 )
		*p = 32767;
	}
nornd:
x[NQ] = 0;
}

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

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

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


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

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