#pragma once

//#include <math/fix8.h>
//#include <math/fix16.h>

#define FPDECL(width,fract)	\
	typedef struct { int ## width ## _t value; } fp ## width ## fract ## _t; \
	static inline fp ## width ## fract ## _t FP ## width ## fract (float v) { return (fp ## width ## fract ## _t){ (int ## width ## _t)(v * 65536) }; }; \
	//
	

FPDECL(8,3);
FPDECL(16,3);
FPDECL(32,3);
FPDECL(64,3);

FPDECL(8,7);
FPDECL(16,7);
FPDECL(32,7);
FPDECL(64,7);

FPDECL(16,11);
FPDECL(32,11);
FPDECL(64,11);

FPDECL(16,15);
FPDECL(32,15);
FPDECL(64,15);

#define FPDOT3(dst,n)	{ dst.value = (n << 3); }
#define FPDOT7(dst,n)	{ dst.value = (n << 7); }
#define FPDOT11(dst,n)	{ dst.value = (n << 11); }
#define FPDOT15(dst,n)	{ dst.value = (n << 15); }

/**
 * Dynamic Fixpoint Arithemtics
 */

typedef struct dfp16 {
	int16_t		value;
	uint8_t		fragsize;
} dfp16_t;

typedef struct dfp32 {
	int32_t		value;
	uint8_t		fragsize;
} dfp32_t;

static inline dfp16_t	dfp16from32(dfp32_t v){ return (dfp16_t){ fragsize: v.fragsize, value: v.value }; };
static inline dfp32_t	dfp32from16(dfp16_t v){ return (dfp32_t){ fragsize: v.fragsize, value: v.value }; };

static inline dfp16_t	dfp16_from_float(uint8_t prec,float v){ return (dfp16_t){ fragsize: prec, value: (int16_t)(v * (1<<prec)) }; };
static inline dfp32_t	dfp32_from_float(uint8_t prec,float v){ return (dfp32_t){ fragsize: prec, value: (int32_t)(v * (1<<prec)) }; };

static inline float		float_from_dfp16(dfp16_t v){ return ((float)v.value) / (1 << v.fragsize); };
static inline float		float_from_dfp32(dfp32_t v){ return ((float)v.value) / (1 << v.fragsize); };

#define SHL(a,n) (n < 0 ? a >> -n : a << n)
#define SHR(a,n) (n < 0 ? a << -n : a >> n)

static inline
dfp16_t	dfp16_add(uint8_t prec,dfp16_t a,dfp16_t b){
	dfp16_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
	v.value += SHL(b.value,(prec-b.fragsize));
	return v;
};

static inline
dfp16_t	dfp16_sub(uint8_t prec,dfp16_t a,dfp16_t b){
	dfp16_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
	v.value -= SHL(b.value,(prec-b.fragsize));
	return v;
};

static inline
dfp16_t	dfp16_mul(int8_t prec,dfp16_t a,dfp16_t b){
	return (dfp16_t){ 
		fragsize:	prec,
		value:  	SHL((int32_t)a.value * (int32_t)b.value, (prec - (a.fragsize + b.fragsize)))
		};
};

static inline
dfp16_t	dfp16_div(int8_t prec,dfp16_t a,dfp16_t b){
	return (dfp16_t){ 
		fragsize:	prec,
		value:  	
			SHL(
				((int32_t)a.value / (int32_t)b.value),
				(prec + b.fragsize - a.fragsize)
				)
		};
};

static inline
dfp32_t	dfp32_add(uint8_t prec,dfp32_t a,dfp32_t b){
	dfp32_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
	v.value += SHL(b.value,(prec-b.fragsize));
	return v;
};

static inline
dfp32_t	dfp32_sub(uint8_t prec,dfp32_t a,dfp32_t b){
	dfp32_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
	v.value -= SHL(b.value,(prec-b.fragsize));
	return v;
};

static inline
dfp32_t	dfp32_mul(uint8_t prec,dfp32_t a,dfp32_t b){
	return (dfp32_t){ 
		fragsize:	prec,
		value:  	SHL((int64_t)a.value * (int64_t)b.value, (prec - (a.fragsize + b.fragsize)))
		};
};

static inline
dfp32_t	dfp32_div(uint8_t prec,dfp32_t a,dfp32_t b){
	return (dfp32_t){ 
		fragsize:	prec,
		value:  	
			SHL(
				((int64_t)a.value),
				(prec + b.fragsize - a.fragsize)
				) / b.value
		};
};