// file: $isip_ifc/class/algo/ParticleFilter/ParticleFilter.h
// version: $Id: ParticleFilter.h 10487 2006-03-15 17:56:10Z srinivas $
//

// make sure definitions are only made once
//
#ifndef ISIP_PARTICLEFILTER
#define ISIP_PARTICLEFILTER

#ifndef ISIP_ALGORITHM_BASE
#include <AlgorithmBase.h>
#endif

#ifndef ISIP_CIRCULAR_DELAY_LINE
#include <CircularDelayLine.h>
#endif

#ifndef ISIP_PREDICTION
#include <Prediction.h>
#endif

#ifndef ISIP_RANDOM
#include <Random.h>
#endif

// ParticleFilter: a class that computes the states (features) of a
// signal using particle filter.
//
// particle filters are based on sequential Monte Carlo (SMC)
// technique. it is a technique for implementaing a recursive Bayesian
// filter by MC simulations.

//
// particle filters are used to model signals using sequential prediction
// techniques. a particle filter is represented by to state space equations:
//
// state equation:       x(k) = F(x(k-1), u(k-1))
// observation equation: y(k) = H(x(k), v(k))
//
// where:
//  k -> time instant
//  x -> hidden (to find) state vector
//  y -> observation (measurement) vector
//  u -> process noise
//  v -> observation noise
//  F -> state transition model
//  H -> observation model
//
// process noise and observation noise can be have either Gaussian or
// nonGaussian statistics.
// state transition model and observation model can be either linear
// or nonlinear and also both of these models can be time-varying.
//
// particle filtering consists of two steps:
//
// step 1:
// 
//  p(x(k)|z(1:k-1)) = (integration) p(x(k)|x(k-1))  p(x(k-1)|y(k-1) dxk-1
//
// step 2: at time step k, as measurement y(k) becomes available,
// update stage is executed
//
//                          p(y(k)|x(k))  p(x(k)|y(1:k-1))
//  p(x(k)k|y(1:k)) = ------------------------------------------
//                                p(y(k)|y(1:k-1))
//
//  normalizing constant
//
//  p(y(k)|y(1:k-1)) =  (integration) p(y(k)|x(k))  p(x(k)|y(1:k-1)) dxk
//
//  where:
//
//  p(y(k)|x(k)) is known from observation equation and statistics of v(k)
//  read p(a|b) as probability of 'a' given 'b'
//
// for our implementation, we have considered the following:
// 1. x    -> a vector
// 2. y    -> a scalar time series
// 3. u, v -> Gaussian zero mean noise with statistics as
//            u -> N(0, Q); v -> N(0, R)
// 4. F, H -> linear AR models, hence matrices.
// 
// such a implementation would be termed as particle kalman filter.
// also refer KalmanFilter class for more details on kalmanfilter
// implementation
//
// the implementations in this class follow:
//
// N. Arulampalam, S. Maskell, N. Gordan, and T. Clapp, "Tutorial On
// Particle Filters For Online Nonlinear/ Non-Gaussian Bayesian
// Tracking," IEEE Transactions on Signal Processing, vol. 50, no. 2,
// pp. 174-188, February 2002.
//
//  R. van der Merve, N. de Freitas, A. Doucet, and E. Wan, "The
//  Unscented Particle Filter," Technical Report CUED/F-INFENG/TR 380,
//  Cambridge University Engineering Department, Cambridge University,
//  U.K., August 2000.
//
// currently, only one mode is supported:
// algorithm: CONVENTIONAL,
// implementation: AR_SCALAR
//
class ParticleFilter : public AlgorithmBase {

  //---------------------------------------------------------------------------
  //
  // public constants
  //
  //---------------------------------------------------------------------------
public:
  
  // define the class name
  //
  static const String CLASS_NAME;

  //----------------------------------------
  //
  // other important constants
  //
  //----------------------------------------
  
  // define the algorithm choices
  //
  enum ALGORITHM { CONVENTIONAL = 0, UNSCENTED, DEF_ALGORITHM = CONVENTIONAL };

  // define the implementation choices
  //
  enum IMPLEMENTATION { AR_SCALAR  = 0, AR_VECTOR,
			DEF_IMPLEMENTATION = AR_SCALAR };

  // define the static NameMap objects
  //
  static const NameMap ALGO_MAP;
  static const NameMap IMPL_MAP;

  //----------------------------------------
  //
  // i/o related constants
  //
  //----------------------------------------

  static const String DEF_PARAM;
  static const String PARAM_ALGORITHM;
  static const String PARAM_IMPLEMENTATION;
  static const String PARAM_CMODE;
  static const String PARAM_PARTICLES;

  // specific to our implementation ideology: particle kalman filter
  // 
  static const String PARAM_ORDER;

