// file: $isip/class/algo/Covariance/cov_05.cc
// version: $Id: cov_05.cc 8021 2002-03-31 00:18:04Z gao $
//

// isip include files
//
#include "Covariance.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 computation method with appropriate input and
// output data types
//
bool8 Covariance::apply(Vector<AlgorithmData>& output_a,
			  const Vector< CircularBuffer<AlgorithmData> >&
			  input_a) {

  // 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;

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

  // if this is the first frame for ACCUMULATE mode, initialize the
  // accumulation variables
  //
  if ((frame_index_d == 0) &&
      (input_a(0)(0).getCoefType() == AlgorithmData::GENERIC) &&
      (algorithm_d == NORMAL)) {
    setAccumulateVar(len, input_a(0)(0).getVectorFloat().length());
  }
  
  // loop over the channels and call the compute method
  //
  for (int32 c = 0; c < len; c++) {

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

    // call AlgorithmData::makeMatrixFloat to force the output vector for
    // this channel to be a MatrixFloat, 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).makeMatrixFloat(),
		   input_a(c)(0).getVectorFloat(),
		   input_a(c)(0).getCoefType(), c);

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

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

  // exit gracefully
  //
  return res;
}

// method: compute
//
// arguments:
//  MatrixFloat& output: (output) covariance coefficients
//  const VectorFloat& input: (input) input data
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  int32 index: (input) channel number
//
// return: a bool8 value indicating status
//
// this method computes the covariance coefficients from signal
//
bool8 Covariance::compute(MatrixFloat& output_a,
			    const VectorFloat& input_a,
			    AlgorithmData::COEF_TYPE input_coef_type_a,
			    int32 index_a) {
  
  // declare local variables
  //
  bool8 status = false;

  // set order
  //
  if (order_d < (int32)1) {
    output_a.setDimensions(0, 0);
    return Error::handle(name(), L"compute", ERR_ORDER,
			 __FILE__, __LINE__, Error::WARNING);
  }

  // branch on algorithm
  //  Algorithm: NORMAL
  //
  if (algorithm_d == NORMAL) {
    if (input_coef_type_a == AlgorithmData::SIGNAL) {
      if (implementation_d == FACTORED) {
	status = computeNormalFactored(output_a, input_a);
      }
      else {
	status = computeNormalUnFactored(output_a, input_a);
      }
    }
    else if (input_coef_type_a == AlgorithmData::GENERIC) {
      status = computeAccumulate(output_a, input_a, index_a);
    }
    else {
      return Error::handle(name(), L"compute", ERR_UNCTYP, __FILE__, __LINE__);
    }
  }  
  
  // possibly display the data
  //
  display(output_a, input_a, name());
  
  // exit gracefully
  //
  return status;
}

// method: computeNormalFactored
//
// arguments:
//  MatrixFloat& covcoeff: (output) covariance coefficients
//  const VectorFloat& input: (input) input data
//
// return: a bool8 value indicating status
//
// this method computes the covariance coefficients using FACTORED
// implementation. See:
//
//  J.D. Markel and A.H. Gray, Jr.,
//  Linear Prediction of Speech, Springer-Verlag Berlin Heidelberg,
//  New York, New York, USA, pp. 220, 1976.
//
// The key equation is Eq. 9.10:
//
//   c[i,j] = c[i-1,j-1] + x[M-i]*x[M-j] - x[N-i]x[N-j]
//
bool8 Covariance::computeNormalFactored(MatrixFloat& covcoeff_a,
					  const VectorFloat& input_a) {

  // setup: note that the size is (order+1).
  //
  if (!covcoeff_a.setDimensions((int32)order_d + 1, (int32)order_d + 1)) {
    return Error::handle(name(), L"computeNormalFactored", ERR,
			 __FILE__, __LINE__, Error::WARNING);
  }
  
  // local variable
  //
  int32 row_index, col_index;
  
  // compute all internal elements
  //
  int32 N = input_a.length();
  int32 M = order_d;
  
  // compute c[0,i] elements
  //
  for (col_index = 0; col_index <= M; col_index++) {
    
    float64 sum = 0.0;
    
    for (int32 n = M; n < N; n++) {
      
      //            N-1
      //   c[0,j] = sum x(n-0) x(n-j)
      //            n=M
      //
      sum += (float32)input_a(n) * (float32)input_a(n - col_index);
    }

    covcoeff_a.setValue(0, col_index, sum);
    covcoeff_a.setValue(col_index, 0, sum);
  }

  // loop though the lower triangular elements
  //
  for (row_index = 1; row_index <= order_d; row_index++) {
    for (col_index = 1; col_index <= row_index; col_index++) {

      // compute c[i,j] = c[i-1,j-1] + x[M-i]*x[M-j] - x[N-i]x[N-j]
      //
      float32 sum = covcoeff_a.getValue(row_index - 1, col_index - 1)
	+ (float32)input_a(M - row_index) * input_a(M - col_index)
	- (float32)input_a(N - row_index) * (float32)input_a(N - col_index);
      
      // set the lower triangular part of the covariance matrix
      //
      covcoeff_a.setValue(row_index, col_index, sum);

      // set the upper triangular part of the covariance matrix
      //
      if (row_index != col_index) {
	covcoeff_a.setValue(col_index, row_index, sum);
      }
    }
  }
  
  // exit gracefully
  //
  return covcoeff_a.div(N);
}

