// file: $isip_ifc/class/algo/ParticleFilter/pf_00.cc
// version: $Id: pf_00.cc 10413 2006-01-30 04:57:31Z patil $
//

// isip include files
//
#include "ParticleFilter.h"

//------------------------------------------------------------------------
//
// required public methods
//
//-----------------------------------------------------------------------

// method: assign
//
// arguments:
//  const ParticleFilter& arg: (input) object to be assigned
//
// return: a boolean value indicating status
//
// this method assigns the input object to the current object
//
boolean ParticleFilter::assign(const ParticleFilter& arg_a) {

  // assign data from the input kalmanfilter object
  //
  algorithm_d = arg_a.algorithm_d;
  implementation_d = arg_a.implementation_d;
  order_d = arg_a.order_d;
  particles_d = arg_a.particles_d;
  
  // assign data from initial state
  //
  state_mem_d.assign(arg_a.state_mem_d);

  // assign data from transition and observation model
  //
  trn_d.assign(arg_a.trn_d);
  obs_d.assign(arg_a.obs_d);

  // assign data from the noise model
  //
  covar_nse_pro_d = arg_a.covar_nse_pro_d;
  covar_nse_obs_d = arg_a.covar_nse_obs_d;
  covar_err_est_d = arg_a.covar_err_est_d;
  
  // assign AlgorithmBase and exit gracefully
  //
  return AlgorithmBase::assign(arg_a);
}

// method: eq
//
// arguments:
//  const ParticleFilter& arg: (input) object to be compared
//
// return: a boolean value indicating status
//
// this method checks whether the current object is identical to the
// input object
//
boolean ParticleFilter::eq(const ParticleFilter& arg_a) const {

  // compare the data members
  // all the data members except for the the observations
  // are compared
  // 
  if ((algorithm_d != arg_a.algorithm_d) ||
      (implementation_d != arg_a.implementation_d) ||
      (!state_mem_d.almostEqual(arg_a.state_mem_d)) ||
      (!trn_d.almostEqual(arg_a.trn_d)) ||
      (!obs_d.almostEqual(arg_a.obs_d)) ||
      (covar_nse_pro_d != arg_a.covar_nse_pro_d) ||
      (covar_nse_obs_d != arg_a.covar_nse_obs_d) ||
      (covar_err_est_d != arg_a.covar_nse_obs_d)) {
    return false;
  }
  
  // exit gracefully
  //
  return true;
}

// method: clear
//
// arguments:
//  Integral::CMODE ctype: (input) clear mode
//
// return: a boolean value indicating status
//
// this method resets the data members to the default values
//
boolean ParticleFilter::clear(Integral::CMODE ctype_a) {
  
  // reset the data members unless the mode is RETAIN
  //
  if (ctype_a != Integral::RETAIN) {

    // clear the design parameters
    //
    algorithm_d = DEF_ALGORITHM;
    implementation_d = DEF_IMPLEMENTATION;  
    order_d = DEF_ORDER;
    particles_d = DEF_PARTICLES;

    // clear the matrix parameter
    // initial state 
    //
    state_mem_d.clear(ctype_a);

    // clear the matrix parameter
    // transition model and observation model
    //
    trn_d.clear(ctype_a);
    obs_d.clear(ctype_a);

    // clear the matrix parameter
    // state sequence which is only order length and observation sequence
    // filtered data only for that frame
    //
    obsn_d.clear(ctype_a);
    data_filt_d.clear(ctype_a);
  }
  
  // exit gracefully
  //
  return AlgorithmBase::clear(ctype_a);
}

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

// method: assign
//
// arguments:
//  const AlgorithmBase& arg: (input) object to be assigned
//
// return: a boolean value indicating status
//
// this method assigns the input algorithm object to the current object.
//
boolean ParticleFilter::assign(const AlgorithmBase& arg_a) {

  // case: input is an ParticleFilter object
  //
  if (typeid(arg_a) == typeid(ParticleFilter)) {
    return assign((ParticleFilter&)arg_a);
  }

  // case: other
  //  if the input algorithm object is not an ParticleFilter object, error.
  //
  else {
    return Error::handle(name(), L"assign", Error::ARG, __FILE__, __LINE__);
  }
}

// method: eq
//
// arguments:
//  const AlgorithmBase& arg: (input) object to be compared
//
// return: a boolean value indicating status
//
// this method checks whether the current ParticleFilter object is identical
// to the input algorithm object.
//
boolean ParticleFilter::eq(const AlgorithmBase& arg_a) const {

  // case: input is an ParticleFilter object
  //
  if (typeid(arg_a) == typeid(ParticleFilter)) {
    return eq((ParticleFilter&)arg_a);
  }

  // case: other
  //  if the input algorithm object is not an ParticleFilter object, error.
  //
  else {
    return Error::handle(name(), L"eq", Error::ARG, __FILE__, __LINE__);
  }
}

