#ifndef _FIX_MATH_H__
#define _FIX_MATH_H__
#include "bsp/bsp.h"
#include "os/os_types.h"
#include "libs/utils.h"

typedef signed short s16q14_t;
typedef signed short s16q10_t;
typedef signed short s16q5_t;
//typedef signed short s16q4_t;
typedef int32_t   s32q4_t;
typedef int32_t   s32q14_t;
typedef int32_t   s32q5_t;
typedef int32_t   s32q19_t;

//#define S16Q4(A) (signed short)((A) * 16.0F)
#define S16Q5(A) (signed short)((A) * 32.0F)
#define S16Q5toF(A) ((float)(A)/32.0f)

#define S16Q10(A) (signed short)((A) * 1024.0F)
#define S16Q10toF(A) ((float)(A)/1024.0f)

#define S16Q14(A) (signed short)((A) * 16384.0F)
#define S16Q14toF(A)  ((float)(A)/16384.0f)

#define S32Q4(A) (signed int)((A) * 16.0F)
#define S32Q4toF(A) ((float)(A)/16.0f)

#define S32Q14(A) (signed int)((A) * 16384.0F)
#define S32Q14toF(A) ((float)(A)/16384.0f)

#define S32Q19(A) (signed int)((A) * (16384.0F * 32.0F))
#define S32Q19toF(A) ((float)(A)/(16384.0F * 32.0F))


#define S32Q14_MUL(a, b) (((a)*(b)) >>14)

#define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)

#define LowPass_Filter_Fix(value, sample, filter_constant)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> 14)

/*
static __INLINE float S16Q4toF(s16q4_t v) {
	s16 num = (v >> 4) & 0xFFFF;
	u16 tail = v & 0x000F;
	float f = num + (float)tail / 16.0f;
	return f;
} 

static __INLINE float S16Q5toF(s16q5_t v) {
	s16 num = (v >> 5) & 0x3FF;
	u16 tail = v & 0x001F;
	float f = num + (float)tail / 32.0f;
	if (v & 0x8000) {
		return -f;
	}	
	return f;
} 


static __INLINE float S16Q10toF(s16q5_t v) {
	s16 num = (v >> 10) & 0x1F;
	u16 tail = v & 0x03FF;
	float f = num + (float)tail / 1024.0f;
	if (v & 0x8000) {
		return -f;
	}	
	return f;
}


static __INLINE float S16Q14toF(s16q5_t v) {
	s16 num = (v >> 14) & 0x1;
	u16 tail = v & 0x03FFF;
	float f = num + (float)tail / 16384.0f;
	if (v & 0x8000) {
		return -f;
	}	
	return f;
}


static __INLINE float S32Q4toF(s32q4_t v) {
	s32 num = (v >> 4) & 0x7FFFFFF;
	u16 tail = v & 0x000F;
	float f = num + (float)tail / 16.0f;
	if (v & 0x80000000) {
		return -f;
	}
	return f;
} 
*/

#define MATH_sat(in, minOut, maxOut) (min((in), MAX((in), (minOut))))

static __INLINE u16 Sqrt_Fix( u32 wInput )
{
	s32 wtemprootnew;

	u8 biter = 0u;
	s32 wtemproot;
	if ( wInput <= ( s32 )2097152 ) {
      	wtemproot = ( s32 )128;
    } else {
      	wtemproot = ( s32 )8192;
    }

    do {
      	wtemprootnew = ( wtemproot + wInput / wtemproot ) / ( s32 )2;
      	if ( wtemprootnew == wtemproot ) {
        	biter = 6u;
      	} else {
        	biter ++;
        	wtemproot = wtemprootnew;
      	}
    } while ( biter < 6u );
  	return ( wtemprootnew );
}

void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c);

#endif /* _FIX_MATH_H__ */