// method: computeNormalUnFactored
//
// arguments:
//  MatrixFloat& covcoeff: (output) covariance coefficients
//  const VectorFloat& input: (input) input data
//
// return: a bool8 value indicating status
//
// this method compute the covariance coefficients within current frame. See:
//
//  J.D. Markel and A.H. Gray, Jr.,
//  Linear Prediction of Speech, Springer-Verlag Berlin Heidelberg,
//  New York, New York, USA, pp. 51, 1976.
//
// The key equation is Eq. 3.37:
//
//            N-1
//   c[i,j] = sum x(n-i) x(n-j)
//            n=M
//
bool8 Covariance::computeNormalUnFactored(MatrixFloat& covcoeff_a,
					    const VectorFloat& input_a) {

  // setup: note that the size is (order+1).
  //
  if (!covcoeff_a.setDimensions((int32)order_d + 1, (int32)order_d + 1)) {
    return Error::handle(name(), L"computeNormalUnFactored", ERR,
			 __FILE__, __LINE__, Error::WARNING);
  }
  
  // compute all internal elements
  //
  int32 N = input_a.length();
  int32 M = order_d;

  // loop through the lower triangular part of the covariance matrix 
  //
  for (int32 row_index = 0; row_index <= order_d; row_index++) {
    for (int32 col_index = 0; col_index <= row_index; col_index++) {

      float64 sum = 0;
      
      for (int32 n = M; n < N; n++) {
      
	//            N-1
	//   c[i,j] = sum x(n-i) x(n-j)
	//            n=M
	//
        sum += (float32)input_a(n - row_index) * (float32)input_a(n - col_index);
      }

      // set the lower triangular part of the covariance matrix
      //
      covcoeff_a.setValue(row_index, col_index, sum);

      // set the upper triangular part of the covariance matrix
      //
      if (row_index != col_index) {
	covcoeff_a.setValue(col_index, row_index, sum);
      }
    }
  }
  
  // exit gracefully
  //
  return covcoeff_a.div(N);
}

// method: computeAccumulate
//
// arguments:
//  MatrixFloat& covcoeff: (output) covariance coefficients
//  const VectorFloat& input: (input) input data
//  int32 chan: (input) channel number
//
// return: a bool8 value indicating status
//
// this method compute the covariance across successive frames of data
// and returns the global covariance matrix for the file in ACCUMULATE
// mode.
//
// The key equation is:
//
//   if it is not the last frame:: c(i, j) += x(i) x(j),  u(i) += x(i)
//
//   if it is  the last frame: C(i, j) = 1/N * sum{c(i,j)} - u(i) u(i)'
//                   
bool8 Covariance::computeAccumulate(MatrixFloat& covcoeff_a,
				      const VectorFloat& input_a,
				      int32 chan_a) {


  float64  num_frames = signal_duration_d / frame_dur_d;
  int32 frame = (int32)Integral::round(num_frames);

  if (frame_index_d < frame - 1) {

    // the first and other frame
    //
  
    // c(i, j) += x(i) x(j),  u(i) += x(i)
    //
    covcoeff_a.outerProduct(input_a, input_a);
    
    // update the accumulation variables
    //
    accum_cov_d(chan_a).add(covcoeff_a);
    covcoeff_a.assign(accum_cov_d(chan_a));
    accum_sum_d(chan_a).add(input_a);
    accum_frame_d++;

  }
  
  // the last frame
  //
  else if ( frame_index_d == frame - 1) {

    // update the accumulation variables if it contains data
    //
    if (input_a.length()) {
      
      // c(i, j) += x(i) x(j),  u(i) += x(i)
      //
      covcoeff_a.outerProduct(input_a, input_a);
      
      // update the accumulation variables
      //
      accum_cov_d(chan_a).add(covcoeff_a);
      accum_sum_d(chan_a).add(input_a);
      accum_frame_d++;
    }
    
    // at the end of the file
    //   C(i, j) = 1/N * sum{c(i,j)} - u(i) u(i)'
    //
    accum_cov_d(chan_a).div(accum_frame_d);
    accum_sum_d(chan_a).div(accum_frame_d);
    covcoeff_a.outerProduct(accum_sum_d(chan_a), accum_sum_d(chan_a));
    covcoeff_a.sub(accum_cov_d(chan_a), covcoeff_a);
  }

  else
    return Error::handle(name(), L"computeAccumulate", ERR_UNCTYP,
			 __FILE__, __LINE__);
  
  // exit gracefully
  //
  return true;
}