// file: $isip/class/algo/Filter/filt_05.cc
// version: $Id: filt_05.cc 8376 2002-07-14 16:48:44Z gao $
//

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

// method: apply
//
// arguments:
//  Vector<AlgorithmData>& output: (output) output data
//  const Vector< CircularBuffer<AlgorithmData> >& input: (input) input data
//
// return: a boolean value indicating status
//
// this method calls the appropriate computation methods
//
boolean Filter::apply(Vector<AlgorithmData>& output_a,
		      const Vector< CircularBuffer<AlgorithmData> >& input_a) {
  
  // for ACCUMULATE mode, error
  //
  if (cmode_d == ACCUMULATE) {
    return Error::handle(name(), L"apply",
			 ERR_UNSUPM, __FILE__, __LINE__);
  }

  // adding one to indicator means we process one frame
  //
  frame_index_d++;  

  // for SAMPLE_BASED mode, error
  //
  if (dmode_d == SAMPLE_BASED) {
    return Error::handle(name(), L"apply",
			 ERR_UNSUPM, __FILE__, __LINE__);
  }
   
  // set the length to the number of channels input
  //
  long len = input_a.length();
  output_a.setLength(len);

  // set the number of channels
  //
  if (len != num_channels_d) {
    setNumChannels(len);
  }

  // accumulate a result status -- if compute method returns false
  // this apply method should return false
  //  
  boolean res = true;
  
  // start the debugging output
  //
  displayStart(this);

  // loop over the channels and call the compute method
  //
  for (long c = 0; c < len; c++) {

    // display the channel number
    //
    displayChannel(c);

    // call AlgorithmData::makeVectorFloat to force the output vector for
    // this channel to be a VectorFloat, use the CircularBuffer<AlgorithmData>
    // directly on the input for this channel
    //          
    res &= computeBuffer(output_a(c).makeVectorFloat(),
			 input_a(c),
			 input_a(c)(0).getCoefType(),
			 c);
    
    // set the coef type for AlgorithmData output
    //
    output_a(c).setCoefType(input_a(c)(0).getCoefType());
  }

  // finish the debugging output
  //
  displayFinish(this);

  // exit gracefully
  //
  return res;
}

// method: compute
// 
// arguments:
//  VectorFloat& output: (output) filtered signal
//  const VectorFloat& input: (input) signal to be filtered
//  AlgorithmData::COEF_TYPE coef_type: (input) coeff type of input signal
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method selects the appropriate private computational method
//
boolean Filter::compute(VectorFloat& output_a, const VectorFloat& input_a,
			AlgorithmData::COEF_TYPE coef_type_a,
			long channel_index_a) {

  // declare local variable
  //
  boolean status = false;

  // check whether we need to initialize
  //
  if (!is_valid_d) {
    init();
  }

  // algorithm: LTI
  //
  if (algorithm_d == LTI) {

    // implementation: CCDE (Constant Coefficient Difference Equation)
    //
    if (implementation_d == CCDE) {
      if (cmode_d == FRAME_INTERNAL) {
	status = computeLtiCcdeFrameInternal(output_a, input_a,
					     channel_index_a);
      }

      else {
	status = computeLtiCcdeCrossFrame(output_a, input_a, channel_index_a);
      }
    }
    
    // error: unknown implementation
    //
    else {
      return Error::handle(name(), L"compute", ERR_UNKIMP,
			   __FILE__, __LINE__);
    }
  }
  
  // error: unknown algorithm
  //
  else {
    status = Error::handle(name(), L"compute", ERR_UNKALG,
			   __FILE__, __LINE__);
  }

  // possibly display the data
  //
  display(output_a, input_a, name());
    
  // exit gracefully
  //
  return status;
}

// method: compute
// 
// arguments:
//  VectorComplexFloat& output: (output) filtered signal
//  const VectorComplexFloat& input: (input) signal to be filtered
//  AlgorithmData::COEF_TYPE coef_type: (input) coeff type of input signal
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method selects the appropriate computational method
//
boolean Filter::compute(VectorComplexFloat& output_a,
			const VectorComplexFloat& input_a,
			AlgorithmData::COEF_TYPE coef_type_a,
			long channel_index_a) {

  return Error::handle(name(), L"compute", Error::NOT_IMPLEM,
		       __FILE__, __LINE__);
}