// method: init
//
// arguments: none
//
// return: a boolean value indicating status
//
// this method precomputes various constants used in this class
// 
boolean ParticleFilter::init() {

  if ((int)order_d > 0) {
    return Error::handle(name(), L"init", ERR_ORDER, __FILE__, __LINE__);
  }
  else {
    if (covar_nse_pro_d.le(0) ||
	covar_nse_obs_d.le(0) ||
	covar_err_est_d.le(0)) {
      return Error::handle(name(), L"init", ERR_INCPAR, __FILE__, __LINE__);
    }

    // check algorithm: CONVENTIONAL and implementation: AR_SCALAR
    // for CONVENTIONAL and AR_SCALAR -
    // initialize the transition and observation model
    // set the dimensions for states_d, state_mem_d, weights_d
    //
    if ((algorithm_d == CONVENTIONAL) && (implementation_d == AR_SCALAR)) {

      int i = 0, j = 0;

      // We will be implementing modified Yule-Walker LPC calculation
      // algorithm. Hene, initialization is needed
      // transition model will have order_d, order_d dimension
      // process noise covariance will have order_d, 1 dimension
      // observation model will have 1, order_d dimension
      // observation noise covariance will have 1, 1 dimension
      // estimate error covariance will have order_d, order_d dimension
      // state will have order_d, 1 dimension
      //
      // except for the last row,
      // first column is all zeros, rest upper right is identity matrix of
      // (order_d - 1, order_d - 1)
      // bottom row: LPC coefficients
      // right now bottom row will be initialized to 1
      //  |   |           |
      //  | 0 |     I     |
      //  |---+-----------|
      //  |ap   ap-1 .. a1|
      //
      trn_d.setDimensions(order_d, order_d);

      // first column will have all zeros
      //
      for (i = 0; i < int(order_d) - 1; i++) {
	trn_d.setValue(i, j, 0.0);
      }

      // remaining elements are identity matrix
      //
      for (i = 0; i < order_d; i++) {
	for (j = 1; j < order_d; j++) {
	  if (j == i + 1) {
	    trn_d.setValue(i, j, 1.0);
	  }
	  else {
	    trn_d.setValue(i, j, 0.0);
	  }
	}
      }

      i = (int)(order_d) - 1;
    
      // last row will all be set to 1
      // afterwards will be store the lpc coefficients
      //
      for (j = 0; j < order_d; j++) {
	trn_d.setValue(i, j, 1.0);
      }
   
      // observation model has its last column value 1.0
      //
      obs_d.setLength((int)order_d);
      for (j = 0; j < (int)order_d - 1; j++) {
	obs_d(j) = 0.0;
      }
      obs_d(j) =  1.0;
    
      // set dimensions for the state accepted for the first time
      //
      state_mem_d.setLength(order_d);

      return true;
    }
    else {
      if (algorithm_d == UNSCENTED) {
	return Error::handle(name(), L"init", ERR_UNSUPA,
			     __FILE__, __LINE__);
      }
      else
	return Error::handle(name(), L"init", ERR_UNKALG,
			     __FILE__, __LINE__);      
    }
  }
}

//-----------------------------------------------------------------------------
//
// we define non-integral constants in the default constructor
//      
//-----------------------------------------------------------------------------

// constants: class name
//
const String ParticleFilter::CLASS_NAME(L"ParticleFilter");

// constants: i/o related constants
//
const String ParticleFilter::DEF_PARAM(L"");
const String ParticleFilter::PARAM_ALGORITHM(L"algorithm");
const String ParticleFilter::PARAM_IMPLEMENTATION(L"implementation");
const String ParticleFilter::PARAM_ORDER(L"order");
const String ParticleFilter::PARAM_PARTICLES(L"particles");
const String ParticleFilter::PARAM_NOISE_PRO(L"process_noise");
const String ParticleFilter::PARAM_NOISE_OBS(L"obsn_noise");
const String ParticleFilter::PARAM_NOISE_ERR(L"err_estimate");

const VectorFloat ParticleFilter::DEF_STATE_MEM(L"-1");
const MatrixFloat ParticleFilter::DEF_TRN(1, 1, L"0", Integral::FULL);
const VectorFloat ParticleFilter::DEF_OBS_D(L"-1");

// constants: name map(s) for the enumerated values
//
const NameMap ParticleFilter::ALGO_MAP(L"CONVENTIONAL, UNSCENTED");
const NameMap ParticleFilter::IMPL_MAP(L"AR_SCALAR, AR_VECTOR");

// static instantiations: memory manager
//
MemoryManager ParticleFilter::mgr_d(sizeof(ParticleFilter),
				    ParticleFilter::name());
