/*
 * Copyright (c) 2023 Lawrence King
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 */

#include <math.h>
#include <zephyr/kernel.h>
#include <zephyr/ztest.h>


#define local_abs(x) (((x) < 0) ? -(x) : (x))

#ifndef NAN
#define NAN	(__builtin_nansf(""))
#endif

#ifndef INFINITY
#define INFINITY	(__builtin_inff())
#endif

static float test_floats[] = {
	1.0f, 2.0f, 3.0f, 4.0f,
	5.0f, 6.0f, 7.0f, 8.0f, 9.0f,	/* numbers across the decade */
	3.14159265359f, 2.718281828f,	/* irrational numbers pi and e */
	123.4f,	0.025f,	0.10f,	1.875f	/* numbers with infinite */
					/* repeating binary representation */
	};
#define	NUM_TEST_FLOATS	(sizeof(test_floats)/sizeof(float))

	static double test_doubles[] = {
		1.0, 2.0, 3.0, 4.0,
		5.0, 6.0, 7.0, 8.0, 9.0,	/* numbers across the decade */
		3.14159265359, 2.718281828,	/* irrational numbers pi and e */
		123.4,	0.025,	0.10,	1.875	/* numbers with infinite */
						/* repeating binary representationa */
};
#define	NUM_TEST_DOUBLES	(sizeof(test_floats)/sizeof(float))

#ifndef	isinf
static int isinf(double x)
{
	union { uint64_t u; double d; } ieee754;
	ieee754.d = x;
	ieee754.u &= ~0x8000000000000000;	/* ignore the sign */
	return ((ieee754.u >> 52) == 0x7FF) &&
		((ieee754.u & 0x000fffffffffffff) == 0);
}
#endif

#ifndef	isnan
static int isnan(double x)
{
	union { uint64_t u; double d; } ieee754;
	ieee754.d = x;
	ieee754.u &= ~0x8000000000000000;	/* ignore the sign */
	return ((ieee754.u >> 52) == 0x7FF) &&
		((ieee754.u & 0x000fffffffffffff) != 0);
}
#endif

#ifndef	isinff
static int isinff(float x)
{
	union { uint32_t u; float f; } ieee754;
	ieee754.f = x;
	ieee754.u &= ~0x80000000;		/* ignore the sign */
	return ((ieee754.u >> 23) == 0xFF) &&
		((ieee754.u & 0x7FFFFF) == 0);
}
#endif

#ifndef	isnanf
static int isnanf(float x)
{
	union { uint32_t u; float f; } ieee754;
	ieee754.f = x;
	ieee754.u &= ~0x80000000;		/* ignore the sign */
	return ((ieee754.u >> 23) == 0xFF) &&
		((ieee754.u & 0x7FFFFF) != 0);
}
#endif

/* small errors are expected, computed as percentage error */
#define MAX_FLOAT_ERROR_PERCENT		(3.5e-5f)
#define MAX_DOUBLE_ERROR_PERCENT	(4.5e-14)