// method: computeLtiCcdeFrameInternal
// 
// arguments:
//  VectorFloat& output: (output) filtered signal
//  const VectorFloat& input: (input) signal to be filtered
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method has filter implemented in Direct Form 1. The past and
// future samples outside the input buffer are considered zero.
//
boolean Filter::computeLtiCcdeFrameInternal(VectorFloat& output_a,
					    const VectorFloat& input_a,
					    long channel_index_a) {
  
  // check whether the channel index is valid or not
  //
  if (channel_index_a >= num_channels_d) {
    return Error::handle(name(), L"computeLtiCcdeFrameInternal", Error::ARG,
			 __FILE__, __LINE__);
  }
  
  // check whether the filter is valid or not
  //
  if (!is_valid_d) {
    init();
  }
  
  // allocate memory for output signal
  //
  long input_len = input_a.length();

  long ma_max = ma_lag_d.max();
  long len = input_len - ma_max;

  // at least one element exist in current frame beside future samples
  // to process
  //
  if (len < 1) {
    return Error::handle(name(), L"computeLtiCcdeFrameInternal", ERR_DATA,
			 __FILE__, __LINE__);
  }
  
  output_a.setLength(len);

  // loop through the input signal
  //
  for (long index = 0; index < len; index++) {

    // fetch the future samples if any and if the filter is anti-causal
    //
    for (long i = ma_lag_d.length() - (long)1; i >= 0; i--) {
      if ((ma_lag_d(i) > (long)0) && ((index + ma_lag_d(i)) < input_len)) {
	ma_mem_d(channel_index_a).
	  advanceAndAssign(input_a(index + ma_lag_d(i)),
			   (ma_lag_d(i) + (long)1));
	
	// reverse the pointer position from future to present
	//
	ma_mem_d(channel_index_a).advance(-ma_lag_d(i) - (long)1);
      }
    }
    
    // advance the read pointer and append the input sample
    //
    ma_mem_d(channel_index_a).advanceAndAssign(input_a(index));
	  
    // declare local variables
    //
    Float sum = 0.0;
    
    // loop through the moving average coefficients
    //
    for (long i = ma_lag_d.length() - (long)1; i >= 0; i--) {

      // limit the input sample to future samples, prevent the
      // circular buffer from fetching the past sample when the
      // pointer makes a complete circle through the buffer
      //
      if (((index + ma_lag_d(i)) < input_len) &&
	  (ma_lag_d(i) + index >= 0)) {
	sum += (float)ma_coef_d(i) *
	  (float)ma_mem_d(channel_index_a)((long)ma_lag_d(i));
      }
    }

    // loop through the auto regressive coefficients only if the
    // filter is regressive (IIR)
    //
    for (long i = ar_lag_d.length() - (long)1; i >= 0; i--) {
      if (ar_lag_d(i) + index >= 0)
	sum -= (float)ar_coef_d(i) *
	  (float)ar_mem_d(channel_index_a)((long)ar_lag_d(i));
    }
    
    // advance the ar memory and save the output
    //
    ar_mem_d(channel_index_a).assignAndAdvance(sum);
    output_a(index) = sum;
  }

  // exit gracefully
  //
  return true;
}

