// file: $isip/class/algo/Energy/enrgy_05.cc
// version: $Id: enrgy_05.cc 8165 2002-05-31 21:57:33Z picone $
//

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

// method: apply
//
// arguments:
//  Vector<AlgorithmData>& output: (output) output data
//  const Vector< CircularBuffer<AlgorithmData> >& input: (input) input data
//
// return: a bool8 value indicating status
//
// this method calls the appropriate computation methods
//
bool8 Energy::apply(Vector<AlgorithmData>& output_a,
		      const Vector< CircularBuffer<AlgorithmData> >& input_a) {

  // check the mode
  //
  if (cmode_d != FRAME_INTERNAL) {
    return Error::handle(name(), L"apply", ERR_UNSUPM, __FILE__, __LINE__);
  }

  // determine the number of input channels and force the output to be
  // that number
  //
  int32 len = input_a.length();
  output_a.setLength(len);
  bool8 res = true;

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

  // start the debugging output
  //
  displayStart(this);

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

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

    // branch on compute method type based on the type of input type
    //
    if (input_a(c)(0).getDataType() == AlgorithmData::VECTOR_FLOAT) {
      
      // call AlgorithmData::makeVectorFloat 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);
    }

    else if (input_a(c)(0).getDataType() ==
	     AlgorithmData::VECTOR_COMPLEX_FLOAT) {

      // call AlgorithmData::makeVectorFloat to force the
      // output vector for this channel to be a VectorFloat,
      // call AlgorithmData::getVectorComplexFloat on the input for
      // this channel to check that the input is already a
      // VectorComplexFloat and return that vector.
      //
      res &= compute(output_a(c).makeVectorFloat(),
		     input_a(c)(0).getVectorComplexFloat(),
		     input_a(c)(0).getCoefType(),
		     c);      
     }

    // set the coefficient type for the output
    //            
    output_a(c).setCoefType(AlgorithmData::ENERGY);
  }

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

  // exit gracefully
  //
  return res;
}

