// -------------------------------------------------------------------------
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// -------------------------------------------------------------------------

#ifndef __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
#define __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__

#include <cstdarg>

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Clear( )
{
  this->m_NumberOfSamples = 0;
  this->m_Mean.Fill( TScalar( 0 ) );
  this->m_Covariance.Fill( TScalar( 0 ) );
  this->m_InverseUpdated = false;
  this->m_InverseUnbiasedCovariance.Fill( TScalar( 0 ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const _TOtherScalar& x1, ... )
{
  TVector s;
  std::va_list lst;
  va_start( lst, x1 );
  s[ 0 ] = TScalar( x1 );
  for( unsigned int d = 1; d < _VDimension; ++d )
    s[ d ] = TScalar( va_arg( lst, double ) );
  va_end( lst );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const _TOtherScalar* array )
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( array[ d ] );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const vnl_vector< _TOtherScalar >& v )
{
  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
  TVector s;
  for( unsigned d = 0; d < len; ++d )
    s[ d ] = TScalar( v[ d ] );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const itk::CovariantVector< _TOtherScalar, _VDimension >& v )
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const itk::Point< _TOtherScalar, _VDimension >& p )
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( p[ d ] );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
AddSample( const itk::Vector< _TOtherScalar, _VDimension >& v )
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  this->_AddSample( s );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis( const _TOtherScalar& x1, ... ) const
{
  TVector s;
  std::va_list lst;
  va_start( lst, x1 );
  s[ 0 ] = TScalar( x1 );
  for( unsigned int d = 1; d < _VDimension; ++d )
    s[ d ] = TScalar( va_arg( lst, double ) );
  va_end( lst );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis( const _TOtherScalar* array ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( array[ d ] );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis( const vnl_vector< _TOtherScalar >& v ) const
{
  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
  TVector s;
  for( unsigned d = 0; d < len; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis(
  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
  ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis( const itk::Point< _TOtherScalar, _VDimension >& p ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( p[ d ] );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
SquaredMahalanobis( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_SquaredMahalanobis( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density( const _TOtherScalar& x1, ... ) const
{
  TVector s;
  std::va_list lst;
  va_start( lst, x1 );
  s[ 0 ] = TScalar( x1 );
  for( unsigned int d = 1; d < _VDimension; ++d )
    s[ d ] = TScalar( va_arg( lst, double ) );
  va_end( lst );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density( const _TOtherScalar* array ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( array[ d ] );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density( const vnl_vector< _TOtherScalar >& v ) const
{
  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
  TVector s;
  for( unsigned d = 0; d < len; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density(
  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
  ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density( const itk::Point< _TOtherScalar, _VDimension >& p ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( p[ d ] );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
template< class _TOtherScalar >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
Density( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
{
  TVector s;
  for( unsigned d = 0; d < _VDimension; ++d )
    s[ d ] = TScalar( v[ d ] );
  return( this->_Density( s ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
IterativeGaussianModelEstimator( )
  : Superclass( )
{
  this->Clear( );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
~IterativeGaussianModelEstimator( )
{
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
void cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
_AddSample( const TVector& v ) const
{
  this->m_NumberOfSamples += 1;
  TScalar a = TScalar( 1 ) / TScalar( this->m_NumberOfSamples );
  TScalar b = TScalar( 1 ) - a;
  TVector c = v - this->m_Mean;
  TMatrix m;
  for( unsigned int i = 0; i < _VDimension; ++i )
  {
    m[ i ][ i ] = c[ i ] * c[ i ];
    for( unsigned int j = i + 1; j < _VDimension; ++j )
      m[ i ][ j ] = m[ j ][ i ] = c[ i ] * c[ j ];

  } // rof
  this->m_Covariance = ( this->m_Covariance + ( m * a ) ) * b;
  this->m_Mean = ( this->m_Mean * b ) + ( v * a );

  TScalar u =
    TScalar( this->m_NumberOfSamples ) /
    TScalar( this->m_NumberOfSamples - 1 );
  this->m_UnbiasedCovariance = this->m_Covariance * u;
  this->m_InverseUpdated = false;
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
_SquaredMahalanobis( const TVector& v ) const
{
  if( !this->m_InverseUpdated )
  {
    double d =
      double(
        vnl_determinant( this->m_UnbiasedCovariance.GetVnlMatrix( ) )
        );
    if( std::fabs( d ) > double( 0 ) )
      this->m_InverseUnbiasedCovariance =
        this->m_UnbiasedCovariance.GetInverse( );
    else
      this->m_InverseUnbiasedCovariance.SetIdentity( );
    this->m_InverseUpdated = true;

  } // fi
  TVector x = v - this->m_Mean;
  return( x * ( this->m_InverseUnbiasedCovariance * x ) );
}

// -------------------------------------------------------------------------
template< class _TScalar, unsigned int _VDimension >
_TScalar cpExtensions::Algorithms::
IterativeGaussianModelEstimator< _TScalar, _VDimension >::
_Density( const TVector& v ) const
{
  static const double _2pik =
    std::pow( double( 2 ) * double( vnl_math::pi ), _VDimension );

  double s = double( this->_SquaredMahalanobis( v ) ) / double( 2 );
  double d =
    double(
      vnl_determinant( this->m_UnbiasedCovariance.GetVnlMatrix( ) )
      );
  return( _TScalar( std::exp( -s ) / std::sqrt( _2pik * d ) ) );
}

#endif // __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__

// eof - $RCSfile$