#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <sys/time.h>

#define BYTE_TO_BINARY_PATTERN "%c%c%c%c%c%c%c%c"
#define BYTE_TO_BINARY(byte)  \
	(byte & 0x80 ? '1' : '0'), \
	(byte & 0x40 ? '1' : '0'), \
	(byte & 0x20 ? '1' : '0'), \
	(byte & 0x10 ? '1' : '0'), \
	(byte & 0x08 ? '1' : '0'), \
	(byte & 0x04 ? '1' : '0'), \
	(byte & 0x02 ? '1' : '0'), \
	(byte & 0x01 ? '1' : '0') 

//https://stackoverflow.com/questions/111928/is-there-a-printf-converter-to-print-in-binary-format
void print_byte(char b)
{
	printf(BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(b));
}

void print_hex(float f)
{
	int i = *(int*)&f;
	printf("[%x %02x %06x] ", 
		(i&0xf0000000)>>31, 
		(i&0x7f800000)>>23, 
		 i&0x007fffff);
}

void print_decompose(float f)
{
	int i = *(int*)&f;
	if((i&0x8000000)>>31)
		printf("-");
	else
		printf("+");

	printf("1.");
	print_byte((i&0x007f8000)>>15);
	print_byte((i&0x00007f80)>>7);
	print_byte((i&0x0000007f)<<1>>7);

	printf("x2^");
	printf("%d ", ((i&0x7f800000)>>23)-127);

}

void print_f(const char* s, float f)
{
	printf(s);
	int i = *(int*)&f;
	print_decompose(f);
	print_hex(f);
	printf("%f %08x\n", f, i);
}

void print_fi(const char* s, int i)
{
	print_f(s, *(float*)&i);
}

float show_q3_method(float number)
{
	int i;
	int c = 0x5f3759df;
	float y;

	y = number;

	print_f("q3 input  ", y);
	print_fi(" constant ", c);

	i = *(int*)&y;
	i = c - (i>>1);
	y = *(float*)&i;
	print_f(" estimate ", y);

	y = y * (1.5 - (0.5*number * y * y));
	print_f(" newton1  ", y);
	y = y * (1.5 - (0.5*number * y * y));
	print_f(" newton2  ", y);
	return y;
}

float show_blinn_method(float number)
{
	float y = number;
	float c = 1.0f;
	int ci = *(int*)&c;
	int i = *(int*)&y;

	print_f("blinn in  ", y);
	print_f(" one      ", c);

	ci = ci + (ci>>1);
	print_fi(" 1 + 1>>1 ", ci);
	i = ci - (i>>1);
	y = *(float*)&i;
	print_f(" estimate ", y);

	y = y * (1.5 - (0.5*number * y * y));
	print_f(" newton1  ", y);
	y = y * (1.5 - (0.5*number * y * y));
	print_f(" newton2  ", y);
	return y;
}

void timing_loop(const char* str, float f, float (*isqrt)(float) )
{
	double time1, time2;
	struct timeval tv;
	printf("Running %s... ", str);
	fflush(stdout);

	float y = f;
	long long max_i = 0x0fffffff;

	gettimeofday(&tv, NULL);
	time1 = tv.tv_sec + tv.tv_usec/1000000.0;

	for(long long i = 0; i<max_i; i++) {
		y = isqrt(y);
	}
	gettimeofday(&tv, NULL);
	time2 = tv.tv_sec + tv.tv_usec/1000000.0;

	printf("%f in %fs\n", y, time2-time1);
}

float float_isqrt(float number)
{
	float y;
	y = 1.0f / sqrtf(number);
	return y;
}

float blinn_isqrt(float number)
{
	float y = number;
	float c = 1.0f;
	int ci = *(int*)&c;
	int i = *(int*)&y;

	ci = ci + (ci>>1);
	i = ci - (i>>1);
	y = *(float*)&i;

	//y = y * (1.5f - (0.5f*number * y * y));
	//y = y * (1.5f - (0.5f*number * y * y));
	return y;

}

float q3_isqrt(float number)
{
	int i;
	int c = 0x5f3759df;
	float y;

	y = number;
	i = *(int*)&y;
	i = c - (i>>1);
	y = *(float*)&i;

	//y = y * (1.5f - (0.5f*number * y * y));
	//y = y * (1.5f - (0.5f*number * y * y));
	return y;
}


int main(int argc, char** argv)
{
	float v;
	if(argc < 2) {
		printf("Usage: %s number\n", argv[0]);
		printf("Will return the inverse square root of the number\n");
		printf("Defaulting to 5.0f\n");
		v = 5.0f;
	}
	else {
		v = atof(argv[1]);
	}

	printf("invsqrt(%f) = %f\n", v, 1.0f/sqrt(v));

	show_blinn_method(v);
	show_q3_method(v);

	timing_loop("std float", v, float_isqrt);
	timing_loop("    blinn", v, blinn_isqrt);
	timing_loop("       q3", v, q3_isqrt);
	
	return 0;

}