// method: compute
//
// arguments:
//  VectorFloat& output: (output) output data
//  const VectorFloat& input: (input) input data
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  int32 channel_index: (input) channel index
//
// return: a bool8 value indicating status
//
// this method computes the energy of the signal using the specified
// algorithm and implementation.
//
bool8 Energy::compute(VectorFloat& output_a,
			const VectorFloat& input_a,
			AlgorithmData::COEF_TYPE input_coef_type_a,
			int32 channel_index_a) {

  // declare local variables
  //
  bool8 status = false;

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

  // algorithm: SUM
  //
  if (algorithm_d == SUM) {
    status = computeSum(output_a, input_a);
  }

  // algorithm: FILTER
  //
  else if (algorithm_d == FILTER) {
    status = computeFilt(output_a, input_a, channel_index_a);
  }

  // error: unknown implementation
  //
  else {
    return Error::handle(name(), L"compute", ERR_UNKIMP,
			 __FILE__, __LINE__);
  }

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

// method: compute
//
// arguments:
//  VectorFloat& output: (output) output data
//  const VectorComplexFloat& input: (input) input data
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  int32 channel_index: (input) channel index
//
// return: a bool8 value indicating status
//
// this method computes the energy of single-channel complex data.
//
bool8 Energy::compute(VectorFloat& output_a,
			const VectorComplexFloat& input_a,
			AlgorithmData::COEF_TYPE input_coef_type_a,
			int32 channel_index_a) {

  // declare local variables
  //
  static VectorFloat v_in_mag;
  bool8 status = false;
  
  // get the magnitude of the complex data
  //
  input_a.mag(v_in_mag);

  // call compute method for magnitude vector
  //
  status = compute(output_a, v_in_mag, input_coef_type_a);

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

// method: computeSum
//
// arguments:
//  VectorFloat& output: (output) output data
//  const VectorFloat& input: (input) input data
//
// return: a bool8 value indicating status
//
// this method computes the energy of the input signal by summing the
// squares of the input signal. this method is called by the public
// compute method.
//
// The result of this operation is then scaled appropriately depending
// in the implementation selected.
//
bool8 Energy::computeSum(VectorFloat& output_a,
			   const VectorFloat& input_a) {

  // compute the sum of squares
  //
  Float e = input_a.sumSquare();

  // compute the scale factor according to specified implementation
  //
  float32 scale_factor = scale(e, input_a.length());

  // set the length of the output vector: should be 1 as it only
  // contains the energy
  //
  output_a.setLength(1);

  // assign the value of energy to the output
  //
  output_a(0) = Integral::max(floor_d, scale_factor);
    
  // exit gracefully
  //
  return true;
}

// method: computeFilt
//
// arguments:
//  VectorFloat& output: (output) output data
//  const VectorFloat& input: (input) input data
//  int32 channel_index: (input) channel index
//
// return: a bool8 value indicating status
//
// this method computes the energy of the input signal by linearly
// filtering the sum of squares. This is described in the
// paper:
//
//  J. Picone, "Signal Modeling Techniques in Speech Recognition,"
//  IEEE Proceedings, vol. 81, no. 9, pp. 1215-1247, September 1993. 
// 
// The result of this operation is then scaled appropriately depending
// in the implementation selected.
//
bool8 Energy::computeFilt(VectorFloat& output_a,
			    const VectorFloat& input_a,
			    int32 channel_index_a) {

  // declare local variables
  //
  int32 len = input_a.length();
  int32 ma_max_lag = ma_coef_d.length();
  int32 ar_max_lag = ar_coef_d.length();

  float32 acc = 0.0;

  // loop through the input signal
  //
  for (int32 index = 0; index < len; index++) {
      
    // advance the read pointer and append the input sample
    // (which is the square of the signal)
    //
    float32 sum = input_a(index);
    sum *= sum;
    ma_mem_d(channel_index_a).advanceAndAssign(sum);
    
    // loop through the filter coefficients
    //
    acc = 0;

    for (int32 i = 0; i < ma_max_lag; i++) {
      acc += (float32)ma_coef_d(i) * (float32)ma_mem_d(channel_index_a)(-i);
    }
      
    for (int32 i = 1; i < ar_max_lag; i++) {
      acc -= (float32)ar_coef_d(i) * (float32)ar_mem_d(channel_index_a)(-i);
    }

    // advance the ar memory and save the output
    //
    ar_mem_d(channel_index_a).assignAndAdvance(acc);
  }

  // compute the scale factor according to specified implementation
  //
  float32 scaled_value = scale(acc, input_a.length());

  // set the length of the output vector: should be 1 as it only
  // contains the energy
  //
  output_a.setLength(1);

  // assign the value of energy to the output
  //
  output_a(0) = Integral::max(floor_d, scaled_value);
    
  // exit gracefully
  //
  return true;
}

// method: scale
//
// arguments:
//  const Float& value: (input) value to scale
//  int32 len: (input) length of the input signal
//
// return: a bool8 value indicating status
//
// this method computes the scale factor according to specified
// implementation. below is formula used:
//
//  implementation    scaling
//  --------------    -------
//   IDENTITY         1.0 (none)
//   LOG              log(s)
//   DB               10 * log10(s)
//   POWER            (1 / N) * s
//   LOG_POWER        log(s / N)
//   DB_POWER         10 * log10(s / N)
//   RMS              sqrt(s / N)
//   LOG_RMS          log(sqrt(s / N))
//   DB_RMS           == DB_POWER
//        
// where "s" is the input energy, and "N" is the number of samples in the
// input.
//
float32 Energy::scale(const Float& value_a, int32 len_a) {

  // branch on implementation:
  //
  //  Implementation: LINEAR
  //   return the input
  //
  if (implementation_d == IDENTITY) {
    return (float32)value_a;
  }

  // Implementation: LOG
  //  return an unscaled log
  //
  else if (implementation_d == LOG) {
    return Integral::log((float32)value_a);
  }

  // Implementation: DB
  //  return a scaled log
  //
  else if (implementation_d == DB) {
    return Integral::DB_POW_SCALE_FACTOR * Integral::log10((float32)value_a);
  }

  // at this point, all remaining methods need to divide by the
  // number of samples
  //
  if (len_a <= 0) {
    return Error::handle(name(), L"scale", ERR_INSUFD,
			 __FILE__, __LINE__);
  }

  // Implementation: POWER
  //  scale by 1/N
  //
  if (implementation_d == POWER) {
    return (float32)value_a / (float32)len_a;
  }

  // Implementation: LOG_POWER
  //  return an unscaled log
  //
  else if (implementation_d == LOG_POWER) {
    return Integral::log((float32)value_a / (float32)len_a);
  }

  // Implementation: DB_POWER
  //  return a scaled log
  //
  else if ((implementation_d == DB_POWER) ||
	   (implementation_d == DB_RMS)) {
    return Integral::DB_POW_SCALE_FACTOR *
      Integral::log10((float32)value_a / (float32)len_a);
  }

  // Implementation: RMS
  //  sqrt(1/N * energy)
  //
  else if (implementation_d == RMS) {
    return Integral::sqrt((float32)value_a / (float32)len_a);
  }

  // Implementation: LOG_RMS
  //  return an unscaled log
  //
  else if (implementation_d == LOG_RMS) {
    return Integral::log(Integral::sqrt((float32)value_a / (float32)len_a));
  }

  // else: unknown condition
  //
  else {
    return Error::handle(name(), L"scale", ERR_UNKIMP,
			 __FILE__, __LINE__);
  }
}