#ifndef SQRTS_RSQRT_CARMACK_H
#define SQRTS_RSQRT_CARMACK_H

#include <cstdint>
#include <cmath>
#include <cstring>

namespace carmack
{

// original, as copied from the Quake 3 source, without useless preprocessor code
inline float Q_rsqrt(float number)
{
	long i;
	float x2, y;
	const float threehalfs = 1.5F;
	
	x2 = number * 0.5F;
	y  = number;
	i  = * (long *) &y;						// evil floating point bit level hacking
	i  = 0x5f3759df - (i >> 1);                 // what the fuck?
	y  = * (float *) &i;
	y  = y * (threehalfs - (x2 * y * y));       // 1st iteration
	
	return y;
}

inline float Q_frsqrt(float number)
{
	long i;
	float x2, y;
	const float threehalfs = 1.5F;
	
	x2 = number * 0.5F;
	y  = number;
	
	memcpy(&i, &y, 4);
	i  = 0x5f3759df - (i >> 1);                 // what the fuck?
	memcpy(&y, &i, 4);
	y  = y * (threehalfs - (x2 * y * y));       // 1st iteration
	
	return y;
}

// Doom 3's version of Q_rsqrt
inline float D3RSqrt(float x)
{
	long i;
	float y, r;
	
	y = x * 0.5f;
	i = *reinterpret_cast<long *>(&x);
	i = 0x5f3759df - (i >> 1);
	r = *reinterpret_cast<float *>(&i);
	r = r * (1.5f - r * r * y);
	return r;
}

// Doom 3's accurate InvSqrt function
union _flint {
	uint32_t	i;
	float		f;
};

enum {
	LOOKUP_BITS				= 8,
	EXP_POS					= 23,
	EXP_BIAS				= 127,
	LOOKUP_POS				= (EXP_POS - LOOKUP_BITS),
	SEED_POS				= (EXP_POS - 8),
	SQRT_TABLE_SIZE			= (2 << LOOKUP_BITS),
	LOOKUP_MASK				= (SQRT_TABLE_SIZE - 1)
};

static uint32_t iSqrt[SQRT_TABLE_SIZE];

// needs to run to fill the sqrt lookup table
inline void init()
{
	union _flint fi, fo;
	
	for(int i = 0; i < SQRT_TABLE_SIZE; i++) {
		fi.i	 = ((EXP_BIAS - 1) << EXP_POS) | (i << LOOKUP_POS);
		fo.f	 = (float)(1.0 / sqrt(fi.f));
		iSqrt[i] = ((uint32_t)(((fo.i + (1 << (SEED_POS - 2))) >> SEED_POS) & 0xFF)) << SEED_POS;
	}
	
	iSqrt[SQRT_TABLE_SIZE / 2] = ((uint32_t)(0xFF)) << (SEED_POS);
}

// Doom 3's rsqrt/invsqrt version
inline float D3InvSqrt(float x)
{


	uint32_t a = ((union _flint*)(&x))->i;
	union _flint seed;
	
	double y = x * 0.5f;
	seed.i = ((((3 * EXP_BIAS - 1) - ((a >> EXP_POS) & 0xFF)) >> 1) << EXP_POS) | iSqrt[(a >> (EXP_POS-LOOKUP_BITS)) & LOOKUP_MASK];
	double r = seed.f;
	r = r * (1.5f - r * r * y);
	r = r * (1.5f - r * r * y);
	return (float) r;
}
} // namespace

#endif