/*
 * fixpoint<T>.h
 *
 *  Created on: Feb 24, 2015
 *      Author: aras
 */

#ifndef FIXPOINT_H_
#define FIXPOINT_H_

#include <inttypes.h>
#include <math.h>

typedef int32_t f1120;
typedef int64_t f4320;

template<class T>
class fixpoint {

private:
	const uint8_t factor;
	T value;

public:
	fixpoint<T>();
	fixpoint<T>(const fixpoint<T>& fp) :
			factor(20) {
		fixpoint<T>();
		value = fp.value;
	}
	;
	fixpoint<T>(T num);

	void set_from_float(float num) {
		value = T(num * (1 << factor));
	}
	void set(T val) {
		value = val;
	}
	float tofloat();

	friend inline fixpoint<T> operator-(const fixpoint<T> &num1,
			const fixpoint<T> &num2) {
		return fixpoint<T>(num1.value - num2.value);
	}
	friend inline fixpoint<T> operator+(const fixpoint<T> &num1,
			const fixpoint<T> &num2) {
		return fixpoint<T>(num1.value + num2.value);
	}
	friend inline fixpoint<T> operator*(const fixpoint<T> &num1,
			const fixpoint<T> &num2) {
		int64_t aux = (int64_t(num1.value) * int64_t(num2.value))
				>> num1.factor;
		return fixpoint<T>(T(aux));
	}
	friend inline fixpoint<T> operator/(const fixpoint<T> &num1,
			const fixpoint<T> &num2) {
		int64_t aux = (int64_t(num1.value) << num1.factor);
		return fixpoint<T>(aux / num2.value);
	}
	inline fixpoint<T>& operator=(const fixpoint<T> &num) {
		value = num.value;
		return *this;
	}

	inline fixpoint<T> power();
	inline fixpoint<T> sqroot();
};

template<class T>
fixpoint<T>::fixpoint() :
		factor(20) {
	value = 0;
}

template<class T>
fixpoint<T>::fixpoint(T num) :
		factor(20) {
	value = num;
}

template<class T>
float fixpoint<T>::tofloat() {
	return float(value) / (1 << factor);
}

template<class T>
fixpoint<T> fixpoint<T>::power() {
	int64_t aux = (int64_t(value) * int64_t(value)) >> factor;
	return fixpoint<T>(aux);
}

template<class T>
fixpoint<T> fixpoint<T>::sqroot() {
	T aux = sqrt(value);
	return fixpoint<T>(aux << (factor / 2));
}

#define fix64 fixpoint<f4320>
#define fix32 fixpoint<f1120>

#endif /* FIXPOINT_H_ */