/**
* @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);
  }
}