#ifndef FIXED_POINT_H_
#define FIXED_POINT_H_

#define FP_SCALE 8192
#define FP_SCALE_SHIFT 13

inline int float_to_fixed(float f) __attribute__ ((no_instrument_function));
inline int float_to_fixed(float f)
{ 
   union 
   { 
      float f; 
      int   i; 
   } u;
   u.f = f; 
    
   /* convert mantissa to two's compliment */ 
   int mantissa = u.i & ((1 << 23) - 1);             /* pick bits */ 
   mantissa |= 1 << 23;                              /* set implicit one bit */ 
    
   int exponent = (u.i >> 23) & 255; /* pick bits */ 
   exponent -= 127;                  /* correct for the FP32 bias */ 
   exponent -= (23 - FP_SCALE_SHIFT);            /* correct for the moved sign */ 
    
   if (exponent >= 0) mantissa <<=  exponent; 
   else 
   { 
      int shift = -exponent; 
//      if (shift > 31) return 0; /* needed on x86 - should not be needed on ARMs */ 
      mantissa >>= shift; 
   } 

   if (0 != (u.i & (1 << 31))) mantissa = -mantissa; /* add sign */ 
   return mantissa; 
}

inline int float_to_fixed_normal(float f)__attribute__ ((no_instrument_function));
inline int float_to_fixed_normal(float f)
{ 
   union 
   { 
      float f; 
      int   i; 
   } u;
   u.f = f; 
    
   /* convert mantissa to two's compliment */ 
   int mantissa = u.i & ((1 << 23) - 1);             /* pick bits */ 
   mantissa |= 1 << 23;                              /* set implicit one bit */ 
    
   int exponent = (u.i >> 23) & 255; /* pick bits */ 
   exponent -= 127;                  /* correct for the FP32 bias */ 
   exponent -= 23;            /* correct for the moved sign */ 
    
   if (exponent >= 0) mantissa <<=  exponent; 
   else 
   { 
      int shift = -exponent; 
//      if (shift > 31) return 0; /* needed on x86 - should not be needed on ARMs */ 
      mantissa >>= shift; 
   } 

   if (0 != (u.i & (1 << 31))) mantissa = -mantissa; /* add sign */ 
   return mantissa; 
} 

inline int float_to_fixed_fpu(float f) 
{ 
   return int(f * (1 << FP_SCALE_SHIFT)); 
} 

inline float fixed_to_float(int i) 
{ 
   return float(i) / (1 << FP_SCALE_SHIFT); 
} 

//void test_number(float f, bool fail_expected) 
//{ 
//   float f1 = fixed_19_13_to_float(float_to_fixed_19_13(f)); 
//   float f2 = fixed_19_13_to_float(float_to_fixed_19_13_fpu(f)); 
//   bool fail = false; 
//
//   if (fabs(f1 - f) > 1.f / (1 << 13)) fail = true; 
//   if (f1 != f2) fail = true; 
//
//   fail = (fail == !fail_expected); 
//   printf("%s %f -> %f (%f) / %f (%f)\n", false == fail ? "SUCCESS" : "FAIL", f, f1, fabs(f1 - f), f2, fabs(f2 - f)); 
//}

class fixed_point
{
public:
	fixed_point(void)__attribute__ ((no_instrument_function));
	fixed_point(const fixed_point &a)__attribute__ ((no_instrument_function));
	
//	inline fixed_point(int a)
//	{
//		value = a * FP_SCALE;
//	};
	
	fixed_point(float a)__attribute__ ((no_instrument_function));
//	fixed_point(double);
	
//	inline operator int()
//	{
//		return value / FP_SCALE;
//	};
	
	operator float()__attribute__ ((no_instrument_function));
//	operator double();
	inline operator fixed_point()__attribute__ ((no_instrument_function));
	
	void set_value_by_float(float a)
	{
		value = float_to_fixed(a);
	}
	
//	float value;
//	long long value;
	int value;
};

struct vec3_f
{
	fixed_point f[3];
};

inline fixed_point::fixed_point(void)
{
	value = 0;
}
inline fixed_point::fixed_point(const fixed_point &a)
{
	value = a.value;
}
inline fixed_point::fixed_point(float a)
{
	value = float_to_fixed(a);
//	value = a * FP_SCALE;
//	value = a;
}
inline fixed_point::operator float()
{
	return (float)value / FP_SCALE;
//	return value;
}

#define DECL_ADDAF(typeA) inline fixed_point operator+(typeA a, fixed_point b);
#define DECL_ADDFB(typeB) inline fixed_point operator+(fixed_point a, typeB b);
#define DECL_SUBAF(typeA) inline fixed_point operator-(typeA a, fixed_point b);
#define DECL_SUBFB(typeB) inline fixed_point operator-(fixed_point a, typeB b);
#define DECL_MULAF(typeA) inline fixed_point operator*(typeA a, fixed_point b)__attribute__ ((no_instrument_function));
#define DECL_MULFB(typeB) inline fixed_point operator*(fixed_point a, typeB b)__attribute__ ((no_instrument_function));
#define DECL_DIVAF(typeA) inline fixed_point operator/(typeA a, fixed_point b);
#define DECL_DIVFB(typeB) inline fixed_point operator/(fixed_point a, typeB b);

#define DECL_LTAF(typeA) inline bool operator<(typeA a, fixed_point b);
#define DECL_LTFB(typeB) inline bool operator<(fixed_point a, typeB b);
#define DECL_GTAF(typeA) inline bool operator>(typeA a, fixed_point b);
#define DECL_GTFB(typeB) inline bool operator>(fixed_point a, typeB b);

