/***********************************
 * (C) 2007-2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPL3+ (See Docs/LICENSE)
 *
 * Desc: PID based on autopilot (sourceforce) implementation.
 *
 * Soon to be rewritten.
 ********************/


namespace Kalman {
	typedef float KalmanType;

	/* Temporary resolution before changing to integer... */
	static inline KalmanType MUL(const KalmanType A, const KalmanType B) __attribute__((const));
	static inline KalmanType MUL(const KalmanType A, const KalmanType B)
	{
		return A * B;
	}

	static inline KalmanType MUL(const KalmanType A, const KalmanType B) __attribute__((const));
	static inline KalmanType DIV(const KalmanType A, const KalmanType B)
	{
		return A / B;
	}


/* Can't be done! */
#define CONSTANT_P 0
#if !CONSTANT_P
	static KalmanType P[2][2] = {
		{ 1000, 0 },
		{ 0, 1000 },
	};

#else
	const KalmanType P[2][2] = {
		{0.00744937522098925, -0.0001186981697849},
		{-0.00011869820652688, 0.00064110209608602}
	};
#endif

	/* Variances */
	static const KalmanType	R_Angle	= (0.3);
	/* The bigger, the more we trust accelometer */
	static const KalmanType	Q_Angle	= (0.010);
	static const KalmanType	Q_Gyro	= (0.00001);

	/* State matrix */
	KalmanType Angle;
	KalmanType Bias;
/*	KalmanType Rate; */

	/* Getters */
	static inline Int GetBias(void)
	{
		return 6000.0/3.14159 * Bias;
	}

	static inline Int GetAngle(void)
	{
		return 60000.0/3.14159 * Angle;
	}


	static inline void Update(const KalmanType q_m, const KalmanType dt)
	{
		/* Unbias our gyro */
		const KalmanType		q = q_m - Bias;

		/* Pdot = A * P + P * A' + Q */
#if !CONSTANT_P
		const KalmanType Pdot[2 * 2] = {
			Q_Angle - P[0][1] - P[1][0],
			-P[1][1],
			-P[1][1],
			Q_Gyro
		};
#endif

		Angle += MUL(q,  dt);

		/* Update the covariance matrix */
#if !CONSTANT_P
		P[0][0] += MUL(Pdot[0], dt);
		P[0][1] += MUL(Pdot[1], dt);
		P[1][0] += MUL(Pdot[2], dt);
		P[1][1] += MUL(Pdot[3], dt);
#endif
	}


	static inline void Predict(const KalmanType AccAngle)
	{
		/* Error estimate */
		const KalmanType Angle_err = AccAngle - Angle;
		const KalmanType C_0 = 1;
		const KalmanType PCt_0 = MUL(C_0, P[0][0]);
		const KalmanType PCt_1 = MUL(C_0, P[1][0]);

		const KalmanType E = R_Angle + MUL(C_0, PCt_0);
		const KalmanType K_0 = DIV(PCt_0, E);
		const KalmanType K_1 = DIV(PCt_1, E);

#if !CONSTANT_P
		const KalmanType t_0 = PCt_0;
		const KalmanType t_1 = MUL(C_0, P[0][1]);

		P[0][0] -= MUL(K_0, t_0);
		P[0][1] -= MUL(K_0, t_1);
		P[1][0] -= MUL(K_1, t_0);
		P[1][1] -= MUL(K_1, t_1);
#endif
		Angle += MUL(K_0, Angle_err);
		Bias  += MUL(K_1, Angle_err);
	}
}