// file: $isip/class/algo/FilterBank/fb_05.cc
// version: $Id: fb_05.cc 9258 2003-07-02 20:44:59Z parihar $
//

// isip include files
//
#include "FilterBank.h"
#include <Bark.h>
#include <Mel.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 FilterBank::apply(Vector<AlgorithmData>& output_a,
			  const Vector< CircularBuffer<AlgorithmData> >&
			  input_a) {

  // for ACCUMULATE mode, error since algorithm TIME operates in
  // FRAME_INTERNAL and CROSS_FRAME and algorithm FREQUENCY in
  // FRAME_INTERNAL mode
  //
  if (cmode_d == ACCUMULATE) {
    return Error::handle(name(), L"apply",
                         ERR_UNSUPM, __FILE__, __LINE__);
  }
  
  // for SAMPLE_BASED mode, error since both the TIME and FREQUENCY
  // algorithms operate in FRAME_BASED cmode
  //
  if (dmode_d == SAMPLE_BASED) {
    return Error::handle(name(), L"apply",
                         ERR_UNSUPM, __FILE__, __LINE__);
  }
  
  // determine the number of input channels
  //
  long len = input_a.length();
  boolean res = true;

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

  // determine the number of output channels
  //  algorithm: TIME
  //
  if (algorithm_d == TIME) {
    output_a.setLength(len * filters_d.length());
  }
  
  // algorithm: FREQUENCY
  //
  else {
    output_a.setLength(len);
  }
  
  // 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);
    
    // algorithm: TIME
    //
    if (algorithm_d == TIME) {
	  
      // input data type: VECTOR_FLOAT
      //
      if (input_a(c)(0).getDataType() == AlgorithmData::VECTOR_FLOAT) {

	// set the temporary output
	//
	Vector<VectorFloat> temp_output;
	temp_output.setLength(filters_d.length());

	// use the CircularBuffer<AlgorithmData> directly on the sigle
	// channel input and directly on the multiple channel output
	//    
	res &= computeTimeCommon(temp_output, input_a(c),
				 input_a(c)(0).getCoefType(),
				 c);

	// convert the Vector<VecorFloat> type to
	// Vector<AlgorithmData> type
	//
	for (long i = 0; i < filters_d.length(); i++) {
	  output_a(c * filters_d.length() + i).
	    makeVectorFloat() = temp_output(i);

	  // set the coef type for AlgorithmData output
	  //
	  output_a(c * filters_d.length() + i).
	    setCoefType(input_a(c)(0).getCoefType());
	}
      }
      
      // input data type: VECTOR_COMPLEX_FLOAT
      // else: error unknown implementation
      //
      else {
	return Error::handle(name(), L"apply", ERR_UNKIMP,
			       __FILE__, __LINE__);
      }      
    }
    
    // algorithm: FREQUENCY
    //
   else if (algorithm_d == FREQUENCY) {
     
     // call AlgorithmData::makeVector to force the output vector for
     // this channel to be a VectorFloat, call AlgorithmData::getVectorFloat
     // on the input for this channel to check that the input is
     // already a VectorFloat and return that vector.
     //
     res &= compute(output_a(c).makeVectorFloat(),
		    input_a(c)(0).getVectorFloat(),
		    input_a(c)(0).getCoefType(),
		    c);
     
     // set the coefficient type of output
     //
     output_a(c).setCoefType(AlgorithmData::SPECTRUM);
   }
    
    // else: unknown algorithm
    //
    else {
      return Error::handle(name(), L"apply", ERR_UNKALG,
			   __FILE__, __LINE__);
    }
  }
  
  // finish the debugging output
  //
  displayFinish(this);

  // exit gracefully
  //
  return res;  
}

