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

*/
#include <iostream>
#include <assert.h>

#include <core/vector/Algorithms.h>

#include "vislearning/math/pdf/PDFParzenMulti.h"

using namespace OBJREC;

const double sqrt_2_m_pi = sqrt(2*M_PI);

using namespace std;
using namespace NICE;



PDFParzenMulti::PDFParzenMulti( const VVector & _samples, const NICE::Matrix & _kernelInvCov ) 
    : samples(_samples), kernelInvCov(_kernelInvCov)
{
    assert(_samples.size());
    det = NICE::det(kernelInvCov);
    dimension = _samples[0].size();
    constant = 0.5 * ( dimension * log (2*M_PI) - log(det) );
}


PDFParzenMulti::~PDFParzenMulti()
{
}

double PDFParzenMulti::kernel ( const NICE::Vector & v ) const
{
    double bilinear_result = 0.0;
    for ( size_t k = 0 ; k < kernelInvCov.rows() ; k++ )
        for ( size_t l = k+1; l < kernelInvCov.cols(); l++ )
            bilinear_result += 2 * kernelInvCov(k,l) * v[k] * v[l];

    for ( size_t k = 0 ; k < kernelInvCov.rows(); k++ )
        bilinear_result += kernelInvCov(k,k) * v[k] * v[k];

    return exp( 0.5 * ( - bilinear_result ) );
}

double PDFParzenMulti::getNLogDensity ( const NICE::Vector & x ) const
{
    assert ( samples.size() > 0 );
	if ( x.size() != samples[0].size() )
		fthrow(Exception, "Dimensions " << x.size() << " (x) and " << samples[0].size() << " do not match.");

    double result = 0.0;
    for ( size_t k = 0 ; k < samples.size() ; k++ )
    {
		Vector n ( samples[k] );
		n -= x;
		result += kernel(n);
    }
    result /= (samples.size());

    return - log(result) + constant;
}

int PDFParzenMulti::getDimension () const
{
    assert ( samples.size() > 0 );
    return dimension;
}