  // specific to the class implementation
  //
  static const String PARAM_NOISE_PRO;
  static const String PARAM_NOISE_OBS;
  static const String PARAM_NOISE_ERR;
  
  //----------------------------------------
  //
  // default values and arguments
  //
  //----------------------------------------  

  // define default values for the filter order
  //
  static const long DEF_ORDER = -1;
  static const long DEF_PARTICLES = -1;
  static const float DEF_PRO = -1.0;
  static const float DEF_OBS = -1.0;
  static const float DEF_ERR_EST = -1.0;
  static const VectorFloat DEF_STATE_MEM;
  static const MatrixFloat DEF_TRN;
  static const VectorFloat DEF_OBS_D;

  // define default argument(s)
  //
  static const AlgorithmData::COEF_TYPE DEF_COEF_TYPE =
  AlgorithmData::SIGNAL;
  
  //----------------------------------------
  //
  // error codes
  //
  //----------------------------------------
  // check if you have updated the ERROR MESSAGE file
  //
  // ERR -> ParticleFilter class general error
  // use ERR_UNKALG -> unknown algorithm
  // use ERR_UNKIMP -> unknown implementation
  // use ERR_UNSUPA -> unsupported algorithm
  // use ERR_UNSUPI -> unsupported implementation
  // use ERR_ORDER -> incorrect analysis order
  // make suitable changes in the ErrorMessage.h file
  //
  static const long ERR = 73350;
  static const long ERR_MISDIM = 73351;
  static const long ERR_INCPAR = 73352;

  //---------------------------------------------------------------------------
  //
  // protected data
  //
  //---------------------------------------------------------------------------
protected:
  
  // algorithm name
  //
  ALGORITHM algorithm_d;

  // implementation name
  //
  IMPLEMENTATION implementation_d;
 
  // algorithm: CONVENTIONAL
  // implementation: AR_SCALAR
  //
  // define regression order:
  // this is similar to a linear prediction order and specifies the
  // number of past elements on which the current state depends
  //
  Long order_d;
  Long particles_d;

  // static memory manager
  //
  static MemoryManager mgr_d;
  
  // similar to transfer functions
  // trans_model_d relates previous state to current state
  // obsn_model_d relates current obsn to current state
  //
  MatrixFloat trn_d;
  VectorFloat obs_d;
  
  // parameter similar to input
  //
  VectorFloat obsn_d;

  // parameter to store the initial state given by the user
  //
  VectorFloat state_mem_d;

  // related to the standard deviation for the noise - process,
  // observation, estimates
  //
  Float covar_nse_pro_d;
  Float covar_nse_obs_d;
  Float covar_err_est_d;
  
  // results will be stored here
  //
  VectorFloat data_filt_d;

  //---------------------------------------------------------------------------
  //
  // required public methods
  //
  //---------------------------------------------------------------------------
public:
    
  // method: name
  //
  static const String& name() {
    return CLASS_NAME;
  }

  // other static methods
  //
  static boolean diagnose(Integral::DEBUG debug_level);
  
  // debug methods:
  //  setDebug is inherited from the base class
  //
  boolean debug(const unichar* msg) const;

  // method: destructor
  //
  ~ParticleFilter() {}

  // method: default constructor
  //
  ParticleFilter(ALGORITHM algorithm = DEF_ALGORITHM,
		 IMPLEMENTATION implementation = DEF_IMPLEMENTATION,
		 long order = DEF_ORDER, long particles = DEF_PARTICLES) {
    algorithm_d = algorithm;
    implementation_d = implementation;
    order_d = order;
    particles_d = particles;
    is_valid_d = false;
  }

  // method: copy constructor
  //
  ParticleFilter(const ParticleFilter& arg) {
    assign(arg);
  }

  // assign methods
  //
  boolean assign(const ParticleFilter& arg);

  // method: operator=
  //
  ParticleFilter& operator= (const ParticleFilter& arg) {
    assign(arg);
    return *this;
  }
  
  // i/o methods
  //
  long sofSize() const;
  
  boolean read(Sof& sof, long tag, const String& name = CLASS_NAME);
  boolean write(Sof& sof, long tag, const String& name = CLASS_NAME) const;

  boolean readData(Sof& sof, const String& pname = DEF_PARAM,
		   long size = SofParser::FULL_OBJECT,
		   boolean param = true,
                   boolean nested = false);
  boolean writeData(Sof& sof, const String& pname = DEF_PARAM) const;

  // equality methods
  //
  boolean eq(const ParticleFilter& arg) const;
  
  // method: new
  //
  static void* operator new(size_t size) {
    return mgr_d.get();
  }

  // method: new[]
  //
  static void* operator new[](size_t size) {
    return mgr_d.getBlock(size);
  }

