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

#ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
#define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__

#include <cpExtensions/Config.h>

#include <itkObject.h>
#include <itkObjectFactory.h>
#include <vnl/vnl_matrix.h>
#include <vnl/vnl_vector.h>

// -------------------------------------------------------------------------
#define kalmanGetSetMacro( type, name )         \
  itkGetConstMacro( name, type );               \
  itkSetMacro( name, type );

// -------------------------------------------------------------------------
#define kalmanGetMatrixMacro( var, name )       \
  virtual const TMatrix& Get##name( ) const     \
  { return( this->m_##var ); }

// -------------------------------------------------------------------------
#define kalmanSetMatrixMacro( var, name )       \
  virtual void Set##name( const TMatrix& m )    \
  { this->Set##var( m ); }

// -------------------------------------------------------------------------
#define kalmanGetSetMatrixMacro( var, name )    \
  kalmanGetMatrixMacro( var, name );            \
  kalmanSetMatrixMacro( var, name );

// -------------------------------------------------------------------------
#define kalmanGetVectorMacro( var, name )       \
  virtual const TVector& Get##name( ) const     \
  { return( this->m_##var ); }

// -------------------------------------------------------------------------
#define kalmanSetVectorMacro( var, name )       \
  virtual void Set##name( const TVector& v )    \
  { this->Set##var( v ); }

// -------------------------------------------------------------------------
#define kalmanGetSetVectorMacro( var, name )    \
  kalmanGetVectorMacro( var, name );            \
  kalmanSetVectorMacro( var, name );

namespace cpExtensions
{
  namespace Algorithms
  {
    /**
     * Abstract class implementing the classical kalman filter. See
     * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
     * for a description of this algorithm.
     */
    template< typename T >
    class cpExtensions_EXPORT KalmanFilter
      : public itk::Object
    {
    public:
      typedef KalmanFilter                    Self;
      typedef itk::Object                     Superclass;
      typedef itk::SmartPointer< Self >       Pointer;
      typedef itk::SmartPointer< const Self > ConstPointer;

      // Template parameters types
      typedef T TScalar;

      // VNL types
      typedef vnl_matrix< TScalar > TMatrix;
      typedef vnl_vector< TScalar > TVector;

      //  stage identificators
      enum
      {
        StPred,
        StInno,
        StFilt
      };

    public:
      itkNewMacro( Self );
      itkTypeMacro( KalmanFilter, itkObject );

      // Values
      itkGetConstMacro( StateSize, unsigned int );
      itkGetConstMacro( InputSize, unsigned int );
      itkGetConstMacro( MeasureSize, unsigned int );

      // Matrices
      kalmanGetSetMacro( TMatrix, A );
      kalmanGetSetMacro( TMatrix, B );
      kalmanGetSetMacro( TMatrix, H );
      kalmanGetSetMacro( TMatrix, Q );
      kalmanGetSetMacro( TMatrix, R );
      kalmanGetSetMacro( TMatrix, P0 );
      kalmanGetSetMacro( TMatrix, K );
      kalmanGetSetMacro( TMatrix, Pm );
      kalmanGetSetMacro( TMatrix, Pp );

      // Vectors
      kalmanGetSetMacro( TVector, x0 );
      kalmanGetSetMacro( TVector, u );
      kalmanGetSetMacro( TVector, m );
      kalmanGetSetMacro( TVector, xm );
      kalmanGetSetMacro( TVector, xp );

      // Human-readable matrices
      kalmanGetSetMatrixMacro( A, TransitionMatrix );
      kalmanGetSetMatrixMacro( B, InputControlMatrix );
      kalmanGetSetMatrixMacro( H, MeasureMatrix );
      kalmanGetSetMatrixMacro( Q, ProcessNoise );
      kalmanGetSetMatrixMacro( R, MeasureNoise );
      kalmanGetSetMatrixMacro( P0, InitialNoise );
      kalmanGetSetMatrixMacro( K, Gain );
      kalmanGetSetMatrixMacro( Pm, APrioriNoise );
      kalmanGetSetMatrixMacro( Pp, APosterioriNoise );

      // Human-readable vectors
      kalmanGetSetVectorMacro( x0, InitialState );
      kalmanGetSetVectorMacro( u, Input );
      kalmanGetSetVectorMacro( m, Measure );
      kalmanGetSetVectorMacro( xm, APrioriState );
      kalmanGetSetVectorMacro( xp, APosterioriState );

    public:

      void Configure( unsigned int s, unsigned int i, unsigned int m );

      // Iteration methods
      virtual void Initialize( );
      virtual void Predict( );
      virtual void Innovate( );
      virtual void Filtrate( );
      virtual void OneStep( )
        {
          this->Predict( );
          this->Innovate( );
          this->Filtrate( );
        }
      virtual void NSteps( unsigned int n )
        {
          for( unsigned int i = 0; i < n; i++ )
            this->OneStep( );
        }
      unsigned int CurrentIteration( ) const
        { return( this->m_I ); }
      unsigned char CurrentStep( ) const
        { return( this->m_Step ); }

    protected:
      KalmanFilter( );
      virtual ~KalmanFilter( );

    private:
      // Purposely not implemented
      KalmanFilter( const Self& );
      void operator=( const Self& );

    protected:
      // Filter dimensions
      unsigned int m_StateSize;
      unsigned int m_InputSize;
      unsigned int m_MeasureSize;

      // Transition matrices
      TMatrix m_A;  // Transition
      TMatrix m_B;  // Input control
      TMatrix m_H;  // Measure
      TMatrix m_Id; // Identity matrix

      // Noise matrices
      TMatrix m_Q; // Process noise covariance
      TMatrix m_R; // Measure noise covariance

      // Initial values
      TVector m_x0; // Initial state
      TMatrix m_P0; // Initial error covariance

      // Loop vectors
      TVector m_u;  // Last real input
      TVector m_m;  // Last real measure
      TVector m_xm; // A priori state
      TVector m_xp; // A posteriori state

      // Loop matrices
      TMatrix m_K; // kalman gain
      TMatrix m_Pm; // A priori error
      TMatrix m_Pp; // A posteriori error

      // Loop values
      unsigned int  m_I;    // Current iteration
      unsigned char m_Step; // Current step within current iteration

      // -------------------------------------------------------------------
      // Classic kronecker product operator
      // -------------------------------------------------------------------
      template< class M >
      static void Kronecker( M& AkB, const M& A, const M& B );
    };

  } // ecapseman

} // ecapseman

#include <cpExtensions/Algorithms/KalmanFilter.hxx>

#endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__

// eof - $RCSfile$