// method: computeBuffer
//
// arguments:
//  VectorFloat& output: (output) output buffer of frames
//  const CircularBuffer<AlgorithmData>& input: (input) circular buffer
//  AlgorithmData::COEF_TYPE coef_type: (input) coeff type of input signal
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method transfers the input circular buffer data into one data vector
// with necessary leading or trailing pad.
//
boolean Filter::computeBuffer(VectorFloat& output_a,
			      const CircularBuffer<AlgorithmData>& input_a,
			      AlgorithmData::COEF_TYPE coef_type_a,
			      long channel_index_a) {  
  
  // make sure there is enough data
  //
  if (input_a.getNumForward() < getLeadingPad()) {
    return Error::handle(name(), L"computeBuffer",ERR_INSUFD, __FILE__,
			 __LINE__);
  }
  
  // compute the number of samples per frame using the current frame
  // in the circular buffer since it always contains one frame of data
  //
  long frame_nsamp = input_a(0).getVectorFloat().length();

  // get the number of future samples and the number of frames
  //
  long future_nsamp = getLeadingNumSamp();
  long future_nframe = getLeadingPad();
  
  if (future_nsamp > ma_mem_d(0).length()) {
    return Error::handle(name(), L"computeBuffer", ERR_INSUFD, __FILE__,
			 __LINE__);
  }
  
  // declare local variables
  //
  long future_offset_num = future_nsamp % (long)frame_nsamp;
  long pad_pos = frame_nsamp;

  // form a vector for that has required future samples for all the
  // samples in the current frame
  //
  VectorFloat buf;

  buf.assign(input_a(0).getVectorFloat());

  if (cmode_d == CROSS_FRAME) {
  
    // if future partial frames are present, copy the future frames,
    // the future partial frame
    //
    if ((future_offset_num > (long)0)) {
      
      // copy the future frames
      //
      for (long i = 1 ; i < future_nframe; i++) {          
	buf.concat(input_a(i).getVectorFloat());
	
	// increase the padding position
	//
	pad_pos += (long)frame_nsamp;
      }
      
      // copy the partial future frame
      //
      buf.move(input_a(future_nframe).getVectorFloat(),
	       future_offset_num, (long)0, pad_pos);
    }
    
    // if no future partial frame is present, copy the full future frames
    //
    if (future_offset_num == (long)0) {
      
      // copy the past frames, current frame and future frames
      //
      for (long i = 1 ; i <= future_nframe; i++) {          
	buf.concat(input_a(i).getVectorFloat());
      
	// increase the padding position
	//
	pad_pos += (long)frame_nsamp;
      }
    }
  }
  
  boolean status;
  
  status = compute(output_a, buf, coef_type_a, channel_index_a);  

  // exit gracefully
  //
  return status;
}



// method: computeLtiCcdeCrossFrame
// 
// arguments:
//  VectorFloat& output: (output) filtered signal
//  const VectorFloat& input: (input) signal to be filtered
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method has filter implemented in Direct Form 1. The past and
// future samples outside the current frame input buffer are used.
//
boolean Filter::computeLtiCcdeCrossFrame(VectorFloat& output_a,
					 const VectorFloat& input_a,
					 long channel_index_a) {
  
  // check whether the channel index is valid or not
  //
  if (channel_index_a >= num_channels_d) {
    return Error::handle(name(), L"computeLtiCcdeCrossFrame", Error::ARG,
			 __FILE__, __LINE__);
  }
  
  // check whether the filter is valid or not
  //
  if (!is_valid_d) {
    init();
  }
  
  // allocate memory for output signal
  //
  long input_len = input_a.length();

  long ma_max = ma_lag_d.max();
  long len = input_len - ma_max;

  // at least one element exist in current frame beside future samples
  // to process
  //
  if (len < 1) {
    return Error::handle(name(), L"computeLtiCcdeCrossFrame", ERR_DATA,
			 __FILE__, __LINE__);
  }
  
  output_a.setLength(len);

  // loop through the input signal
  //
  for (long index = 0; index < len; index++) {

    // fetch the future samples if any and if the filter is anti-causal
    //
    for (long i = ma_lag_d.length() - (long)1; i >= 0; i--) {
      if ((ma_lag_d(i) > (long)0) && ((index + ma_lag_d(i)) < input_len)) {
	ma_mem_d(channel_index_a).
	  advanceAndAssign(input_a(index + ma_lag_d(i)),
			   (ma_lag_d(i) + (long)1));
	
	// reverse the pointer position from future to present
	//
	ma_mem_d(channel_index_a).advance(-ma_lag_d(i) - (long)1);
      }
    }
    
    // advance the read pointer and append the input sample
    //
    ma_mem_d(channel_index_a).advanceAndAssign(input_a(index));
	  
    // declare local variables
    //
    Float sum = 0.0;
    
    // loop through the moving average coefficients
    //
    for (long i = ma_lag_d.length() - (long)1; i >= 0; i--) {

      sum += (float)ma_coef_d(i) *
	(float)ma_mem_d(channel_index_a)((long)ma_lag_d(i));

    }

    // loop through the auto regressive coefficients only if the
    // filter is regressive (IIR)
    //
    for (long i = ar_lag_d.length() - (long)1; i >= 0; i--) {
      sum -= (float)ar_coef_d(i) *
	(float)ar_mem_d(channel_index_a)((long)ar_lag_d(i));
    }
    
    // advance the ar memory and save the output
    //
    ar_mem_d(channel_index_a).assignAndAdvance(sum);
    output_a(index) = sum;
  }

  // exit gracefully
  //
  return true;
}
