#include "Math.hh"
#include <cstdint>
#include <cstdlib>

#if defined _MSC_VER && defined __x86_64
#include <emmintrin.h>
#endif

namespace openmsx {

#ifdef _MSC_VER

// Poor man's implementations to get openmsx building with VC++

#ifdef __x86_64

// What follows below are C++ implementations of several C99 math functions missing
// from VC++'s CRT as of 2008. These implementations using SSE/SSE2 intrinsics
// for floating point operations. The (generally safe) assumption is that those
// instructions are always available on CPUs capable of running 64-bit Windows.
//
// The assembler in Visual Studio (ml64.exe) no longer allows inline assembly,
// so for ease of reading we've opted to use these intrinsic-based functions instead
// of using the hand-written versions in a separate .asm file. The resulting
// optimized code is pretty close to the hand-written code.

// gcc-generated data section
//;.LC0:
//;	.long	0
//;	.long	1127219200
//;	.section	.rodata.cst16,"aM",@progbits,16
//;	.align 16
//;.LC1:
//;	.long	4294967295
//;	.long	2147483647
//;	.long	0
//;	.long	0
//;	.section	.rodata.cst8
//;	.align 8
//;.LC2:
//;	.long	4294967295
//;	.long	1071644671
//;	.section	.rodata.cst4,"aM",@progbits,4
//;	.align 4
//;.LC3:
//;	.long	1258291200
//;	.section	.rodata.cst16
//;	.align 16
//;.LC4:
//;	.long	2147483647
//;	.long	0
//;	.long	0
//;	.long	0
//;	.ident	"GCC: (GNU) 4.4.0 20090130 (experimental)"
//;	.section	.note.GNU-stack,"",@progbits
// ML64 hand-written data section:
//LC0 QWORD  4330000000000000h
//LC1 QWORD  7FFFFFFFFFFFFFFFh
//LC2 QWORD  3FDFFFFFFFFFFFFFh
//LC3 DWORD          4B000000h
//LC4 DWORD          7FFFFFFFh

// lrint(): round double according to current floating-point rounding direction
// gcc-generated:
//;	cvtsd2siq	%xmm0, %rax
//;	ret
// ML64 hand-written:
//LEAF_ENTRY lrinta, Math
//    cvtsd2si	rax, xmm0
//    ret
//LEAF_END lrinta, Math
long lrint(double x)
{
	return _mm_cvtsd_si32(_mm_load_sd(&x));
}

// lrintf(): round float according to current floating-point rounding direction
// gcc-generated:
//;	cvtss2siq	%xmm0, %rax
//;	ret
// ML64 hand-written:
//LEAF_ENTRY lrintf, Math
//    cvtss2si    rax, xmm0
//    ret
//LEAF_END lrintf, Math
long lrintf(float x)
{
	return _mm_cvtss_si32(_mm_load_ss(&x));
}

// truncf(): round float to the nearest integer not larger in absolute value
// gcc-generated:
//;	movss	.LC4(%rip), %xmm1
//;	movaps	%xmm0, %xmm2
//;	andps	%xmm1, %xmm2
//;	comiss	.LC3(%rip), %xmm2
//;	jae	.L6
//;	cvttss2si	%xmm0, %eax
//;	cvtsi2ss	%eax, %xmm0
//;.L6:
//;	rep
//;	ret
// ML64 hand-written:
//LEAF_ENTRY truncf, Math
//	movss	    xmm1, LC4
//	movaps	    xmm2, xmm0
//	andps	    xmm2, xmm1
//	comiss	    xmm2, LC3
//	jae	L6
//	cvttss2si	eax, xmm0
//	cvtsi2ss	xmm0, eax
//L6:
//  ret
//LEAF_END truncf, Math
float truncf(float x)
{
	int64_t tempi = _mm_cvttss_si64(_mm_load_ss(&x));
	__m128 xmmx = _mm_cvtsi64_ss(_mm_setzero_ps(), tempi);

	float ret;
	_mm_store_ss(&ret, xmmx);
	return ret;
}

// round(): round double to nearest integer, away from zero
// gcc-generated:
//;	movsd	.LC1(%rip), %xmm1
//;	movapd	%xmm0, %xmm2
//;	andpd	%xmm1, %xmm2
//;	comisd	.LC0(%rip), %xmm2
//;	jae	.L2
//;	addsd	.LC2(%rip), %xmm2
//;	andnpd	%xmm0, %xmm1
//;	cvttsd2siq	%xmm2, %rax
//;	cvtsi2sdq	%rax, %xmm2
//;	movapd	%xmm2, %xmm0
//;	orpd	%xmm1, %xmm0
//;.L2:
//;	rep
//;	ret
// ML64 hand-written:
//LEAF_ENTRY round, Math
//	movsd	    xmm1, LC1
//	movapd      xmm2, xmm0
//	andpd	    xmm2, xmm1
//	comisd	    xmm2, LC0
//	jae	L2
//	addsd	    xmm2, LC2
//	andnpd	    xmm1, xmm0
//	cvttsd2si	rax,  xmm2
//	cvtsi2sd	xmm2, rax
//	movapd	    xmm0, xmm2
//	orpd	    xmm0, xmm1
//L2:
//	ret
//LEAF_END round, Math
double round(double x)
{
	const double half = 0.5;
	__m128d xmmhalf = _mm_load_sd(&half);
	__m128d xmmx = _mm_load_sd(&x);

	// Add + or -0.5 depending on sign...
	if (_mm_comige_sd(xmmx, _mm_setzero_pd())) {
		xmmx = _mm_add_sd(xmmx, xmmhalf);
	} else {
		xmmx = _mm_sub_sd(xmmx, xmmhalf);
	}

	// ... then round towards zero
	int64_t tempi64 = _mm_cvttsd_si64(xmmx);
	xmmx = _mm_cvtsi64_sd(_mm_setzero_pd(), tempi64);

	double ret;
	_mm_store_sd(&ret, xmmx);
	return ret;
}

#else

// lrint(): round double according to current floating-point rounding direction
long lrint(double x)
{
	long retval;
	__asm {
		fld			qword ptr [x]
		fistp		dword ptr [retval]
	}
	return retval;
}

// lrint(): round float according to current floating-point rounding direction
long lrintf(float x)
{
	long retval;
	__asm {
		fld			dword ptr [x]
		fistp		dword ptr [retval]
	}
	return retval;
}

// trunc(): round double to the nearest integer not larger in absolute value
double trunc(double x)
{
	short ctrl1, ctrl2;
	double retval;
	__asm {
		fnstcw		word ptr [ctrl1]
		movzx		eax,word ptr [ctrl1]
		or			ah,0Ch
		mov			word ptr [ctrl2],ax
		fldcw		word ptr [ctrl2]
		fld			qword ptr [x]
		frndint
		fldcw		word ptr [ctrl1]
		fstp		qword ptr [retval]
	}
	return retval;
}

// truncf(): round float to the nearest integer not larger in absolute value
float truncf(float x)
{
	short ctrl1, ctrl2;
	float retval;
	__asm {
		fnstcw		word ptr [ctrl1]
		movzx		eax,word ptr [ctrl1]
		or			ah,0Ch
		mov			word ptr [ctrl2],ax
		fldcw		word ptr [ctrl2]
		fld			dword ptr[x]
		frndint
		fldcw		word ptr [ctrl1]
		fstp		dword ptr [retval]
	}
	return retval;
}

// round(): round double to nearest integer, away from zero
double round(double x)
{
	return (x > 0) ? trunc(x + 0.5) : trunc(x - 0.5);
}

#endif	// __x86_64
#endif	// _MSC_VER

namespace Math {

unsigned powerOfTwo(unsigned a)
{
	// classical implementation:
	//   unsigned res = 1;
	//   while (a > res) res <<= 1;
	//   return res;

	// optimized version
	a += (a == 0); // can be removed if argument is never zero
	return floodRight(a - 1) + 1;
}

void gaussian2(double& r1, double& r2)
{
	static const double S = 2.0 / RAND_MAX;
	double x1, x2, w;
	do {
		x1 = S * rand() - 1.0;
		x2 = S * rand() - 1.0;
		w = x1 * x1 + x2 * x2;
	} while (w >= 1.0);
	w = sqrt((-2.0 * log(w)) / w);
	r1 = x1 * w;
	r2 = x2 * w;
}

} // namespace Math

} // namespace openmsx