// method: compute
//
// arguments:
//  Vector<VectorFloat>& output: (output) multichannel signals
//  const VectorFloat& input: (input) single-channel signal
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method applies multichannel filters on one single-channel
// signal to generate multichannel output signals
//
boolean FilterBank::compute(Vector<VectorFloat>& output_a,
			    const VectorFloat& input_a,
			    AlgorithmData::COEF_TYPE input_coef_type_a,
			    long channel_index_a) {
  
  // declare local variable
  //
  boolean status = true;

  // check whether we need to initialize
  //
  if (!is_valid_d) {
    init();
  }
  
  // algorithm: TIME
  //
  if (algorithm_d == TIME) {
    
    // implementation: CCDE
    //
    if (implementation_d == CCDE) {
      
      // call corresponding compute method
      //
      status = computeTimeCcde(output_a, input_a, channel_index_a);
    }
    
    // else: error unknown implementation
    //
    else {
      status = Error::handle(name(), L"compute", ERR_UNKIMP,
			     __FILE__, __LINE__);
    }
  }
  
  // else: 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:
//  VectorFloat& output: (output) output data vector
//  const VectorFloat& input: (input) input data vector
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  long channel_index: (input) channel index for the input data vector
//
// return: a boolean value indicating status
//
// this method computes the filter bank amplitude for given input
// vector for FREQUENCY algorithm
//
boolean FilterBank::compute(VectorFloat& output_a,
			    const VectorFloat& input_a,
			    AlgorithmData::COEF_TYPE input_coef_type_a,
			    long channel_index_a) {
  
  // declare local variable
  //
  boolean status = true;

  // algorithm: FREQUENCY
  //
  if (algorithm_d == FREQUENCY) {
    
    // implementation: UNIFORM
    //
    if (implementation_d == UNIFORM) {
      
      // call corresponding compute method
      //
      status = computeFrequencyUniform(output_a, input_a);
      
    }
    
    // implementation: TRIANGULAR
    //
    else if (implementation_d == TRIANGULAR) {
      
      // call corresponding compute method
      //
      status = computeFrequencyTriangular(output_a, input_a);
    }

    // implementation: RAISED_COSINE
    //
    else if (implementation_d == RAISED_COSINE) {
      
      // call corresponding compute method
      //
      status = computeFrequencyRaisedcosine(output_a, input_a);
    }
    
    // else: error unknown implementation
    //
    else {
      status = Error::handle(name(), L"compute", ERR_UNKIMP,
			     __FILE__, __LINE__);
    }
  }

  // else: 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: computeTimeCommon
//
// arguments:
//  VectorFloat<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 FilterBank::computeTimeCommon(Vector<VectorFloat>& output_a,
				      const CircularBuffer<AlgorithmData>&
				      input_a,
				      AlgorithmData::COEF_TYPE coef_type_a,
				      long channel_index_a) {  
  // check whether we need to initialize
  //
  if (!is_valid_d) {
    init();
  }

  // declare a return value
  //
  boolean status = true;
  
  // make sure there is enough data
  //
  if (input_a.getNumForward() < getLeadingPad()) {
    return Error::handle(name(), L"compute", 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();
  
  // form the buffer for each filter
  //
  long len = filters_d.length();
  
  output_a.setLength(len);
  
  long index = 0;
  
  for (long i = 0; i < len; i++) {
    
    // get the number of future samples and the number of frames
    //
    long future_nsamp = filters_d(i).getLeadingNumSamp();
    long future_nframe = filters_d(i).getLeadingPad();
    
    // 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());
    
    // declare local variables
    //
    long future_offset_num = future_nsamp % (long)frame_nsamp;
    long pad_pos = frame_nsamp;
    
    // if future partial frames are present, copy future frames, and
    // finally the future partial frame
    //
    if (future_offset_num > (long)0) {
      
      // copy the future frames
      //
      for (long j = 1 ; j < future_nframe; j++) {
	buf.concat(input_a(j).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 future frames
      //
      for (long j = 1; j <= future_nframe; j++) {          
	buf.concat(input_a(j).getVectorFloat());
	
	// increase the padding position
	//
	pad_pos += (long)frame_nsamp;
      }
    }
    
    VectorFloat temp_buf;
    
    // call the compute method of the Filter class
    //          
    status &= filters_d(i).compute(temp_buf, buf);

    // add each filter output to form a vector of multichannels
    //
    output_a(index).assign(temp_buf);
    index++;
  }

  // exit gracefully
  //
  return status;

}

// method: computeTimeCcde
//
// arguments:
//  Vector<VectorFloat>& output: (output) multichannel signals
//  const VectorFloat& input: (input) single-channel signal
//  long channel_index: (input) the channel index of input signal
//
// return: a boolean value indicating status
//
// this method applies multichannel filters on one single-channel
// signal to generate multichannel output signals.
//
boolean FilterBank::computeTimeCcde(Vector<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"computeTimeCcde",
			 ERR, __FILE__, __LINE__);
  }
  
  // check the number of filters
  //
  if (filters_d.length() < 1) {
    return Error::handle(name(), L"computeTimeCcde", ERR,
			 __FILE__, __LINE__);
  }
  
  // declare local variable
  //
  boolean status = true;
  long index = 0;
  VectorFloat tmp_output;
  
  // determine the number of output channels
  //
  output_a.setLength(filters_d.length(), false);
  
  // loop over the filters and generate multichannel output signals
  //
  for (long i = 0; i < filters_d.length(); i++) {

    // call the compute method of the Filter Class
    //          
    status &= filters_d(i).compute(tmp_output, input_a);

    // add each filter output to form a vector of multichannels
    //
    output_a(index).assign(tmp_output);
    index++;
  }

  // exit gracefully
  //
  return status;
}

// method: computeFrequencyCommon
//
// arguments:
//  VectorFloat& output: (output) output data vector
//  const VectorFloat& input: (input) input data vector
//
// return: a boolean value indicating status
//
// this method does all the preprocessing like computing scale
// (LINEAR, MEL, BARK) and central frequencies common to all the
// implementations (UNIFORM, TRIANGULAR, RAISED_COSINE) in FREQUENCY
// algorithm
//
boolean FilterBank::computeFrequencyCommon(VectorFloat& output_a,
					   const VectorFloat& input_a) {

  // FREQUENCY only operates in FRAME_INTERNAL mode
  //
  if (cmode_d == ACCUMULATE) {
    return Error::handle(name(), L"computeFrequencyCommon",
                         ERR_UNSUPM, __FILE__, __LINE__);
  }


  // local variables
  //
  long len = (long)0;
  
  // FBA expects a full spectrum as input, but only uses the first
  // half of it. use the complete input if the input mode is half
  // (SYMMETRIC)
  //
  if (input_mode_d == SYMMETRIC) {
    len = input_a.length();
  }
  
  else  {
    len = input_a.length() / 2;
  }
  
  long last_length = warp_freq_d.length();
  
  // do error checking
  //
  if (len <= 0) {
    return Error::handle(name(), L"computeFrequencyCommon",
			 ERR, __FILE__, __LINE__);
  }
  
  // generate the new data if necessary
  //
  if (!is_valid_d || (last_length != len + 1)) {
    
    // declare the frequency vector to hold the spectrum of the input
    // signal described by the frequency values from 0 to
    // sample_freq/2 that is given by (N/2+1) points of the FFT
    //
    lin_freq_d.setLength(len + 1, false);
    for (long i = 0; i < len + 1; i++) {
      lin_freq_d(i) = ((float)i * sample_freq_d / (float)(2.0 * len));
    }
    
    // case: MEL
    //
    if (scale_d == MEL) {
      Mel mel;
      mel.compute(warp_freq_d, lin_freq_d);
    }
    
    // case: BARK
    //
    else if (scale_d == BARK) {
      Bark bark;
      bark.compute(warp_freq_d, lin_freq_d);
    }
    
    // case: LINEAR
    //
    else {
      warp_freq_d.assign(lin_freq_d);
    }

    // compute the number of filter banks if filter spacing is given
    //
    if (fsmp_d == BANDWIDTH) {
      
      float num = (warp_freq_d(warp_freq_d.length() - 1)) / bandwidth_d;

      // tolerate upper cut-off of the last filter to be 0.1% greater
      // than fs/2
      //
      order_d = (long)Integral::floor(num);
    }
    
    // do error checking
    //
    if (order_d <= (long)0) {
      return Error::handle(name(), L"computeFrequencyCommon", ERR,
			   __FILE__, __LINE__);
    }
  
    // compute the central frequency for the filters, order_d + 2
    // central frequency values altogether. To compute n filterbanks,
    // we need (n+1) central frequencies. The one extra central
    // frequency is to the right of sample_freq/2 that will be used in
    // the computation of the filterbanks but will not influence the
    // filterbank values since no mel or bark or linear frequency will
    // fall to the right of sample_freq/2
    //
    cen_freq_d.setLength((long)order_d + 2, false);
    cen_freq_d(0) = 0.0;

    if (fsmp_d == BANDWIDTH) {
      for (int i = 1; i <= (long)order_d + 1; i++) {
	cen_freq_d(i) = (float)i * bandwidth_d;
      }
    }
    else if (fsmp_d == ORDER) {
      float s = warp_freq_d(warp_freq_d.length() - 1);
      for (int i = 1; i <= (long)order_d + 1; i++) {
	cen_freq_d(i) = (float)i / (float)((long)order_d + 1) * s;
      }
    }
    else {
      return Error::handle(name(), L"computeFrequencyCommon", ERR_UNKALG,
			   __FILE__, __LINE__);
    }

    // reset the flag
    //
    is_valid_d = true;
  }

  // setup and clear the output vector
  //
  output_a.setLength(order_d, false);
  output_a.assign(0.0);
  
  // exit gracefully
  //
  return true;
}

// method: computeFrequencyTriangular
//
// arguments:
//  VectorFloat& output: (output) output data vector
//  const VectorFloat& input: (input) input data vector
//
// return: a boolean value indicating status
//
// this method gives the filter bank amplitude for given input vector
// for FREQUENCY algorithm and TRIANGULAR implementation
//
boolean FilterBank::computeFrequencyTriangular(VectorFloat& output_a,
					       const VectorFloat& input_a) {
  
  // call common compute method for FREQUENCY algorithm
  //
  if (!computeFrequencyCommon(output_a, input_a)) {
    return Error::handle(name(), L"computeFrequencyTriangular", ERR,
			 __FILE__, __LINE__);
  }

  // local variables
  //
  long len;
  
  // FBA expects a full spectrum as input, but only uses the first
  // half of it. use the complete input if the input mode is half
  // (SYMMETRIC)
  //
  if (input_mode_d == SYMMETRIC) {
    len = input_a.length();
  }
  
  else {
    len = input_a.length() / 2;
  }
  
  // employ an index for guessing the correct bin. If the warping
  // preserves monotonicity, then this will be much more efficient
  // than looping over all possibilities every time
  //
  long guess_ind = 0;
  
  // loop over each frequency value, we don't consider the first and
  // the last sample frequencies since typically the first sample
  // frequency is a dc bias and the last frequency sample at fs/2 is
  // zero
  //
  for (long l = 1; l < len; l++) {

    // check the last bin
    //
    if (warp_freq_d(l) >= cen_freq_d(order_d)) {
      output_a((long)order_d - 1) += input_a(l) *
	  (1 - ((warp_freq_d(l) - cen_freq_d(order_d)) / 
		(cen_freq_d((long)order_d + 1) - cen_freq_d(order_d))));
    }
    
    else {
      
      // try to guess the right place to start
      //
      long start_ind = 0;
      if (warp_freq_d(l) >= cen_freq_d(guess_ind)) {
	start_ind = guess_ind;
      }
      for (long i = start_ind; i < order_d; i++) {
	
	// check this bin: if the frequency falls in this bin then update
	// this bin's output (the upward slope of this bin's triangle) and
	// update the next bin (the downward slope of the previous bin's
	// triangle
	//
	if (warp_freq_d(l) < cen_freq_d(i + 1)) {
	  
	  // update the current bin (upward slope)
	  //
	  output_a(i) += input_a(l) * (warp_freq_d(l) - cen_freq_d(i)) / 
	    (cen_freq_d(i + 1) - cen_freq_d(i));
	  
	  // update the previous bin (downward slope) if necessary
	  //
	  if (i != 0) {
	    output_a(i - 1) += input_a(l) *
	      (1 - ((warp_freq_d(l) - cen_freq_d(i)) / 
		    (cen_freq_d(i + 1) - cen_freq_d(i))));
	  }
	  
	  // no need to keep checking, we've found the right bin, so break
	  // out of the inner for loop
	  //
	  guess_ind = i;
	  break;
	}
      }
    }
  }
  
  // exit gracefully
  //
  return true;
}

// method: computeFrequencyUniform
//
// arguments:
//  VectorFloat& output: (output) output data vector
//  const VectorFloat& input: (input) input data vector
//
// return: a boolean value indicating status
//
// this method gives the filter bank amplitude for given input vector
// for FREQUENCY algorithm and UNIFORM implementation
//
boolean FilterBank::computeFrequencyUniform(VectorFloat& output_a,
					    const VectorFloat& input_a) {
  
  // call common compute method for FREQUENCY algorithm
  //
  if (!computeFrequencyCommon(output_a, input_a)) {
    return Error::handle(name(), L"computeFrequencyUniform", ERR,
			 __FILE__, __LINE__);
  }
  
  // local variables
  //
  long len;
  
  // FBA expects a full spectrum as input, but only uses the first
  // half of it. use the complete input if the input mode is half
  // (SYMMETRIC)
  //
  if (input_mode_d == SYMMETRIC) {
    len = input_a.length();
  }
  
  else {
    len = input_a.length() / 2;
  }
  
  // employ an index for guessing the correct bin. If the warping
  // preserves monotonicity, then this will be much more efficient
  // than looping over all possibilities every time
  //
  long guess_ind = 0;
  
  // loop over each frequency value, we don't consider the first and
  // the last sample frequencies since typically the first sample
  // frequency is a dc bais and the last frequency sample at fs/2 is
  // zero
  //
  for (long l = 1; l < len; l++) {

    // check the last bin
    //
    if (warp_freq_d(l) >= cen_freq_d(order_d)) {
      output_a((long)order_d - 1) += input_a(l);
    }
    
    else {
      
      // try to guess the right place to start
      //
      long start_ind = 0;
      if (warp_freq_d(l) >= cen_freq_d(guess_ind)) {
	start_ind = guess_ind;
      }
      for (long i = start_ind; i < order_d; i++) {
	
	// check this bin: if the frequency falls in this bin then
	// update this bin's output and update the next bin
	//
	if (warp_freq_d(l) < cen_freq_d(i + 1)) {
	  
	  // update the current bin (upward slope)
	  //
	  output_a(i) += input_a(l);
	  
	  // update the previous bin (downward slope) if necessary
	  //
	  if (i != 0) {
	    output_a(i - 1) += input_a(l);
	  }
	  
	  // no need to keep checking, we've found the right bin, so break
	  // out of the inner for loop
	  //
	  guess_ind = i;
	  break;
	}
      }
    }
  }
    
  // exit gracefully
  //
  return true;
}

// method: computeFrequencyRaisedcosine
//
// arguments:
//  VectorFloat& output: (output) output data vector
//  const VectorFloat& input: (input) input data vector
//
// return: a boolean value indicating status
//
// this method gives the filter bank amplitude for given input vector
// for FREQUENCY algorithm and RAISED_COSINE implementation
//
boolean FilterBank::computeFrequencyRaisedcosine(VectorFloat& output_a,
						 const VectorFloat& input_a) {
  
  // call common compute method for FREQUENCY algorithm
  //
  if (!computeFrequencyCommon(output_a, input_a)) {
    return Error::handle(name(), L"computeFrequencyRaisedcosine", ERR,
			 __FILE__, __LINE__);
  }
  
  // local variables
  //
  long len;
  
  // FBA expects a full spectrum as input, but only uses the first
  // half of it. use the complete input if the input mode is half
  // (SYMMETRIC)
  //
  if (input_mode_d == SYMMETRIC) {
    len = input_a.length();
  }
  
  else {
    len = input_a.length() / 2;
  }
  
  // employ an index for guessing the correct bin. If the warping
  // preserves monotonicity, then this will be much more efficient
  // than looping over all possibilities every time
  //
  long guess_ind = 0;
  
  // loop over each frequency value, we don't consider the first and
  // the last sample frequencies since typically the first sample
  // frequency is a dc bais and the last frequency sample at fs/2 is
  // zero
  //
  for (long l = 1; l < len; l++) {

    // check the last bin
    //
    if (warp_freq_d(l) >= cen_freq_d(order_d)) {
      output_a((long)order_d - 1) += input_a(l) *
	(float)Integral::cos(((warp_freq_d(l) - cen_freq_d(order_d)) / (cen_freq_d((long)order_d + 1) - cen_freq_d(order_d))) * (((float)Integral::PI) / (float)4));
    }
    
    else {
      
      // try to guess the right place to start
      //
      long start_ind = 0;
      if (warp_freq_d(l) >= cen_freq_d(guess_ind)) {
	start_ind = guess_ind;
      }
      for (long i = start_ind; i < order_d; i++) {
	
	// check this bin: if the frequency falls in this bin then update
	// this bin's output (the upward slope of this bin's cosine) and
	// update the next bin (the downward slope of the previous bin's
	// cosine fuction
	//
	if (warp_freq_d(l) < cen_freq_d(i + 1)) {
	  
	  // update the current bin (upward slope)
	  //
	  output_a(i) += input_a(l) *
	    (float)Integral::cos(((1- (warp_freq_d(l) - cen_freq_d(i))) / (cen_freq_d(i + 1) - cen_freq_d(i))) * (((float)Integral::PI)/(float)4));
	  
	  // update the previous bin (downward slope) if necessary
	  //
	  if (i != 0) {
	    output_a(i - 1) += input_a(l) *
	      (float)Integral::cos(((warp_freq_d(l) - cen_freq_d(i)) / (cen_freq_d(i + 1) - cen_freq_d(i))) * (((float)Integral::PI)/(float)4));
	  }
	  
	  // no need to keep checking, we've found the right bin, so break
	  // out of the inner for loop
	  //
	  guess_ind = i;
	  break;
	}
      }
    }
  }
  
  // exit gracefully
  //
  return true;
}

