#include <gpk_std.h>
#include <readwrite.h>
#include "floatfmt.h"
#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <mylib.h>
#include <float.h>

LOCAL struct b2float make_b2float(double x)
{
 struct b2float rv;
 int tmp, tmp2;
 if(x == 0.0)	{
 	rv.mantissa = 0;	rv.exponent = 0;
	return rv;
	}
 tmp = round(ceil(log(fabs(x))/log(2.0)));
 if(tmp < -128)
 	tmp = -128;
 if(tmp > 127)	{
	fprintf(stderr, "make_b2float: INPUT TOO BIG: x=%g\n", x);
	abort();
	}
 for(;(tmp2 = round(x/pow(2.0, tmp-7)))>127 || tmp2<-128;tmp++)
	;
 if(tmp > 127)	{
	fprintf(stderr, "make_b2float: INPUT TOO BIG2: x=%g\n", x);
	abort();
	}
 rv.exponent = tmp;
 rv.mantissa = tmp2;

 if(fabs(b2float_double(rv)-x) > 0.01*(fabs(x)+pow(2.0, -128.0)))	{
	fprintf(stderr, "make_b2float: ERROR, x=%g, b2f(rv)=%g, exp=%d, man=%d\n",
					x, b2float_double(rv), rv.exponent, rv.mantissa);
	abort();
	}
 return rv;
}

LOCAL int round_1b(double x)
{
 LOCAL double c0 = (127 + (127 + (127.0/256.0))/256.0)/256.0;
 return x>0 ? (int)(x+1-c0) : -(int)(c0-x);
}

LOCAL int round_2b(double x)
{
 LOCAL double c0 = (127 + (127/256.0))/256.0;
 return x>0 ? (int)(x+1-c0) : -(int)(c0-x);
}

LOCAL int round_3b(double x)
{
 LOCAL double c0 = 127/256.0;
 return x>0 ? (int)(x+1-c0) : -(int)(c0-x);
}

LOCAL struct b4float make_b4float(double x)
{
 struct b4float rv;
 int tmp, tmp0, tmp1, tmp2;
 BOOL again1 = FALSE, again2=FALSE;

 if(x == 0.0)	{
 	rv.mantissa[0] = rv.mantissa[1] = rv.mantissa[2] = 0;
	rv.exponent = 0;
	return rv;
	}
 tmp = round(ceil(log(fabs(x))/log(2.0)));
 if(tmp < -128)
 	tmp = -128;
 if(tmp > 127)	{
	fprintf(stderr, "make_b4float: INPUT TOO BIG: x=%g\n", x);
	abort();
	}
 rv.exponent = tmp;
 rv.mantissa[1] = rv.mantissa[2] = 0;

 do	{
 	tmp0 = round_1b(x/pow(2.0, rv.exponent-7));
	if(tmp0>127 || tmp0<-128)	{
		assert(rv.exponent < 127);
 		if(tmp >= 127)	{
			fprintf(stderr, "make_b4float: INPUT TOO BIG2: x=%g\n", x);
			abort();
			}
		rv.exponent++;
 		tmp0 = round_1b(x/pow(2.0, rv.exponent-7));
		assert(tmp0>=-128 && tmp0<=127);
		}
	rv.mantissa[0] = tmp0;
	rv.mantissa[1] = 0;
 	tmp1 = round_2b((x-b4float_double(rv))/pow(2.0, rv.exponent-7-8));
	/* printf("x=%25.22g exp=%d m0=%d m1=%d\n", x, tmp, tmp0, tmp1); */
	assert(tmp1>=-128 && tmp1<=127);
	rv.mantissa[1] = tmp1;
	rv.mantissa[2] = 0;
	tmp2 = round_3b((x-b4float_double(rv))/pow(2.0, rv.exponent-7-16));
	assert(tmp2>=-128 && tmp2<=127);
	/* printf("exp=%d m0=%d m1=%d m2=%d\n", tmp, tmp0, tmp1, tmp2); */
	/* printf("loopend\n"); */
	} while(again1);
 rv.mantissa[2] = tmp2;

 if(tmp > -128)
 	assert(fabs(b4float_double(rv)-x) <= 3e-6*(fabs(x)+pow(2.0, -128.0)));

 if(!(fabs(b4float_double(rv)-x) < 3e-6*fabs(x)))	{
		fprintf(stderr, "A exp=%d m0=%d m1=%d m2=%d\n", tmp, tmp0, tmp1, tmp2);
		exit(0);
		}

 return rv;
}


BOOL write_double_2(FILE *fp, double q)
{
 C struct b2float tmp = make_b2float(q);

 return write_schar_1(fp, tmp.mantissa)
 			&& write_schar_1(fp, tmp.exponent);
}


BOOL write_double_4(FILE *fp, double q)
{
 C struct b4float tmp = make_b4float(q);

 return write_schars(fp, 3, tmp.mantissa)
 			&& write_schar_1(fp, tmp.exponent);
}
