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

#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__

#include <cstdarg>

// -------------------------------------------------------------------------
template< class S, unsigned int D >
template< class V, class M >
void
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
GetModel( V& m, M& E ) const
{
  TMatrix Es = ( this->m_S1 * this->m_S1.transpose( ) );

  for( unsigned int i = 0; i < D; ++i )
  {
    m[ i ] = this->m_S1[ i ][ 0 ];
    if( this->m_N > 0 )
      m[ i ] /= double( this->m_N );

    for( unsigned int j = 0; j < D; ++j )
    {
      E[ i ][ j ] = double( 0 );
      if( this->m_N > 0 )
        E[ i ][ j ] -= double( Es[ i ][ j ] / S( this->m_N ) );
      if( this->m_N > 1 )
      {
        E[ i ][ j ] += double( this->m_S2[ i ][ j ] );
        E[ i ][ j ] /= double( this->m_N - 1 );

      } // fi

    } // rof

  } // rof
}

// -------------------------------------------------------------------------
template< class S, unsigned int D >
void
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
Clear( )
{
  this->m_N = 0;
  this->m_S1.set_size( D, 1 );
  this->m_S2.set_size( D, D );
  this->m_S1.fill( S( 0 ) );
  this->m_S2.fill( S( 0 ) );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class S, unsigned int D >
template< class V >
void
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
AddSample( const V& sample )
{
  for( unsigned int i = 0; i < D; ++i )
  {
    this->m_S1[ i ][ 0 ] += S( sample[ i ] );
    for( unsigned int j = i; j < D; ++j )
    {
      this->m_S2[ i ][ j ] += S( sample[ i ] ) * S( sample[ j ] );
      this->m_S2[ j ][ i ] = this->m_S2[ i ][ j ];

    } // rof

  } // rof
  this->m_N++;
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class S, unsigned int D >
void
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
AddSample( const S& s_x, const S& s_y, ... )
{
  static S sample[ D ];

  std::va_list args_lst;
  va_start( args_lst, s_y );
  sample[ 0 ] = s_x;
  sample[ 1 ] = s_y;
  for( unsigned int d = 2; d < D; ++d )
    sample[ d ] = S( va_arg( args_lst, double ) );
  va_end( args_lst );
  this->AddSample( sample );
}

// -------------------------------------------------------------------------
template< class S, unsigned int D >
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
IterativeGaussianModelEstimator( )
  : Superclass( )
{
  this->Clear( );
}

// -------------------------------------------------------------------------
template< class S, unsigned int D >
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
~IterativeGaussianModelEstimator( )
{
}

#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__

// eof - $RCSfile$