#ifndef _FIX_MATH_H__
#define _FIX_MATH_H__
#include "bsp/bsp.h"
#include "os/os_types.h"
typedef signed short sfix10_t;
typedef signed short sfix6_t;
typedef signed short sfix2_t;
typedef signed short sfix1_t;

#define Fix10_6(x) (x >> 4)
#define i2sFix2(x) ((sfix2_t)(x <<2))
#define i2sFix6(x) ((sfix6_t)(x <<6))
#define i2sFix10(x) ((sfix6_t)(x <<10))


#define i2sFix1(x) ((sfix2_1)(x <<1))

#define sFix2ToI(x) ((s16)(x >> 2))

static __INLINE sfix6_t ftoSfix10(float v) {
	s16 num = (s16)v;
	float tail_f = v - num;
	u8 tail_u8 = (u8)(tail_f / (1.0f / 1024.0f)); 

	return (s16) ((num << 10) | (tail_u8 & 0x03FF));
}


static __INLINE float sfix6toF(sfix6_t v) {
	s16 num = (v >> 6) & 0xFFFF;
	u16 tail = v & 0x003F;
	float f = num + (float)tail / 64.0f;
	return f;
} 

static __INLINE sfix6_t ftoSfix6(float v) {
	s16 num = (s16)v;
	float tail_f = v - num;
	u8 tail_u8 = (u8)(tail_f / (1.0f / 64.0f)); 

	return (s16) ((num << 6) | (tail_u8 & 0x003F));
}

static __INLINE float sfix2toF(sfix6_t v) {
	s16 num = (v >> 2) & 0xFFFF;
	u16 tail = v & 0x003;
	float f = num + (float)tail / 4.0f;
	return f;
} 

static __INLINE sfix6_t ftoSfix2(float v) {
	s16 num = (s16)v;
	float tail_f = v - num;
	u8 tail_u8 = (u8)(tail_f / (1.0f / 4.0f)); 

	return (s16) ((num << 2) | (tail_u8 & 0x0003));
}


#endif /* _FIX_MATH_H__ */