/**************************************************************************/
/*                       File: spectrum_compute.h                         */
/**************************************************************************/
/* Project: Computing spectrum of a given signal Class: EE 8993 Spring 96 */
/* Author: Neeraj Deshmukh                           Date: March 2, 1996  */
/**************************************************************************/

/*========================================================================
  This file defines the various parameters and the classes for spectral 
  computation of any given signal
  ========================================================================*/
/*------------------------------------------------------------------------
  system and ISIP include files
  ------------------------------------------------------------------------*/
#include "spectrum_prms.h"

/*------------------------
  debias the signal window
  ------------------------*/
void debias_signal_cc (int_2 *input_fr_l, int_2 win_dur_l)
{
  // compute the average signal value
  //
  int_4 sig_sum_l = 0;
  for (int i = 0; i < win_dur_l; i++)
    sig_sum_l += input_fr_l[i];
  
  int_2 sig_avg_l = (int_2) (sig_sum_l / win_dur_l);
  
  // subtract this from sample values
  //
  for (int i = 0; i < win_dur_l; i++)
    input_fr_l[i] -= sig_avg_l;
}

/*----------------------------------
  create the twiddle factors for FFT
  ----------------------------------*/
void compute_twiddle_factors_cc (float_4 *tw_real_l, float_4 *tw_imag_l, int_2 fft_ord_l)
{
  // compute the angle factor
  //
  float_4 const_factor_l = 2.0 * PI / fft_ord_l;
  int_2   interval_l     = fft_ord_l / 2;

  // now compute both real and imaginary twiddle factors
  //
  for (int_2 i = 0; i < interval_l; i++)
    {
      tw_real_l[i] = (float_4) cos (i * const_factor_l);
      tw_imag_l[i] = (float_4) sin (i * const_factor_l);

      tw_real_l[i + interval_l] = -1.0 * tw_real_l[i];
      tw_imag_l[i + interval_l] = -1.0 * tw_imag_l[i];
    }
}

/*----------------------------------------------------------------------
  compute the N-point FFT of the given signal frame 
  ----------------------------------------------------------------------*/
// Simple DFT
//
void compute_dft_cc (float_4 *fft_fr_l, float_4 *twid_real_l, 
		     float_4 *twid_imag_l, int_2 fft_ord_l)
{
  // the component FFTs
  //
  float_4 *fft_real_l = new float_4[fft_ord_l];
  float_4 *fft_imag_l = new float_4[fft_ord_l];
  
  for (int_2 i = 0; i < fft_ord_l; i++)
    {
      fft_real_l[i] = 0;
      fft_imag_l[i] = 0;
      
      for (int_2 j = 0; j < fft_ord_l; j++)
	{
	  fft_real_l[i] += (fft_fr_l[j] * twid_real_l[(i * j) % fft_ord_l]);
	  fft_imag_l[i] += (fft_fr_l[j] * twid_imag_l[(i * j) % fft_ord_l]);
	}
    }      

  for (int_2 i = 0; i < fft_ord_l; i++)
    {
      fft_fr_l[i] = DB_POW_FACTOR * log10 (fft_real_l[i] * fft_real_l[i] + 
					   fft_imag_l[i] * fft_imag_l[i]);
    }

  // free memory and return
  //
  delete [] fft_real_l;
  delete [] fft_imag_l;
}

