#include <cpExtensions/Algorithms/KalmanFilter.h>

#include <cstdlib>
#include <vnl/algo/vnl_matrix_inverse.h>

// -------------------------------------------------------------------------
template< typename T >
void cpExtensions::Algorithms::KalmanFilter< T >::
Configure( unsigned int s, unsigned int i, unsigned int m )
{
  this->m_StateSize   = s;
  this->m_InputSize   = i;
  this->m_MeasureSize = m;

  // Matrices
  this->m_A.set_size( s, s );
  this->m_B.set_size( s, i );
  this->m_H.set_size( m, s );
  this->m_Id.set_size( s, s );
  this->m_Q.set_size( s, s );
  this->m_R.set_size( m, m );
  this->m_P0.set_size( s, s );
  this->m_K.set_size( s, m );
  this->m_Pm.set_size( s, s );
  this->m_Pp.set_size( s, s );

  // Vectors
  this->m_x0.set_size( s );
  this->m_u.set_size( i );
  this->m_m.set_size( m );
  this->m_xm.set_size( s );
  this->m_xp.set_size( s );

  // Static values
  this->m_Id.set_identity( );
}

// -------------------------------------------------------------------------
template< typename T >
void cpExtensions::Algorithms::KalmanFilter< T >::
Initialize( )
{
  // Set all to the first iteration
  this->m_I = 0;
  this->m_Step = Self::StFilt;
}

// -------------------------------------------------------------------------
template< typename T >
void cpExtensions::Algorithms::KalmanFilter< T >::
Predict( )
{
  if( this->m_Step == Self::StFilt )
  {
    if( this->m_I == 0 )
    {
      this->m_xp = this->m_x0;
      this->m_Pp = this->m_P0;

    } // fi

    // Project the state
    this->m_xm = ( this->m_A * this->m_xp ) + ( this->m_B * this->m_u );

    // Project the error
    this->m_Pm  = this->m_A * this->m_Pp * this->m_A.transpose( );
    this->m_Pm += this->m_Q;

    this->m_Step = Self::StPred;

  } // fi
}

// -------------------------------------------------------------------------
template< typename T >
void cpExtensions::Algorithms::KalmanFilter< T >::
Innovate( )
{
  typedef vnl_matrix_inverse< T > _TInv;

  if( this->m_Step == Self::StPred )
  {
    // Kalman gain
    this->m_K  = this->m_Pm * this->m_H.transpose( );
    this->m_K *=
      _TInv(
        ( this->m_H * this->m_Pm * this->m_H.transpose( ) ) + this->m_R
        );

    this->m_Step = Self::StInno;

  } // fi
}

// -------------------------------------------------------------------------
template< typename T >
void cpExtensions::Algorithms::KalmanFilter< T >::
Filtrate( )
{
  if( this->m_Step == Self::StInno )
  {
    // A posteriori state
    this->m_xp  = this->m_xm;
    this->m_xp += this->m_K * ( this->m_m - ( this->m_H * this->m_xm ) );

    // A posteriori error
    this->m_Pp = ( this->m_Id - ( this->m_K * this->m_H ) ) * this->m_Pm;

    this->m_Step = Self::StFilt;
    this->m_I++;

  } // fi
}

// -------------------------------------------------------------------------
template< typename T >
cpExtensions::Algorithms::KalmanFilter< T >::
KalmanFilter( )
  : Superclass( )
{
  this->Configure( 1, 1, 1 );
}

// -------------------------------------------------------------------------
template< typename T >
cpExtensions::Algorithms::KalmanFilter< T >::
~KalmanFilter( )
{
}

// -------------------------------------------------------------------------
// Explicit instantiations

using namespace cpExtensions::Algorithms;

template class KalmanFilter< float >;
template class KalmanFilter< double >;

// eof - $RCSfile$