  // method: delete
  //
  static void operator delete(void* ptr) {
    mgr_d.release(ptr);
  }

  // method: delete[]
  //
  static void operator delete[](void* ptr) {
    mgr_d.releaseBlock(ptr);
  }

  // method: setGrowSize
  //
  static boolean setGrowSize(long grow_size) {
    return mgr_d.setGrow(grow_size);
  }

  // other memory management methods
  //
  boolean clear(Integral::CMODE ctype = Integral::DEF_CMODE);

  //---------------------------------------------------------------------------
  //
  // class-specific public methods:
  //  set methods
  //
  //---------------------------------------------------------------------------

  // method: setAlgorithm
  //
  boolean setAlgorithm(ALGORITHM algorithm) {
    algorithm_d = algorithm;
    is_valid_d = false;
    return true;
  }

  // method: setImplementation
  //
  boolean setImplementation(IMPLEMENTATION implementation) {
    implementation_d = implementation;
    is_valid_d = false;
    return true;  
  }
  
  // method: setTransitionModel
  //
  boolean setTransitionModel(MatrixFloat trn = DEF_TRN) {
    trn_d.assign(trn);
    is_valid_d = false;
    return true;  
  }

  // method: setObsnModel
  //
  boolean setObsnModel(VectorFloat obs = DEF_OBS) {
    obs_d.assign(obs);
    is_valid_d = false;
    return true;  
  }

  // method: setModel
  //
  boolean setModel(MatrixFloat trn = DEF_TRN, VectorFloat obs = DEF_OBS) {
    trn_d.assign(trn);
    obs_d.assign(obs);
    is_valid_d = false;
    return true;  
  }

  // method setParticles
  //
  boolean setParticles(long particles = DEF_PARTICLES) {
    particles_d = particles;
    is_valid_d = false;
    return true;  
  }  

  // method: setObservation
  //
  boolean setObservation (VectorFloat& data) {
    obsn_d = data;
    return true;  
  }

  // method: setOrder
  //
  boolean setOrder(long order = DEF_ORDER) {
    order_d = order;
    is_valid_d = false;
    return true;  
  }

  // Method: setProcessNoise
  //
  boolean setProcessNoise (float& covar_nse_pro) {
    covar_nse_pro_d = covar_nse_pro;
    return true;
  }
      
  // Method: setObsnNoise
  //
  boolean setObsnNoise (float& covar_nse_obs) {
    covar_nse_obs_d = covar_nse_obs;
    return true;
  }
      
  // Method: setEstiamteError
  //
  boolean setEstimateError (float& covar_err_est) {
    covar_err_est_d = covar_err_est;
    return true;
  }

  // Method: setNoise
  //
  boolean setNoise (float& covar_nse_pro, float& covar_nse_obs,
		    float& covar_err_est) {
    covar_nse_pro_d = covar_nse_pro;
    covar_nse_obs_d = covar_nse_obs;
    covar_err_est_d = covar_err_est;
    return true;
  }
  
  // method: set
  //
  boolean set(ALGORITHM algorithm = DEF_ALGORITHM,
	      IMPLEMENTATION implementation = DEF_IMPLEMENTATION,
	      long order = DEF_ORDER, long particles = DEF_PARTICLES,
	      float covar_nse_pro = DEF_PRO, float covar_nse_obs = DEF_OBS,
	      float covar_err_est = DEF_ERR_EST) {
    algorithm_d = algorithm;
    implementation_d = implementation;
    order_d = order;
    particles_d = particles;
    covar_nse_pro_d = covar_nse_pro;
    covar_nse_obs_d = covar_nse_obs;
    covar_err_est_d = covar_err_est;
    is_valid_d = false;
    return true;
  }
  
  //---------------------------------------------------------------------------
  //
  // class-specific public methods:
  //  get methods
  //
  //---------------------------------------------------------------------------

  // method: getAlgorithm
  //
  ALGORITHM getAlgorithm() const {
    return algorithm_d;
  }  

  // method: getImplementation
  //
  IMPLEMENTATION getImplementation() const {
    return implementation_d;
  }
  
  // method: getInitialState
  // to get the value of the initial (starting) state
  //
  boolean getInitialState(VectorFloat& state_mem) const {
    return state_mem.assign(state_mem_d);
  }
  
  // method: getOrder
  // to get the number of data points the implementation is working on
  //
  long getOrder() const {
    return order_d;
  }
  
  // method: getParticles
  // to get the number of samples (particles) representing
  // the sample distribution
  //
  long getParticles() const {
    return particles_d;
  }
  
  // method: getTransitionModel
  // to get the transition model
  // transition model relates the state with the previous state
  //
  boolean getTransitionModel(MatrixFloat& trn) const {
    return trn.assign(trn_d);
  }

