// file: fbc_detect2.C
//
// Detect feedback in frame of data.
//

#include "fbc.h"


int fbc_detect2_C (double* fft_out, float threshold_const2, float bandwidth,
		   float* center_freq2, float* max2, int fft_num,
		   float sample_frequency, int& num_maximums2, int frame_number) {


  //  Declare local variable for the magnitude of the fft
  //
  double* fft_mag = (double*)malloc(fft_num/2 * sizeof(double));
  
  //  Compute the magnitude of the fft,
  //  The fft is in real and imaginary parts,  0: real 1: imaginaries, 2:
  //  3: imaginary, ...   2*k are reals, 2*k+1 are imaginaries.
  //
  for (int k = 0; k<fft_num/2; k++){
      fft_mag[k] = sqrt(fft_out[2*k]*fft_out[2*k] + fft_out[2*k+1]*fft_out[2*k+1]);
      } 

  /*
  if(frame_number == 5){
  for(int j=0;j<fft_num/2;j++){
       printf("fft_num: %d, real: %f, img: %f\n", j,fft_out[2*j],fft_out[2*j+1]);
       printf("multiple detect, fft_num: %d, mag: %f\n", j,fft_mag[j]);
    }
  }
  */
  
  //  Compute the average value of the FFT
  //
  float average_peak = 0.0;
  for (int current_sample_num=0; current_sample_num<fft_num/2; current_sample_num++){
      average_peak = average_peak + fft_mag[current_sample_num];
      }
  average_peak = average_peak / fft_num;

  //  Define the threshold to determine what is above it
  //
  float threshold = threshold_const2 * average_peak;

  /*
    if(frame_number == 5){

  printf("threshold: %f\n\n",threshold);
   }
   */

  
  //  Declare local variable for counting the max number of samples
  //
  int max_sample_num = 0;

  //  Declare pointer to hold the position of the max in the fft
  //
  int* position_of_cf = (int*)malloc(fft_num/2 * sizeof(int));
  
  //  Set the first sample to the maximum, and set the max counter to 0 (1st sample)
  //
  max2[0] = 0.0;
  max_sample_num = 0;
  num_maximums2 = 0;

  //  Loop through frame to find max
  //
  for (int current_sample_num=0; current_sample_num<fft_num/2; current_sample_num++){
      if (fft_mag[current_sample_num] > threshold) {
         max2[num_maximums2] = fft_mag[current_sample_num];
	 position_of_cf[num_maximums2] = current_sample_num; 
         num_maximums2 = num_maximums2 + 1;
         }
      }
  

  // Determine center frequency of each maximum sample
  //
  for (int current_center_freq=0; current_center_freq<num_maximums2; current_center_freq++){
      center_freq2[current_center_freq] = position_of_cf[current_center_freq]
	                                 * sample_frequency / fft_num;

      //  printf("detect2, centerfreq: %f\n", center_freq2[current_center_freq]);
      /*
      if(frame_number == 5){
    printf("peak at sample %d, center freq: %f\n\n", position_of_cf[current_center_freq], center_freq2[current_center_freq]);
        }
      */
  }
  // exit gracefully
  //
  return L_TRUE;

}