// FFT
// we use the radix-2 decimation-in-frequency algorithm by Burrus & Parks
// the FFT aray is initialized with the signal values padded by 0s
//
void compute_fft_cc (float_4 *fft_fr_l, float_4 *twid_real_l, 
		     float_4 *twid_imag_l, int_2 fft_ord_l)
{
  // compute number of FFT levels
  //
  int_2 num_levels_l   = (int_2) rint (LOG_2_FACTOR * log10 ((float_4) fft_ord_l));
  
  // initialize the component FFTs at the start level
  //
  float_4 *fft_real_l = new float_4[fft_ord_l + 1];
  float_4 *fft_imag_l = new float_4[fft_ord_l + 1];
  
  for (int_2 i = 1; i < fft_ord_l + 1; i++)
    {
      fft_real_l[i] = fft_fr_l[i - 1];
      fft_imag_l[i] = 0;
    }
  
  // now perform step-wise FFT computation at each level
  //
  int_2 this_fft_ord_l = fft_ord_l;
  
  for (int_2 i = 1; i < num_levels_l + 1; i++)
    {
      // intialize parameters for this level
      //
      int_2 num_fft_l     = this_fft_ord_l;
      this_fft_ord_l     /= 2;
      int_2 twid_ind_1_l  = 1;
      int_2 twid_ind_2_l  = fft_ord_l / num_fft_l;
      
      // compute FFTs for this level
      // this is the DIF butterfly computation
      //
      for (int_2 j = 1; j < this_fft_ord_l + 1; j++)
	{
	  float_4 cos_temp      = twid_real_l[twid_ind_1_l];
	  float_4 sin_temp      = twid_imag_l[twid_ind_1_l];
	  twid_ind_1_l += twid_ind_2_l;

	  for (int_2 k = j; k < fft_ord_l; k += num_fft_l)
	    {
	      int_2 l           = k + this_fft_ord_l;
	      float_4 real_temp = fft_real_l[k] - fft_real_l[l];
	      float_4 imag_temp = fft_imag_l[k] - fft_imag_l[l];
	      
	      fft_real_l[k] += fft_real_l[l];
	      fft_imag_l[k] += fft_imag_l[l];
	      
	      fft_real_l[l] = cos_temp * real_temp + sin_temp * imag_temp;
	      fft_imag_l[l] = cos_temp * imag_temp - sin_temp * real_temp;
	    }
	}
    }
  
  // now re-arrange the FFT coefs obtained in proper order
  //
  int_2 j = 1;
  int_2 fft_part_l;
  
  for (int_2 i = 1; i < fft_ord_l; i++)
    {
      if (i < j)
	{
	  float_4 real_temp = fft_real_l[j];
	  fft_real_l[j]     = fft_real_l[i];
	  fft_real_l[i]     = real_temp;
	  
	  float_4 imag_temp = fft_imag_l[j];
	  fft_imag_l[j]     = fft_imag_l[i];
	  fft_imag_l[i]     = imag_temp;
	}

	fft_part_l    = fft_ord_l / 2;
	
      while (fft_part_l < j)
	{
	  j          -= fft_part_l;
	  fft_part_l /= 2;
	}
    
      j += fft_part_l;
    }
  
  // now compute the magnitude spectrum in dB
  //
  for (int_2 i = 0; i < fft_ord_l; i++)
    {
      fft_fr_l[i] = DB_POW_FACTOR * log10 (fft_real_l[i+1] * fft_real_l[i+1] + 
					   fft_imag_l[i+1] * fft_imag_l[i+1]);
    }

  // free memory and return
  //
  delete [] fft_real_l;
  delete [] fft_imag_l;
}

/*---------------------------------------------------------
  compute the Nth order LP coefficients of the signal frame
  use the Levinson Durbin recursion to do this
  ---------------------------------------------------------*/
float_4 compute_lp_cc (float_4 *lp_pred_l, int_2 lp_ord_l, float_4 *autocorr_l)
{
  // the reflection coefficients and temporary predictor coefficients
  //
  float_4 *lp_refl_l = new float_4[lp_ord_l + 1];
  float_4 *pred_temp = new float_4[lp_ord_l + 1];

  // initialize the error
  //
  float_4 lp_err_l = autocorr_l[0];
  
  // the LD recursion
  //
  for (int_2 i = 1; i < lp_ord_l + 1; i++)
    {
      // reflection coefficients
      //
      lp_refl_l[i] = 0;
      for (int_2 j = 1; j < i; j++)
	lp_refl_l[i] -= pred_temp[j] * autocorr_l [i - j];
      lp_refl_l[i] += autocorr_l[i];
      lp_refl_l[i] /= lp_err_l;
      
      // predictor coefficients
      //
      lp_pred_l[i] = lp_refl_l[i];
      for (int_2 j = 1; j < i; j++)
	lp_pred_l[j] -= (lp_refl_l[i] * pred_temp[i - j]);
      
      // update LP error
      //
      lp_err_l *= (1 - lp_refl_l[i] * lp_refl_l[i]);
      
      // update the predictor coefficients
      //
      for (int_2 j = 0; j <= i; j++)
	pred_temp[j] = lp_pred_l[j];
      
      // if reflection coefficient > 1 something is very wrong
      //
      if (fabs (lp_refl_l[i]) > 1.0)
	{
	  fprintf (stderr, "\nERROR! Reflection coefficients are out of bounds!\n\n");
	  exit (-1);
	}
    }
  
  // free memory
  //
  delete [] lp_refl_l;
  delete [] pred_temp;

  // return the predictor gain in dB
  //
  return (DB_POW_FACTOR * log10 (lp_err_l));
}

/*------------------------------------------------------------
  compute DFT values to plot the response of inverse LP filter
  ------------------------------------------------------------*/
void lp_response_cc (float_4 *lp_pred_l, float_4 *twid_real_l, float_4 *twid_imag_l, 
		     float_4 *lp_spectrum_l, int_2 lp_ord_l, int_2 freq_ord_l)
{
  // we will keep the sampling of frequency at a scale same as that in 
  // the FFT for matching the two plots 
  // therefore freq_ord_l will be always fft_ord_l
  // now compute the magnitude spectrum
  //
  for (int_2 i = 0; i < freq_ord_l; i++)
    {
      float_4 real_mag_l = 1.0;
      float_4 imag_mag_l = 0;
      for (int_2 j = 1; j < lp_ord_l + 1; j++)
	{
	  real_mag_l -= lp_pred_l[j] * twid_real_l[(i*j) % freq_ord_l];
	  imag_mag_l += lp_pred_l[j] * twid_imag_l[(i*j) % freq_ord_l];
	}
      lp_spectrum_l[i] = DB_POW_FACTOR * log10 (real_mag_l * real_mag_l + 
						imag_mag_l * imag_mag_l);
    }
}