#include <itpp/itbase.h>
#include "libBM.h"
#include "libKF.h"

using namespace itpp;

using std::endl;

KalmanFull::KalmanFull( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
	dimx = A0.rows();
	dimu = B0.cols();
	dimy = C0.rows();

	it_assert_debug( A0.cols()==dimx, "KalmanFull: A is not square" );
	it_assert_debug( B0.rows()==dimx, "KalmanFull: B is not compatible" );
	it_assert_debug( C0.cols()==dimx, "KalmanFull: C is not square" );
	it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),	"KalmanFull: D is not compatible" );
	it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "KalmanFull: R is not compatible" );
	it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "KalmanFull: Q is not compatible" );

	A = A0;
	B = B0;
	C = C0;
	D = D0;
	R = R0;
	Q = Q0;
	mu = mu0;
	P = P0;
}

void KalmanFull::bayes( const vec &dt, bool evalll ) {
	it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );

	vec u = dt.get( dimy,dimy+dimu-1 );
	vec y = dt.get( 0,dimy-1 );
	//Time update
	mu = A*mu + B*u;
	P  = A*P*A.transpose() + Q;

	//Data update
	_Ry = C*P*C.transpose() + R;
	_iRy = inv( _Ry );
	_K = P*C.transpose()*_iRy;
	P -= _K*C*P; // P = P -KCP;
	mu += _K*( y-C*mu-D*u );
};

std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) {
	os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl;
}