/* --------------------------------------------------------------- */
/* nFP.tea  (c) Copyright Mike Noel, 2006, 2007  mike-noel@gci.net */
/* --------------------------------------------------------------- */
/* Floating point emulation library for Acroname Brainstem.        */
/* --------------------------------------------------------------- */
/* Floating point numbers are represented by a signed integer      */
/* mantissa and associated signed char binary exponent. Binary     */
/* in this context means the base is two, so if the exponent is    */
/* 0x0c (12) that means multiply the mantissa by 2^12.             */
/*                                                                 */
/* These routines have NOT been extensively tested and almost      */
/* certainly contain serious undiscovered bugs.                    */
/*                                                                 */
/*                    Use at your own risk!                        */
/*                                                                 */
/* Code is too big for a single 1k slot, so it's only useful on an */
/* Acroname Moto board where it can reside in the 16k slot along   */
/* with whatever of your code needs to use it, or if you go to the */
/* trouble to load your code 'across' slots.                       */
/* --------------------------------------------------------------- */


#ifndef _nFP_T_
#define _nFP_T_


// globals
int  nFP_mant=0;
char nFP_exp=0, nFP_error=0;


// constants for return values in global nFP_error
#define	nFP_err_none		0
#define	nFP_err_divzero		1
#define	nFP_err_toobigforint	2
#define	nFP_err_expoverflow	3
#define	nFP_err_expundrflow	4

// constants for normalized values of common numbers
#define nFP_zero		0,0
#define nFP_one			16384,-14
#define nFP_two			16384,-13
#define nFP_three		24576,-13
#define nFP_four		16384,-12
#define nFP_five		20480,-12
#define nFP_six			24576,-12
#define nFP_seven		28672,-12
#define nFP_eight		16384,-11
#define nFP_nine		18432,-11
#define nFP_ten			20480,-11
#define nFP_100			25600,-8
#define nFP_1000		32000,-5
#define nFP_Pi			25735,-13
#define nFP_PiDiv180		18301,-20
#define nFP_180divPi		29335,-9


void nFP_add(int mant1, char exp1, int mant2, char exp2) {
// add the two numbers and store the result in globals nFP_mant & nFP_exp

int e1, e2, expdiff;
unsigned int U;
char s1=1, s2=1;

	// set no error
	nFP_error = nFP_err_none;

	// speciel fast handling for add zero
	if (mant1 == 0) { nFP_mant=mant2; nFP_exp=exp2; return; }
	if (mant2 == 0) { nFP_mant=mant1; nFP_exp=exp1; return; }

	// make mantissa's large (in case not already normalized)
	e1 = exp1; e2 = exp2; 
	if (mant1<0) s1=-1; mant1 = s1 * mant1;
	while (mant1<16384) { mant1 = mant1 << 1; e1--; }
	if (mant2<0) s2=-1; mant2 = s2 * mant2;
	while (mant2<16384) { mant2 = mant2 << 1; e2--; }

	// make exponents equal
	expdiff = e1 - e2;
	if (expdiff > 0) {
		nFP_mant=s1 * mant1; nFP_exp =exp1;
		if (expdiff > 14) return;
		mant2 = mant2 >> expdiff; e2 = e2 + expdiff;
		}
	else
	if (expdiff < 0) {
		nFP_mant=s2 * mant2; nFP_exp =exp2;
		if (expdiff < -14) return;
		mant1 = mant1 >> -expdiff; e1 = e1 - expdiff;
		}

	// add mantissa's
	if (s1 == s2) {	// overflow possible if same sign
		U=(unsigned int)mant1+(unsigned int)mant2;
		if(U < (unsigned int)32768) nFP_mant = s1 * U;
		else {	// it did overflow so reduce precision...
			nFP_mant = s1 * ((mant1 >> 1) + (mant2 >> 1));
			e1  = e1 + 1;
			}
		}	// no overflow if different sign
	else nFP_mant = s1 * mant1 + s2 * mant2;
	
	// renormalize
	if (nFP_mant == 0) { nFP_exp = 0; return; }
	s1 = 1; if (nFP_mant<0) s1=-1; nFP_mant = s1 * nFP_mant;
	while (nFP_mant<16384) { nFP_mant = nFP_mant << 1; e1--; }
	nFP_mant = nFP_mant * s1;
	
	// check to make sure no exp over or under flow 
	if (e1 < -128) nFP_error = nFP_err_expundrflow;
	else
	if (e1 > 127) nFP_error = nFP_err_expoverflow;
	else
	nFP_exp = (char)e1;
}


#define	nFP_subtract(m1, e1, m2, e2) nFP_add(m1, e1, -m2, e2)
// subtract the second number from the first and store the 
// result in globals nFP_mant & nFP_exp


int  nFP_compare(int mant1, char exp1, int mant2, char exp2) {
// compare the second number to the first and 
// return comparison value suitable for branching

int savemant;
char saveexp, signof;

	savemant=nFP_mant; saveexp=nFP_exp;
	nFP_subtract(mant1, exp1, mant2, exp2);
	signof=(char)nFP_intsign(nFP_mant);
	nFP_mant=savemant; nFP_exp=saveexp;
	return ((int)signof);
}


