// file: $isip_ifc/class/algo/KalmanFilter/kf_00.cc
// version: $Id: kf_00.cc 10602 2006-08-17 17:43:12Z ss754 $
//

// isip include files
//
#include "KalmanFilter.h"
#include <typeinfo>

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

// method: assign
//
// arguments:
//  const KalmanFilter& arg: (input) object to be assigned
//
// return: a boolean value indicating status
//
// this method assigns the input object to the current object
//
boolean KalmanFilter::assign(const KalmanFilter& 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;
  
  // 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 process noise covariance,
  // observation noise covariance and estimate error
  // covariance
  //
  covar_nse_pro_d.assign(arg_a.covar_nse_pro_d);
  covar_nse_obs_d = arg_a.covar_nse_obs_d;
  covar_err_est_d.assign(arg_a.covar_err_est_d);

  // assign AlgorithmBase and exit gracefully
  //
  return AlgorithmBase::assign(arg_a);
}

// method: eq
//
// arguments:
//  const KalmanFilter& 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 KalmanFilter::eq(const KalmanFilter& 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) ||
      (order_d != arg_a.order_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.almostEqual(arg_a.covar_nse_pro_d)) ||
      (!covar_nse_obs_d.almostEqual(arg_a.covar_nse_obs_d)) ||
      (!covar_err_est_d.almostEqual(arg_a.covar_err_est_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 KalmanFilter::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;

    // 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
    // process noise covariance, observation noise covariance,
    // and estimate error covariance
    //
    covar_nse_pro_d.clear(ctype_a);
    covar_nse_obs_d = 0.0;
    covar_err_est_d.clear(ctype_a);

    // clear the matrix parameter
    // state sequence which is only order length and observation sequence
    // filtered data only for that frame
    //
    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 KalmanFilter::assign(const AlgorithmBase& arg_a) {

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

  // case: other
  //  if the input algorithm object is not an KalmanFilter 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 KalmanFilter object is identical
// to the input algorithm object.
//
boolean KalmanFilter::eq(const AlgorithmBase& arg_a) const {

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

  // case: other
  //  if the input algorithm object is not an KalmanFilter 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 and initializes various constants used in
// this class. method called during the first frame
// 
boolean KalmanFilter::init() {
  
  // check implementation: AR_SCALAR
  //
  if (implementation_d == AR_SCALAR) {
    
    int i = 0, j = 0;
    // transition model will be (order_d, order_d) dimension
    // process noise covariance will be (order_d, order_d) dimension
    // observation model will be (1, order_d) dimension
    // observation noise covariance will be (1, 1) dimension
    // estimate error covariance will be (order_d, order_d) dimension
    // state will be (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|
    // 
    if(trn_d.getNumRows() != (int)order_d) {
      trn_d.setDimensions((int)order_d, (int)order_d);
      
      // first column will be 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 < (int)order_d; i++) {
	for (j = 1; j < (int)order_d; j++) {
	  if (j == i + 1) {
	    trn_d.setValue(i, j, 1.0);
	  }
	  else {
	    trn_d.setValue(i, j, 0.0);
	  }
	}
      }
      
      i--;
      
      // last row will all be set to 1
      // afterwards will store the lpc coefficients
      //
      for (j = 0; j < (int)order_d; j++) {
	trn_d.setValue(i, j, -1.0);
      }
    }
    
    if (obs_d.length() != (int)order_d) {
      // 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;
    }
    
    if (algorithm_d == CONVENTIONAL) {
      if ((covar_nse_pro_d.getNumRows() != (int)order_d) ||
	  (covar_nse_obs_d == -1.0) ||
	  (covar_err_est_d.getNumRows() != (int)order_d)) {
	// actually, the error should reflect that noise parameters
	// not initialized
	//
	return Error::handle(name(), L"init",
			     ERR_MISDIM, __FILE__, __LINE__);
      }
      else { is_valid_d = true; }
    }
    
    if (algorithm_d == UNSCENTED) {
      if ((covar_nse_pro_d.getNumRows() != 1) ||
	  (covar_nse_obs_d == -1.0) ||
	  (covar_err_est_d.getNumRows() != ((int)order_d + 2))) {
	// actually, the error should reflect that noise parameters
	// not initialized
	//
	return Error::handle(name(), L"init",
			     ERR_MISDIM, __FILE__, __LINE__);
      }
      else if (alpha_d.le(0.0) || beta_d.le(0.0) || kappa_d.le(0.0)) {
	// default parameters not initialized
	//
	return Error::handle(name(), L"init",
			     ERR_UNIPAR, __FILE__, __LINE__);
      }
      else { is_valid_d = true; }
    }
  }
  else {
    return Error::handle(name(), L"init", ERR_UNKIMP, __FILE__, __LINE__);
  }
  
  // exit gracefully
  //
  return true;
}

// method: setOrder
//
boolean KalmanFilter::setOrder(long order) {
  order_d = order;
  covar_nse_pro_d.setDimensions(order, order);
  covar_err_est_d.setDimensions(order, order);
    
  is_valid_d = false;
  return true;
}



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

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

// constants: i/o related constants
//
const String KalmanFilter::DEF_PARAM(L"");
const String KalmanFilter::PARAM_ALGORITHM(L"algorithm");
const String KalmanFilter::PARAM_IMPLEMENTATION(L"implementation");
const String KalmanFilter::PARAM_ORDER(L"order");
const String KalmanFilter::PARAM_STATE_MEM(L"state_mem");
const String KalmanFilter::PARAM_TRN(L"trn");
const String KalmanFilter::PARAM_OBS(L"obs");
const String KalmanFilter::PARAM_NOISE_PRC(L"process_noise");
const String KalmanFilter::PARAM_NOISE_OBS(L"obsn_noise");
const String KalmanFilter::PARAM_NOISE_ERR(L"estimate_error");
const String KalmanFilter::PARAM_ALPHA(L"alpha");
const String KalmanFilter::PARAM_BETA(L"beta");
const String KalmanFilter::PARAM_KAPPA(L"kappa");

// constants: default values of the class data
//
const VectorDouble KalmanFilter::DEF_STATE_MEM(L"-1");
const VectorDouble KalmanFilter::DEF_OBS(L"0");
const MatrixDouble KalmanFilter::DEF_TRN(1, 1, L"0", Integral::FULL);

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

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