#include <stdio.h>
#include <math.h>

#if 0
#define FPTYPE float
#define IEEEOFS 0
#define INITGUESS 0xBE700000u
/* #define INITGUESS 0xBE6EC85Eu */
#else
#define FPTYPE double
#define IEEEOFS 1
#define INITGUESS 0xBFCD4600u
/* #define INITGUESS 0xBFCDD90Au */
#endif

FPTYPE fsqrta (FPTYPE y, unsigned long a) {
FPTYPE x, tempf;
unsigned long *tfptr = ((unsigned long *)&tempf) + IEEEOFS;

	tempf = y;
	*tfptr = (a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
	x =  tempf;
#if 0
	{
		FPTYPE z;
		z =  y*0.5;                 /* hoist out the "/2"    */
		x = (1.5*x) - (x*x)*(x*z);  /* iteration formula     */
		x = (1.5*x) - (x*x)*(x*z);
		x = (1.5*x) - (x*x)*(x*z);
		x = (1.5*x) - (x*x)*(x*z);
		x = (1.5*x) - (x*x)*(x*z);
	}
	x *= y;
#endif
	return x;
}

FPTYPE fsqrt (FPTYPE y) {
	return fsqrta (y, 0xBE6F0000);
}


#define NUM 10000000

FPTYPE tab[NUM];

struct deviation {
	int up: 1;
	int down: 1;
	int count;
	unsigned long tv;
};

#define SCALE(i) (1.0f + (i) * (3.0f / (FPTYPE) NUM))
#define RELDEVIANCE(y1,y2) fabs(((y1)-(y2))/(y2))
// #define DEVIANCE(y1,y2) fabs((y1)-(y2))
#define DEVIANCE(y1,y2) RELDEVIANCE(y1,y2)

FPTYPE test (unsigned long l, struct deviation * dev) {
FPTYPE d = 0.0f;
FPTYPE x, y, q;
int i, up, down;

	up = down = 0;
	dev->count = 0;
	dev->tv = l;

	for (i=0; i < NUM; i++) {
		x = SCALE(i);
		y = fsqrta(x, l);
		q = DEVIANCE (y, tab[i]);
		if (q >= d) {
			if (q > d) {
				d = q;
				dev->count = 1;
				if (y > tab[i]) {
					up = 1;
					down = 0;
				} else {
					up = 0;
					down = 1;
				}
			} else {
				dev->count++;
				if (y > tab[i]) {
					up = 1;
				} else {
					down = 1;
				}
			}
		}
	}

	printf ("%08x -> dev: %10.8f  %7d  ", l, d, dev->count);
	if (up) printf ("^"); else printf (" ");
	if (down) printf ("V"); else printf (" ");
	printf ("\n");

	dev->up = up;
	dev->down = down;

	return d;
}

int main () {
FPTYPE d = 1e10;
struct deviation dev;
int i, r;
unsigned long t, ot;
unsigned long td;

	t  = INITGUESS; /* Initial guess for the offset */
	td = 0x08000000u; /* Starting perturbation */
	ot = t;

	for (i=0; i < NUM; i++) {
		FPTYPE x = SCALE(i);
		tab[i] = 1/sqrt(x);  /* Precalculate the real values */
	}

	while (td > 0) {
		FPTYPE ld = test (t, &dev);
		printf ("deviation %08x: %10.8f\n", t, ld);

		if (ld < d) {
			d = ld;
			ot = t; /* Accept this improvement */
		} else {
			t = ot; /* Back out, we perturbed too far */
			printf ("Reduce perturbation from %08x\n", td);
		}

		/* Perturb to compensate for the direction of the deviance */
		if (0 == dev.down) {
			t -= td;
		} else if (0 == dev.up) {
			t += td;
		} else {
			/* If we have equal up and down deviance stop */
			break;
		}

		td = (unsigned long) (0.75f * td); /* Reduce perturbation */
	}

	t = ot;
	printf ("t = %08x\n", t);

	ot = t;
	/* Search neighborhood */
	for (td = 1; td < 3; td++) {
		struct deviation dev2;
		FPTYPE q;

		if (d > (q = test (t-td, &dev2))) {
			dev = dev2;
			d = q;
			ot = t-td;
		}

		if (d > (q = test (t+td, &dev2))) {
			dev = dev2;
			d = q;
			ot = t+td;
		}
	}
	t = ot;

	printf ("t = %08lx  %08lx\n", t, dev.tv);

	/* Search neighborhood for least incidences with maximum deviance */
	for (r = 2, td = 1; r; td++) {
		struct deviation dev2;
		int f = 0;

		if (d == test (t-td, &dev2)) {
			f = 1;
			if (dev2.count < dev.count) {
				dev = dev2;
			}
		}

		if (d == test (t+td, &dev2)) {
			f = 1;
			if (dev2.count < dev.count) {
				dev = dev2;
			}
		}

		if (0 == f) r--;
	}

	/* Return the result */
	d = test (t, &dev);
	printf ("Optimized offset: %08lx %10.8f\n", t, d);

	return 0;
}