#define DECL_LTEAF(typeA) inline bool operator<=(typeA a, fixed_point b);
#define DECL_LTEFB(typeB) inline bool operator<=(fixed_point a, typeB b);
#define DECL_GTEAF(typeA) inline bool operator>=(typeA a, fixed_point b);
#define DECL_GTEFB(typeB) inline bool operator>=(fixed_point a, typeB b);

#define DECL_EQAF(typeA) inline bool operator==(typeA a, fixed_point b);
#define DECL_EQFB(typeB) inline bool operator==(fixed_point a, typeB b);

//#define ADDFB(typeB, name) \
//fixed_point operator+(fixed_point a, typeB b)\
//{\
//	return fixed_point(a.value + b);\
//}
//
//#define ADDAF(typeA, name) \
//fixed_point operator+(typeA a, fixed_point b)\
//{\
//	return fixed_point(a + b.value);\
//}

//#define SUBFB(typeB, name) \
//fixed_point operator-(fixed_point a, typeB b)\
//{\
//	return fixed_point((float)a + (float)b);\
//}
//
//#define SUBAF(typeA, name) \
//fixed_point operator-(typeA a, fixed_point b)\
//{\
//	return fixed_point((float)a - (float)b);\
//}

#define MULFB(typeB, name) \
fixed_point operator*(fixed_point a, typeB b)\
{\
	return fixed_point((float)a * (float)b);\
}

#define MULAF(typeA, name) \
fixed_point operator*(typeA a, fixed_point b)\
{\
	return fixed_point((float)a * (float)b);\
}

//#define DIVFB(typeB, name) \
//fixed_point operator/(fixed_point a, typeB b)\
//{\
//	return fixed_point(a.value / b);\
//}
//
//#define DIVAF(typeA, name) \
//fixed_point operator/(typeA a, fixed_point b)\
//{\
//	return fixed_point(a / b.value);\
//}
//
//#define LTAF(typeA, name) \
//bool operator<(typeA a, fixed_point b)\
//{\
//	return a < b.value;\
//}
//
//#define LTFB(typeB, name) \
//bool operator<(fixed_point a, typeB b)\
//{\
//	return a.value < b;\
//}
//
//#define GTAF(typeA, name) \
//bool operator>(typeA a, fixed_point b)\
//{\
//	return a > b.value;\
//}
//
//#define GTFB(typeB, name) \
//bool operator>(fixed_point a, typeB b)\
//{\
//	return a.value > b;\
//}
//
//#define LTEAF(typeA, name) \
//bool operator<=(typeA a, fixed_point b)\
//{\
//	return a <= b.value;\
//}
//
//#define LTEFB(typeB, name) \
//bool operator<=(fixed_point a, typeB b)\
//{\
//	return a.value <= b;\
//}
//
//#define GTEAF(typeA, name) \
//bool operator>=(typeA a, fixed_point b)\
//{\
//	return a >= b.value;\
//}
//
//#define GTEFB(typeB, name) \
//bool operator>=(fixed_point a, typeB b)\
//{\
//	return a.value >= b;\
//}
//
//#define EQAF(typeA, name) \
//bool operator==(typeA a, fixed_point b)\
//{\
//	return a == b.value;\
//}
//
//#define EQFB(typeB, name) \
//bool operator==(fixed_point a, typeB b)\
//{\
//	return a.value == b;\
//}

//DECL_ADDAF(float)
//DECL_ADDFB(float)
//DECL_ADDAF(int)
//DECL_ADDFB(int)
//
//DECL_SUBAF(float)
//DECL_SUBFB(float)
//DECL_SUBAF(int)
//DECL_SUBFB(int)

//DECL_MULAF(float)
//DECL_MULFB(float)
//DECL_MULAF(int)
//DECL_MULFB(int)

//DECL_DIVAF(float)
//DECL_DIVFB(float)
//DECL_DIVAF(int)
//DECL_DIVFB(int)

//DECL_LTAF(float)
//DECL_LTFB(float)
//DECL_LTAF(int)
//DECL_LTFB(int)
//
//DECL_GTAF(float)
//DECL_GTFB(float)
//DECL_GTAF(int)
//DECL_GTFB(int)
//
//DECL_LTEAF(float)
//DECL_LTEFB(float)
//DECL_LTEAF(int)
//DECL_LTEFB(int)
//
//DECL_GTEAF(float)
//DECL_GTEFB(float)
//DECL_GTEAF(int)
//DECL_GTEFB(int)
//
//DECL_EQAF(float)
//DECL_EQFB(float)
//DECL_EQAF(int)
//DECL_EQFB(int)

//SUBAF(float, "float")
//SUBFB(float, "float")
//MULAF(float, "float")
//MULFB(float, "float")
//MULAF(int, "int")
//MULFB(int, "int")

//inline fixed_point operator+(fixed_point a, fixed_point b)
//{
//	fixed_point temp;
//	temp.value = a.value + b.value;
//	return temp;
//}
//inline fixed_point operator-(fixed_point a, fixed_point b)
//{
//	return fixed_point((float)a - (float)b);
//}
//inline fixed_point operator*(fixed_point a, fixed_point b)
//{
//	return fixed_point((float)(a.value * b.value) / FP_SCALE);
//}
//fixed_point operator/(fixed_point a, fixed_point b);
//
//bool operator!(fixed_point a);
//fixed_point operator-(fixed_point a);

#endif /*FIXED_POINT_H_*/
