#include <winpr/crt.h>
#include <winpr/sysinfo.h>
#include <winpr/windows.h>

#include <winpr/intrin.h>

static BOOL g_LZCNT = FALSE;

static INLINE UINT32 lzcnt_s(UINT32 x)
{
	if (!x)
		return 32;

	if (!g_LZCNT)
	{
		UINT32 y;
		int n = 32;
		y = x >> 16;
		if (y != 0)
		{
			n = n - 16;
			x = y;
		}
		y = x >> 8;
		if (y != 0)
		{
			n = n - 8;
			x = y;
		}
		y = x >> 4;
		if (y != 0)
		{
			n = n - 4;
			x = y;
		}
		y = x >> 2;
		if (y != 0)
		{
			n = n - 2;
			x = y;
		}
		y = x >> 1;
		if (y != 0)
			return n - 2;
		return n - x;
	}

	return __lzcnt(x);
}

int test_lzcnt()
{
	if (lzcnt_s(0x1) != 31)
	{
		fprintf(stderr, "__lzcnt(0x1) != 31: %" PRIu32 "\n", __lzcnt(0x1));
		return -1;
	}

	if (lzcnt_s(0xFF) != 24)
	{
		fprintf(stderr, "__lzcnt(0xFF) != 24\n");
		return -1;
	}

	if (lzcnt_s(0xFFFF) != 16)
	{
		fprintf(stderr, "__lzcnt(0xFFFF) != 16\n");
		return -1;
	}

	if (lzcnt_s(0xFFFFFF) != 8)
	{
		fprintf(stderr, "__lzcnt(0xFFFFFF) != 8\n");
		return -1;
	}

	if (lzcnt_s(0xFFFFFFFF) != 0)
	{
		fprintf(stderr, "__lzcnt(0xFFFFFFFF) != 0\n");
		return -1;
	}

	return 0;
}

int test_lzcnt16()
{
	if (__lzcnt16(0x1) != 15)
	{
		fprintf(stderr, "__lzcnt16(0x1) != 15\n");
		return -1;
	}

	if (__lzcnt16(0xFF) != 8)
	{
		fprintf(stderr, "__lzcnt16(0xFF) != 8\n");
		return -1;
	}

	if (__lzcnt16(0xFFFF) != 0)
	{
		fprintf(stderr, "__lzcnt16(0xFFFF) != 0\n");
		return -1;
	}

	return 0;
}

int TestIntrinsics(int argc, char* argv[])
{
	g_LZCNT = IsProcessorFeaturePresentEx(PF_EX_LZCNT);

	printf("LZCNT available: %" PRId32 "\n", g_LZCNT);

	// test_lzcnt16();
	return test_lzcnt();
}