/* Copyright (C) Thornwave Labs Inc - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 * Written by Razvan Turiac <razvan.turiac@thornwave.com>
 */

#include <math.h>

#include <stdbool.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 expf(float x)
{
	bool neg = false;

	if (x < 0)
	{
		neg = true;
		x = -x;
	}

	float rv;
	
	uint32_t x_int = x;
	x -= x_int;

	if (x_int > 127)
	{
		return neg ? 0 : INFINITY;	
	}
	else
	{
		rv = 1;
		float e = M_E;

		for(uint32_t i = 0; (i < 7) && x_int; i++, x_int >>= 1, e *= e)
			if (x_int & 0x01)
				rv *= e;
	}
		
	float rv_frac = 1;
	if (x > 0)
	{
		float f = x;

		for(uint32_t i = 2; i < 8; i++)
		{
			rv_frac += f;
			f *= x / (float)i;
		}
	}

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


double exp(double x)
{
	bool neg = false;

	if (x < 0)
	{
		neg = true;
		x = -x;
	}

	double rv;
	
	uint32_t x_int = x;
	x -= x_int;

	if (x_int > 1023)
	{
		return INFINITY;	
	}
	else
	{
		rv = 1;
		double e = M_E;

		for(uint32_t i = 0; (i < 10) && x_int; i++, x_int >>= 1, e *= e)
			if (x_int & 0x01)
				rv *= e;
	}
		
	double rv_frac = 1;
	if (x > 0)
	{
		double f = x;

		for(uint32_t i = 2; i < 16; i++)
		{
			rv_frac += f;
			f *= x / (double)i;
		}
	}

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



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));
}



uint64_t gcd(uint64_t a, uint64_t b)
{
    if (a == 0) 
		return b;
    
	if (b == 0) 
		return a;

    uint64_t r = a % b;
    while (r) 
	{
        a = b;
        b = r;
        r = a % b;
    }

    return b;
}