#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 signed int   s32q4_t;
typedef signed int   s32q14_t;


#define S16Q4(A) (signed short)((A) * 16.0F)
#define S16Q5(A) (signed short)((A) * 32.0F)
#define S16Q10(A) (signed short)((A) * 1024.0F)
#define S16Q14(A) (signed short)((A) * 16384.0F)
#define S32Q4(A) (signed int)((A) * 16.0F)
#define S32Q14(A) (signed int)((A) * 16384.0F)
#define S32Q14_MUL(a, b) (((a)*(b)) >>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) & 0xFFFF;
	u16 tail = v & 0x001F;
	float f = num + (float)tail / 32.0f;
	return f;
} 

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

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

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

#endif /* _FIX_MATH_H__ */