/**
* @file PDFGaussian.cpp
* @brief Normal Distribution / Gaussian PDF
* @author Erik Rodner
* @date 01/29/2008

*/
#include <iostream>
#include <sys/time.h>

#include "vislearning/math/pdf/PDFGaussian.h"
#include "core/algebra/CholeskyRobustAuto.h"

#include "core/vector/Algorithms.h"
#include "core/basics/numerictools.h"


using namespace OBJREC;

using namespace std;
using namespace NICE;



PDFGaussian::PDFGaussian(int dimension) : mean(dimension)
{
	constant = dimension * log(2 * M_PI);
	covariance.resize ( dimension, dimension );
	covariance.setIdentity();
	mean.resize ( dimension );
	mean.set(0.0);
	covCholesky = covariance;
	ldet = 0.0;
}

PDFGaussian::PDFGaussian(const NICE::Matrix & _covariance, const NICE::Vector & _mean)
{
	covariance = _covariance;
	
	CholeskyRobustAuto cr ( true, regEPS, - std::numeric_limits<double>::max(), true );
	ldet = cr.robustChol ( covariance, covCholesky );

	mean = _mean;
	constant = mean.size() * log(2 * M_PI);
	constant = 0.0;
	
	ldet=0.0;
}

PDFGaussian::~PDFGaussian()
{
}

double PDFGaussian::getNLogDensity(const NICE::Vector & x) const
{
	// (x-mean)
	Vector diff ( x-mean );
	// covInvX = covariance^-1 (x-mean)
	Vector covInvX;
	choleskySolve ( covCholesky, diff, covInvX );
	double result = constant + ldet + diff.scalarProduct( covInvX );
	return result;
}

int PDFGaussian::getDimension() const
{
	return mean.size();
}

NICE::Vector PDFGaussian::getMean()
{
	return mean;
}


NICE::Matrix PDFGaussian::getCovariance()
{
	if (covariance.rows() == 0 || covariance.cols() == 0)
		fthrow(Exception, "No covariance initialized, use other constructor !!");
	return covariance;
}

void PDFGaussian::sample(VVector & samples, int count) const
{
	for (int i = 0 ; i < count ; i++)
	{
		NICE::Vector x(mean.size());
		NICE::Vector tmp(mean.size());

		tmp = NICE::VectorT<double>::GaussRandom(mean.size(), 0.0, 1.0);
		// cholesky decomposed covariance matrix * gauss vector
		x = mean + covCholesky * tmp;
		samples.push_back(x);
	}
}