#include <math.h>


#if defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4)

#include <platform.h>
#include <arm_math.h>

float sin(float x)
{
	return arm_sin_f32(x);
}


float cos(float x)
{
	return arm_cos_f32(x);
}


float tan(float x)
{
	float s, c;
	
	arm_sin_cos_f32(x, &s, &c);

	return s / c;
}


float cotan(float x)
{
	float s, c;
	
	arm_sin_cos_f32(x, &s, &c);

	return c / s;
}

#else

static float cos_taylor(float x);

static float sin_taylor(float x)
{
	if (x > (M_PI / 4.0))
		return cos_taylor((M_PI / 2.0) - x);

	float rv = x;
	const float xx = x * x;
	
	float power_term = x;
	float factorial_term = 1;
	uint32_t factorial_n = 1;

	for(uint32_t i = 0; i < 4; i++)
	{
		power_term *= xx;
		factorial_term *= ++factorial_n;
		factorial_term *= ++factorial_n;
		
		if (i & 0x1)
			rv += power_term / factorial_term;
		else
			rv -= power_term / factorial_term;
	}

	return rv;
}


static float cos_taylor(float x)
{
	if (x > (M_PI / 4.0))
		return sin_taylor((M_PI / 2.0) - x);
		
	float rv = 1;
	const float xx = x * x;
	
	float power_term = 1;
	float factorial_term = 1;
	uint32_t factorial_n = 0;

	for(uint32_t i = 0; i < 4; i++)
	{
		power_term *= xx;
		factorial_term *= ++factorial_n;
		factorial_term *= ++factorial_n;
		
		if (i & 0x1)
			rv += power_term / factorial_term;
		else
			rv -= power_term / factorial_term;
	}

	return rv;
}

float sin(float x)
{
	x = fmod(x, 2 * M_PI);
	if (x < 0)
		x += 2 * M_PI;

	if (x > M_PI)
		return -sin(x - M_PI);

	if (x > (M_PI / 2.0))
		return cos_taylor(x - (M_PI / 2.0));

	return sin_taylor(x);
}


float cos(float x)
{
	x = fmod(x, 2 * M_PI);
	if (x < 0)
		x += 2 * M_PI;

	if (x > M_PI)
		return -cos(x - M_PI);

	if (x > (M_PI / 2.0))
		return -cos_taylor(M_PI - x);

	return cos_taylor(x);
}


float tan(float x)
{
	return sin(x) / cos(x)
}


float cotan(float x)
{
	return cos(x) / sin(x)
}

#endif


static float agm(float x, float y)
{
	float a = (x + y) / 2.0;
	float g = sqrt(x * y);

	while(fabs(a - g) > 1E-7)
	{
		float an = (a + g) / 2.0;
		float gn = sqrt(a * g);
		
		a = an;
		g = gn;
	}

	return a;
}


float exp(float x)
{
	int32_t neg = 0;
	
	if (x < 0)
	{
		neg = 1;
		x = -x;
	}
	
	float rv = 1;
	
	int32_t x_int = x;
	x -= x_int;
	
	while(x_int--)
		rv *= M_E;
		
	if (x > 0)
	{
		float rv_frac = 1;
		
		for(uint32_t i = 8; i > 0; i--)
			rv_frac = 1 + x * rv_frac / (float)i;

		if (neg)
			return 1 / (rv * rv_frac);			
		else
			return rv * rv_frac;			
		
	}

	if (neg)
		return 1 / rv;
	else
		return rv;
}



float log(float x)
{
	if (x < 0)
		return 0.0/0.0;

	if (x == 0)
		return -INFINITY;
		
	if (x == 1)
		return 0;
		
	float rv1 = 0;
	while(x < 1)
	{
		x *= 2;
		rv1 += M_LN2;
	}

	const int32_t m = 12;
	float rv = M_PI / (2 * agm(1, 4 / (x * (float)(1 << m)))) - m * M_LN2;

	return rv - rv1;
}



float powf(float b, float x)
{
	return exp(x * log(b));
}