#ifndef _RTLIBC_MATH_H
#define _RTLIBC_MATH_H


#include <stdint.h>


#define M_E				2.718281828459045235360287471352662498L //e
#define M_LOG2E			1.442695040888963407359924681001892137L //log_2 e
#define M_LOG10E		0.434294481903251827651128918916605082L //log_10 e
#define M_LN2			0.693147180559945309417232121458176568L //log_e 2
#define M_LN10			2.302585092994045684017991454684364208L //log_e 10
#define M_PI			3.141592653589793238462643383279502884L //pi
#define M_PI_2			1.570796326794896619231321691639751442L //pi/2
#define M_PI_4			0.785398163397448309615660845819875721L //pi/4
#define M_1_PI			0.318309886183790671537767526745028724L //1/pi
#define M_2_PI			0.636619772367581343075535053490057448L //2/pi
#define M_2_SQRTPI		1.128379167095512573896158903121545172L //2/sqrt(pi)
#define M_SQRT2			1.414213562373095048801688724209698079L //sqrt(2)
#define M_SQRT1_2		0.707106781186547524400844362104849039L //1/sqrt(2)


#define INFINITY	(__builtin_inff())
#define M_NAN		(__builtin_nanf("0"))



#ifdef __cplusplus
extern "C"
{
#endif


static inline float round(float x)
{
#ifdef ARM_MATH_CM7
	float rv;
	__asm volatile ("vrinta.f32 %0, %1" : "=w" (rv) : "w" (x));
	return rv;
#else
	return (int32_t)(x + 0.5L);
#endif
}



static inline float floor(float x)
{
#ifdef ARM_MATH_CM7
	float rv;
	__asm volatile ("vrintm.f32 %0, %1" : "=w" (rv) : "w" (x));
	return rv;
#else
	return (x >= 0) ? (int32_t)x : (int32_t)x - 1;
#endif
}


static inline float trunc(float x)
{
#ifdef ARM_MATH_CM7
	float rv;
	__asm volatile ("vrintz.f32 %0, %1" : "=w" (rv) : "w" (x));
	return rv;
#else
	return (int32_t)x;
#endif
}


static inline float ceil(float x)
{
#ifdef ARM_MATH_CM7
	float rv;
	__asm volatile ("vrintp.f32 %0, %1" : "=w" (rv) : "w" (x));
	return rv;
#else
	return (x >= 0) ? 	
	(((x - (int32_t)x) > 0) ? (int32_t)(x + 1) : (int32_t)x) : 
	(((x - (int32_t)x) < 0) ? (int32_t)(x - 1) : (int32_t)x);
#endif
}


static inline float sqrt(float x)
{
#if defined ARM_MATH_CM7 || defined ARM_MATH_CM4
	if (x >= 0)
	{
		float rv;
		__asm volatile ("vsqrt.f32 %0, %1" : "=w" (rv) : "w" (x));
		return rv;
	}
	else
	{
		return 0;
	}
#else

	return 0;			//TODO not implemented
#endif
}


static inline float sqrtf(float x)
{
	return sqrt(x);
}


static inline float fmod(float x, float y)
{
	return x - floor(x / y) * y;
}


static inline float frac(float x)
{
	return x - trunc(x);
}



static inline float fabs(float x)
{
#if defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4)
	float rv;
	__asm volatile ("vabs.f32 %0, %1" : "=w" (rv) : "w" (x));
	return rv;
#else
	return (x >= 0) ? x : -x;
#endif
}


static inline float fmax(float a, float b)
{
#ifdef ARM_MATH_CM7
	float v;
	__asm volatile ("vmaxnm.f32 %0, %1, %2" : "=w" (v) : "w" (a), "w" (b));         //max instruction
	return v;
#else
	return (a > b) ? a : b;
#endif
}


static inline float fmin(float a, float b)
{
#ifdef ARM_MATH_CM7
	float v;
	__asm volatile ("vminnm.f32 %0, %1, %2" : "=w" (v) : "w" (a), "w" (b));         //min instruction
	return v;
#else
	return (a < b) ? a : b;
#endif
}


static inline float fsat(float x, float min, float max)
{
	return fmin(fmax(min, x), max);
}


static inline int32_t smin(int32_t a, int32_t b)
{
	return (a < b) ? a : b;
}

static inline int32_t smax(int32_t a, int32_t b)
{
	return (a > b) ? a : b;
}

static inline int32_t ssat(int32_t x, int32_t min, int32_t max)
{
	if (x < min)
		return min;
	else if (x > max)
		return max;
	else
		return x;
}



static inline uint32_t umin(uint32_t a, uint32_t b)
{
	return (a < b) ? a : b;
}

static inline uint32_t umax(uint32_t a, uint32_t b)
{
	return (a > b) ? a : b;
}

static inline uint32_t usat(uint32_t x, uint32_t min, uint32_t max)
{
	if (x < min)
		return min;
	else if (x > max)
		return max;
	else
		return x;
}


static inline int32_t round_closest_signed(int32_t n, int32_t d)
{
	return (((n < 0) ^ (d < 0)) ? ((n - d / 2) / d) : ((n + d / 2) / d)) * d;
}


static inline uint32_t round_closest_unsigned(uint32_t n, uint32_t d)
{
	return ((n + d / 2) / d) * d;
}


float sin(float x);
float cos(float x);
float tan(float x);
float cotan(float x);


float expf(float x);
double exp(double x);

float log(float x);
float powf(float b, float x);


uint64_t gcd(uint64_t a, uint64_t b);

static inline uint64_t lcm(uint32_t a, uint64_t b)
{
	return (a * b) / gcd(a, b);
}


#ifdef __cplusplus
}
#endif


#endif