// file: $isip_ifc/class/algo/ParticleFilter/pf_05.cc
// version: $Id: pf_05.cc 10377 2006-01-23 20:05:09Z patil $
//

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

boolean ParticleFilter::apply(Vector<AlgorithmData>& output_a,
		     const Vector<CircularBuffer<AlgorithmData> >& input_a) {

  long len = input_a.length();
  output_a.setLength(len);
  boolean res = true;

  displayStart(this);

  for ( long c = 0; c < len; c++) {

    displayChannel(c);
    
    if (input_a(c)(0).getDataType() == AlgorithmData::VECTOR_FLOAT) {
      
      res &= compute(output_a(c).makeVectorFloat(),
		     input_a(c)(0).getVectorFloat(),
		     input_a(c)(0).getCoefType(),
		     c);
    }

    output_a(c).setCoefType(AlgorithmData::SIGNAL);
    
  }
  
  displayFinish(this);

  return res;
}

boolean ParticleFilter::compute(VectorFloat& output_a,
				const VectorFloat& input_a,
				AlgorithmData::COEF_TYPE input_coef_type_a,
				long channel_index_a) {
  
  boolean status = false;
  
//   if (!is_valid_d) {
//        init();
//   }
  
  Long frame_nsamp = input_a.length();
  
  obsn_d = input_a;
  
  covar_nse_pro_d.sqrt();
  covar_nse_obs_d.sqrt();
  frame_index_d++;
  Long frame_count = (long)frame_index_d;
  frame_count.debug(L"incremented frame count L58, pf_05.cc");
  status = computeStates(input_a);
  output_a.concat(data_filt_d);
  data_filt_d.clear();
  
  display(output_a, input_a, name());
  
  return status;
}

// method: computeStates
//
// main method which will branch to other methods
//
boolean ParticleFilter::computeStates(const VectorFloat& input_a) {
  
  boolean status = true;
  // long time_temp = (int)obsn_d.length();
  long time_temp = (int)input_a.length();

  // change the name of the variable afterwards
  // remove _d from the variable name
  //
  MatrixFloat states_d;
  VectorFloat weights_d((int)particles_d);
  states_d.setDimensions(order_d, particles_d);
  
  if (time_temp > 0) {

    VectorFloat lpc((int)order_d);
    computeLPC(lpc);
    initializeTransitionModel(lpc);
    VectorFloat current_value;
    Float average;

    if (frame_index_d == 0) {

      // you should assign the length in init() method
      // it is AOK for the time being
      //
      state_mem_d.setLength(order_d);
      for ( int i = 0; i < order_d; i++) {
	float j = input_a(i);
	state_mem_d(i) = j;
	data_filt_d(i) = j;
      }
  
      // initializeState();

      // generating particles
      //
      VectorFloat particle(particles_d);
      
      for ( int i = 0; i < (int) order_d; i++) {
	float j = 0;
	j = state_mem_d(i);
	particle.grand(j, (float)covar_nse_pro_d);
	states_d.setRow(i, particle);
      }
      
//       for ( int i = 0; i < (int)order_d; i++) {
// 	data_filt_d(i) = obsn_d(i);
//       }
      
      for (int index = (long)order_d; index < time_temp; index++) {

	// float obsn = obsn_d(index);
	float obsn = input_a(index);
	
	computePredictStates(states_d);
	computeImportanceWeights(weights_d, states_d,
				 obsn);
	computeUpdateStates(states_d, weights_d);
	
	Console::put(L"pf_05.cc line 115");	
	states_d.getRow(current_value, index);
	average = current_value.mean();
	average.debug(L"state at time instant : ");
	Console::put(L"pf_05.cc line 119");
	data_filt_d.debug(L"filtered value :");
	data_filt_d(index) = (float)average;
	data_filt_d.debug(L"after assignment ?");
	Console::put(L"pf_05.cc line 123");
      }
    }
    else if(frame_index_d > 0) {

      for (int index = 0; index < time_temp; index++) {

	// float obsn = obsn_d(index);
	float obsn = input_a(index);

	computePredictStates(states_d);
	computeImportanceWeights(weights_d, states_d, obsn);
	computeUpdateStates(states_d, weights_d);
	
	Console::put(L"pf_05.cc line132");     
	
	states_d.getRow(current_value, index);
	average = current_value.mean();
	average.debug(L"state at time instant : ");
	Console::put(L"pf_05.cc line 139");
	data_filt_d.debug(L"filtered value :");
	data_filt_d(index) = (float)average;
	data_filt_d.debug(L"after assignment ?");
	Console::put(L"pf_05.cc line 143");
      }
    }
  }

  return status;
}

// method: computePredictStates
// for each particle index, pick up  the states,
// get the next state by using the transition model and process 
//
boolean ParticleFilter::computePredictStates(MatrixFloat& states_d) {
  
  VectorFloat process_noise((int)particles_d);
  process_noise.grand(0, (float)covar_nse_pro_d);
  MatrixFloat noise((int)order_d, (int)particles_d);
  
  // setting up the last row as the update element row
  //
  noise.setRow((int)order_d - 1, process_noise);
  
  VectorFloat tmp;
  VectorFloat x_temp, x_temp1;

  for ( int i = 0; i < (int)particles_d; i++) {
    states_d.getColumn(tmp, i);
    noise.getColumn(x_temp1, i);
    trn_d.multv(x_temp, tmp);
    x_temp.add(x_temp1);
    states_d.setColumn(i, x_temp);
  }
  return true;
}