ZTEST(libc_common, test_sqrtf)
{
int i;
float	exponent, resf, square, root_squared, error;
uint32_t max_error;
int32_t ierror;
int32_t *p_square = (int32_t *)&square;
int32_t *p_root_squared = (int32_t *)&root_squared;


	max_error = 0;

	/* test the special cases of 0.0, NAN, -NAN, INFINITY, -INFINITY,  and -10.0 */
	zassert_true(sqrtf(0.0f) == 0.0f, "sqrtf(0.0)");
	zassert_true(isnanf(sqrtf(NAN)), "sqrt(nan)");
#ifdef issignallingf
	zassert_true(issignallingf(sqrtf(NAN)), "ssignalingf(sqrtf(nan))");
/*	printf("issignallingf();\n"); */
#endif
	zassert_true(isnanf(sqrtf(-NAN)), "isnanf(sqrtf(-nan))");
	zassert_true(isinff(sqrtf(INFINITY)), "isinff(sqrt(inf))");
	zassert_true(isnanf(sqrtf(-INFINITY)), "isnanf(sqrt(-inf))");
	zassert_true(isnanf(sqrtf(-10.0f)), "isnanf(sqrt(-10.0))");

	for (exponent = 1.0e-10f; exponent < 1.0e10f; exponent *= 10.0f) {
		for (i = 0; i < NUM_TEST_FLOATS; i++) {
			square = test_floats[i] * exponent;
			resf = sqrtf(square);
			root_squared = resf * resf;
			zassert_true((resf > 0.0f) && (resf < INFINITY),
					"sqrtf out of range");
			if ((resf > 0.0f) && (resf < INFINITY)) {
				error = (square - root_squared) /
					square * 100;
				if (error < 0.0f) {
					error = -error;
				}
				/* square and root_squared should be almost identical
				 * except the last few bits, the EXOR will only set
				 * the bits that are different
				 */
				ierror = (*p_square - *p_root_squared);
				ierror = local_abs(ierror);
				if (ierror > max_error) {
					max_error = ierror;
				}
			} else {
				/* negative, +NaN, -NaN, inf or -inf */
				error = 0.0f;
			}
			zassert_true(error < MAX_FLOAT_ERROR_PERCENT,
					"max sqrtf error exceeded");
		}
	}
	zassert_true(max_error < 0x03, "huge errors in sqrt implementation");
	/* print the max error */
	TC_PRINT("test_sqrtf max error %d counts\n", max_error);
}

ZTEST(libc_common, test_sqrt)
{
int i;
double	resd, error, square, root_squared, exponent;
uint64_t max_error;
int64_t ierror;
int64_t *p_square = (int64_t *)&square;
int64_t *p_root_squared = (int64_t *)&root_squared;


	max_error = 0;

	/* test the special cases of 0.0, NAN, -NAN, INFINITY, -INFINITY,  and -10.0 */
	zassert_true(sqrt(0.0) == 0.0, "sqrt(0.0)");
	zassert_true(isnan(sqrt((double)NAN)), "sqrt(nan)");
#ifdef issignalling
	zassert_true(issignalling(sqrt((double)NAN)), "ssignaling(sqrt(nan))");
/*	printf("issignalling();\n"); */
#endif
	zassert_true(isnan(sqrt((double)-NAN)), "isnan(sqrt(-nan))");
	zassert_true(isinf(sqrt((double)INFINITY)), "isinf(sqrt(inf))");
	zassert_true(isnan(sqrt((double)-INFINITY)), "isnan(sqrt(-inf))");
	zassert_true(isnan(sqrt(-10.0)), "isnan(sqrt(-10.0))");

	for (exponent = 1.0e-10; exponent < 1.0e10; exponent *= 10.0) {
		for (i = 0; i < NUM_TEST_DOUBLES; i++) {
			square = test_doubles[i] * exponent;
			resd = sqrt(square);
			root_squared = resd * resd;
			zassert_true((resd > 0.0) && (resd < (double)INFINITY),
					"sqrt out of range");
			if ((resd > 0.0) && (resd < (double)INFINITY)) {
				error = (square - root_squared) /
					square * 100;
				if (error < 0.0) {
					error = -error;
				}
				/* square and root_squared should be almost identical
				 * except the last few bits, the EXOR will only set
				 * the bits that are different
				 */
				ierror = (*p_square - *p_root_squared);
				ierror = local_abs(ierror);
				if (ierror > max_error) {
					max_error = ierror;
				}
			} else {
				/* negative, +NaN, -NaN, inf or -inf */
				error = 0.0;
			}
			zassert_true(error < MAX_DOUBLE_ERROR_PERCENT,
					"max sqrt error exceeded");
		}
	}
	zassert_true(max_error < 0x04, "huge errors in sqrt implementation");
	/* print the max error */
	TC_PRINT("test_sqrt max error %d counts\n", (uint32_t)max_error);
}