#include "chmat.h"

//using std::endl;


void chmat::opupdt ( const vec &v, double w ) {
//TODO see cholupdt in lhotse
	mat Z;
	mat R;
	mat V(1,v.length());
	V.set_row(0,v*sqrt(w));
	Z = concat_vertical ( Ch,V );
	qr ( Z,R );
	Ch = R ( 0, Ch.rows()-1, 0, Ch.cols()-1 );
};
mat chmat::to_mat() const {mat F=Ch.T() *Ch;return F;};
void chmat::mult_sym ( const mat &C ) {
	it_assert_debug(C.cols()==dim, "Wrong dimension of U");
	if(!qr(Ch*C.T(), Ch)) {it_warning("QR unstable in chmat mult_sym");}
};
void chmat::mult_sym ( const mat &C , chmat &U ) const {
	it_assert_debug(C.cols()==U.dim, "Wrong dimension of U");
	if(!qr(Ch*C.T(), U.Ch)) {it_warning("QR unstable in chmat mult_sym");}
};
void chmat::mult_sym_t ( const mat &C ) {
	it_assert_debug(C.rows()==dim, "Wrong dimension of U");
	if(!qr(Ch*C, Ch)) {it_warning("QR unstable in chmat mult_sym");}
};
void chmat::mult_sym_t ( const mat &C, chmat &U ) const {
	it_assert_debug(C.rows()==U.dim, "Wrong dimension of U");
	if(!qr(Ch*C, U.Ch)) {it_warning("QR unstable in chmat mult_sym");}
};
double chmat::logdet() const {
	double ldet=0.0; int i;
	//sum of logs of (possibly negative!) diagonal entries
	for ( i=0;i<Ch.rows();i++ ) {ldet+=log ( std::fabs ( Ch ( i,i ) ) );}
	return 2*ldet; //compensate for Ch being sqrt()
};
//TODO can be done more efficiently using BLAS, see triangular matrices
vec chmat::sqrt_mult ( const vec &v ) const {vec pom; pom = Ch*v; return pom;};
double chmat::qform ( const vec &v ) const {vec pom; pom = Ch*v; return pom*pom;};
double chmat::invqform ( const vec &v ) const {
	vec pom(v.length());
	forward_substitution(Ch.T(),v,pom);
	return pom*pom;
};
void chmat::clear() {Ch.clear();};