// method: computeImportanceWeights
//
// to get the importance weights for the filtered state if the weights
// are negligible (i.e, the sum of the weights is very small) then,
// adjust the weights to have equal value, value equal to 1 by number
// of particles
//
boolean ParticleFilter::computeImportanceWeights(VectorFloat& weights_d,
						 MatrixFloat& states_d,
						 float obsn_a) {

  Float m_value;
  float m, sum;
  
  for (int i = 0; i < particles_d; i++) {

    // last row of the state is the estimated observation i.e,
    // filtered speech signal
    //
    states_d.getValue(m_value, 3, i);
    m = (float)m_value;
    weights_d(i) =
             exp(-0.5 * (obsn_a - m) * (obsn_a - m) / covar_nse_obs_d);
  }

  sum = weights_d.sum();
  if (sum < 0.05)
    for ( int j = 0; j < particles_d; j++) 
      weights_d(j) = 1 / (float)particles_d;
    else weights_d.div(sum); 
    
  return true;
}

// method: computeUpdateStates
//
boolean ParticleFilter::computeUpdateStates(MatrixFloat& states_d,
					    VectorFloat& weights_d) {
//  int i, j;
//   VectorFloat cum_weights((int)particles_d);
//   cum_weights(0) = weights_d(0);
//   for (i = 1; i < (int)particles_d; i++) {
//     cum_weights(i) = weights_d(i) + cum_weights(i - 1);
//   }
//   // this might be a trouble spot for us...
//   // it is different from that implemented in pf_sundar/ folder under
//   // asgmts/
//   //
//   Float uniform;
//   uniform.rand();
//   Float cum_sum;
//   i = 0;
//   uniform /= (float)particles_d;
//   cum_sum = uniform;
//   for (j = 1; j < (int) particles_d; j++) {
//     cum_sum += 1/(float)particles_d;
//     Float assign_data;
//     while  (cum_sum > cum_weights(i))  {
//       i++;
//     }
//     states_d.getValue(assign_data, 3, i);
//     states_d.setValue(3, j, assign_data);
//   }
//   Console::put(L"line 261 pf_05.cc"); 

  VectorFloat cum_uni_wts((int)particles_d + 1);
    Float uniform;
    uniform.rand();
    cum_uni_wts(0) = -1 * uniform.log10();
    
   for ( int i = 0; i < (int)particles_d; i++) {
     uniform.rand();
     cum_uni_wts(i+1) = -1 * uniform.log10() + cum_uni_wts(i);
   }

   VectorFloat temp_state((int)particles_d);
   VectorFloat cum_wts((int)particles_d);
   cum_wts(0) = weights_d(0);
   temp_state(0) = 0.0;
   
   for (int i = 1; i < (int)particles_d; i++) {
     temp_state(i) = 0.0;
     cum_wts(i) = cum_wts(i-1) + weights_d(i-1);
   }

   int j = 0;
   int k = 0;
   while(j < particles_d) {
     if (((float)cum_wts(j) * (float)cum_uni_wts((int)particles_d -1)) >
	 (float)cum_uni_wts(k)) {
       temp_state(k) = (float)states_d.getValue((int)order_d - 1, j);
       k++;
     }
     else { j++;}
   }

   for (int i = 0; i < (int)particles_d; i++)
     states_d.setValue((int)order_d - 1, i, (float)temp_state(i));
       
  return true;
}

// method: initializeState
//
// used with first frame only
//
boolean ParticleFilter::initializeState() {

  // accept the initial 'order' values of observations (input)
  // just assign it to state_mem_d variable
  //
  state_mem_d.setLength(order_d);
  for ( int i = 0; i < order_d; i++) {
    float j = obsn_d(i);
    state_mem_d(i) = j;
  }
  
  return true;
}

// method: computeLPC
//
// calculate the LP coefficients which will be used within the
// transition model matrix (last row of the matrix)
//
boolean ParticleFilter::computeLPC(VectorFloat& lpc_a) {

  // 1. first compute the autocorrelation coefficients
  // 2. then compute the LP coefficients
  //
  int i, j;
  int frame_length = (int) obsn_d.length();
  VectorFloat autoc((int)order_d + 1);
  for (i = 0; i < (int)order_d + 1; i++) {
    autoc(i) = 0;
    for (j = 0; j < frame_length - i; j++) {
      autoc(i) = autoc(i) + obsn_d(j) * obsn_d(j + i);
    }
  }

  for ( i = order_d; i >= 0; i--) {
    autoc(i) = autoc(i) / autoc(0);
  }

  Prediction lp(Prediction::AUTOCORRELATION,
		Prediction::DURBIN, order_d, -60);
  lp.compute(lpc_a, autoc);
  lpc_a.debug(L"LP coefficients:");
  return true;
  
}

// method: initializeTransitionModel
// set the values of lp coefficients as part of transition model
//
boolean ParticleFilter::initializeTransitionModel(VectorFloat lpc_a) {

  // set up the parameters for the calculation
  // transition model will have the last row as lp coefficients
  //
  float lpc_value = 0;
  int j;
  for ( j = 0; j < order_d; j++) {
    lpc_value = (float)lpc_a(j + 1);
    trn_d.setValue((int)order_d - 1, j, lpc_value);
  }

  // trn_d.debug(L"transition model : ");
  return true;
}