  // method: getObservationModel
  // to get the observation model,
  // observation model relates the observation to the state
  //
  boolean getObservationModel(VectorFloat& obs) const {
    return obs.assign(obs_d);
  }

  // method: getModel
  // to get the transition model and observation model
  //
  boolean getModel(MatrixFloat& trn, VectorFloat& obs) const {
    trn.assign(trn_d);
    return obs.assign(obs_d);
  }
    
  // Method: getProcessNoise
  //
  boolean getProcessNoise (float covar_nse_pro) {
    covar_nse_pro = covar_nse_pro_d;
    return true;
  }
      
  // Method: getObsnNoise
  //
  boolean getObsnNoise (float covar_nse_obs) {
    covar_nse_obs = covar_nse_obs_d;
    return true;
  }
      
  // Method: getEstiamteError
  //
  boolean getEstimateError (float covar_err_est) {
    covar_err_est = covar_err_est_d;
    return true;
  }

  // Method: getNoise
  //
  boolean getNoise (float covar_nse_pro, float covar_nse_obs,
		    float covar_err_est) {
    covar_nse_pro = covar_nse_pro_d;
    covar_nse_obs = covar_nse_obs_d;
    covar_err_est = covar_err_est_d;
    return true;
  }
  
 // method: getResult
  // to get the filtered data for a particular frame
  //
  boolean getResult(VectorFloat& data_filt) const {
    return data_filt.assign(data_filt_d);
  }
  
  // method: get
  //
  boolean get(ALGORITHM& algorithm,
	      IMPLEMENTATION& implementation) {
    algorithm = algorithm_d;
    implementation = implementation_d;
    return true;
  }
  
  //---------------------------------------------------------------------------
  //
  // class-specific public methods:
  //  computational methods
  //
  //---------------------------------------------------------------------------

  // compute methods
  //
  boolean compute(VectorFloat& output, const VectorFloat& input,
		  AlgorithmData::COEF_TYPE input_coef_type = DEF_COEF_TYPE,
		  long index = DEF_CHANNEL_INDEX);

  //---------------------------------------------------------------------------
  //
  // class-specific public methods:
  //  public methods required by the AlgorithmBase interface contract
  //
  //---------------------------------------------------------------------------

  // assign method
  //
  boolean assign(const AlgorithmBase& arg);

  // equality method
  //
  boolean eq(const AlgorithmBase& arg) const;

  // method: className
  //
  const String& className() const {
    return CLASS_NAME;
  }

  // initialization method
  //
  boolean init();

  // apply methods
  //
  boolean apply(Vector<AlgorithmData>& output,
		const Vector< CircularBuffer<AlgorithmData> >& input);
  
  // method to set the parser
  //
  boolean setParser(SofParser* parser);

  //---------------------------------------------------------------------------
  //
  // private methods
  //
  //---------------------------------------------------------------------------
private:

  // filter i/o methods
  //
  boolean readDataParticleFilter(Sof& sof, const String& pname,
				 long size, boolean param, boolean nested);
  boolean writeDataParticleFilter(Sof& sof, const String& pname) const;

  // class-specific computational methods
  //
  // algorithm: CONVENTIONAL
  // implementation: AR_SCALAR 
  //
  // to compute the states, actually current state is calculated and
  // passed to the filtered_data_d which keeps on storing the data for
  // the complete frame
  //
  boolean computeStates(const VectorFloat& obsn);

  // to generate gaussian distributed random sequence
  //
  boolean generateParticles();
  
  // to estimate the state based on the previous state value
  //
  boolean computePredictStates(MatrixFloat& states);
  
  // to compute the importance weights
  //
  boolean computeImportanceWeights(VectorFloat& weights,
				   MatrixFloat& states, float obsn);
  
  // to compute the importance weights
  //
  boolean computeUpdateStates(MatrixFloat& states, VectorFloat& weights);

  // to compute the states back from their particles
  //
  boolean getStates();

  // to calculate LP coefficients
  // should accept the first frame of data from user and then return
  // the lpc coefficients and noise variance
  //
  boolean computeLPC(VectorFloat& lpc);

  // to initialize the transition model matrix with the LP
  // coefficients
  //
  boolean initializeTransitionModel(VectorFloat lpc);

  // to initialize the state
  //
  boolean initializeState();
  
  // to initialize the dimensions of the weights
  //
  boolean initializeWeights();

  // to initialize the noise variance
  //
  boolean initializeNoiseVariances();

  // to initialize the observation model matrix, even though it is a
  // standard one with the dimensions equal to order
  //
  boolean initializeObservationModel();
};
// end of include file
// 
#endif