void nFP_multiply(int mant1, char exp1, int mant2, char exp2) {
// multiply the two numbers and store result in globals nFP_mant & nFP_exp

unsigned int one4;
int dd;
char e=0, xone4;
char s1=1, s2=1;
	
	// speciel fast handling for multiply by zero
	if (mant1 == 0) { nFP_mant=0; nFP_exp=0; return; }
	if (mant2 == 0) { nFP_mant=0; nFP_exp=0; return; }

	// make mantissa's large (in case not already normalized)
	if (mant1<0) s1=-1; mant1 = s1 * mant1;
	while (mant1<16384) { mant1 = mant1 << 1; exp1--; }
	if (mant2<0) s2=-1; mant2 = s2 * mant2;
	while (mant2<16384) { mant2 = mant2 << 1; exp2--; }

	// multiply the lowest nibbles
	one4 = (mant1 & 255) * (mant2 & 255);
	xone4 = 0;
	if (one4 > 32767) { one4 = one4 >> 1; xone4++; }
	nFP_mant = one4; nFP_exp = xone4;
	
	// multiply the 'cross' nibbles
	one4 = ((mant1>>8) & 255) * (mant2 & 255);
	xone4=8;
	if (one4 > 32767) { one4 = one4 >> 1; xone4++; }
	nFP_add(nFP_mant, nFP_exp, one4, xone4);
	if (nFP_error > e) e=nFP_error;
	
	one4 = (mant1 & 255) * ((mant2>>8) & 255);
	xone4=8;
	if (one4 > 32767) { one4 = one4 >> 1; xone4++; }
	nFP_add(nFP_mant, nFP_exp, one4, xone4);
	if (nFP_error > e) e=nFP_error;
	
	// multiply the high nibbles
	one4 = ((mant1>>8) & 255) * ((mant2>>8) & 255);
	xone4=16;
	if (one4 > 32767) { one4 = one4 >> 1; xone4++; }
	nFP_add(nFP_mant, nFP_exp, one4, xone4);
	if (nFP_error > e) e=nFP_error;
	
	nFP_mant = nFP_mant * s1 * s2;
	dd  = (int)nFP_exp + (int)exp1 + (int)exp2;
	nFP_exp=(char)dd;
	if (dd > 127) e = nFP_err_expoverflow;
	if (dd < -128) e = nFP_err_expundrflow;
	nFP_error = e;
}


void nFP_divide(int mant1, char exp1, int mant2, char exp2) {
// divide the first number by the second and store the result
// in globals nFP_mant & nFP_exp

	nFP_reciprocal(mant2, exp2);
	if(nFP_error != nFP_err_none) return;
	nFP_multiply(mant1, exp1, nFP_mant, nFP_exp);	
}


void nFP_reciprocal(int mant2, char exp2) {
// compute reciprocal and store the result in globals nFP_mant & nFP_exp

int e2, guess, gx;
char s2=1;

// following constants 16384,-14 are nFP_one, aka '1'
#define mant1 16384
#define e1    -14

	// set no error
	nFP_error = nFP_err_none;

	// test for devisor mantissa zero
	if (mant2 == 0) { nFP_error = nFP_err_divzero; return; }

	// make divisor mantissa large (in case not already normalized)
	e2 = exp2; 
	if (mant2<0) s2=-1;
	mant2 = s2 * mant2;
	while (mant2<16384) { mant2 = mant2 << 1; e2--; }

	// make initial guess by reducing divisor precision
	// (guess should be within a few % of final result)
	guess = (mant1 / (mant2 >> 8));
	gx = e1 - (e2 + 8);
	if (gx > 127)  { nFP_error = nFP_err_expoverflow; return; }
	if (gx < -128) { nFP_error = nFP_err_expundrflow; return; }
	nFP_mant = guess; nFP_exp = (char)gx;
	
	// refine the guess by y = y * (2 - yd) (newton raphson)
	do	{
		guess = nFP_mant; gx = nFP_exp;
		nFP_multiply(guess, (char)gx, mant2, (char)e2);
		if(nFP_error != nFP_err_none) return;
		nFP_subtract(2, 0, nFP_mant, nFP_exp);
		if(nFP_error != nFP_err_none) return;
		nFP_multiply(nFP_mant, nFP_exp, guess, (char)gx);
		if(nFP_error != nFP_err_none) return;
		}
	while ((nFP_exp != gx) || (nFP_intabsval(nFP_mant-guess)>10));
	nFP_mant = s2 * nFP_mant;
	
#undef mant1
#undef e1
}


void nFP_normalize() {
// normalize nFP_mant and nFP_exp so mantissa is as large as possible

int s=1;

	nFP_error = nFP_err_none;
	if (nFP_mant == 0) return;
	if (nFP_mant<0) s=-1;
	nFP_mant = s * nFP_mant;
	while (nFP_mant<16384) {
		nFP_mant = nFP_mant << 1;
		if (nFP_exp > -128) nFP_exp = nFP_exp - 1;
		else {
			nFP_error = nFP_err_expundrflow;
			return;
			}
		}
	nFP_mant = nFP_mant * s;
}


int nFP_denormalize() {
//make exp zero (so that number is integer again)

	nFP_error = nFP_err_none;
	if (nFP_mant == 0) return(0);
	while (nFP_exp > 0) {
		nFP_exp = nFP_exp - 1;
		if (nFP_intabsval(nFP_mant) > 16383)
			nFP_error = nFP_err_toobigforint;
		else
			nFP_mant = nFP_mant * 2;		
		}
	while (nFP_exp < 0) {
		nFP_exp = nFP_exp + 1;
		nFP_mant = nFP_mant / 2;		
		}
	return (nFP_mant);
}


int nFP_intabsval(int theint) {
// similiar to aMath_Absval

	if(theint<0) return(-theint);
	return(theint);
}


int nFP_intsign(int theint) {
// similiar to aMath_Signum but for zero it returns 1 instead of 0

	if(theint<0) return(-1);
	return(1);
}


#endif /* _nFP_T_ */