/*
 *  yin_helpers.cpp
 *  
 *
 */

#include "fftw.h"
#include <math.h>


void yin::ACF(const float *ins, int n, float* r)
{
  fftw_plan forward = fftw_create_plan(2*n, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_complex in[2*n];
  fftw_complex out[2*n];
  
  for (int i=0; i<n; i++) 
  	{
  		in[i].re = ins[i];
  		in[i].im = 0;
  	}
  
  // zero-pad
  for (int i=n; i<2*n; i++) 
  	{
  	   in[i].re = 0;
  	   in[i].im = 0;
  	}
  
  fftw_one(forward, in, out);

  fftw_destroy_plan(forward);
  
  //calculate power spectral density
  for (int i=0;i<2*n;i++) in[i].re = (out[i].re*out[i].re) + (out[i].im*out[i].im);
  
  //inverse fft to get autocorrelation
  fftw_plan backward = fftw_create_plan(2*n, FFTW_BACKWARD, FFTW_ESTIMATE);
    
  fftw_one(backward, in, out);

  fftw_destroy_plan(backward);
  
  for (int i=0; i<n; i++) r[i] = (out[i].re/(2*n))*((float)(n/(n-i))); //remove bias
}

void yin::diff(const float *ins, float* r, int n, float *difference)
{
  float rtTau = r[0];
  
  for (int i=0; i<n; i++)
  {
	difference[i] = r[0] + rtTau - (2 * r[i]);
	
	rtTau = rtTau + (ins[n+i]*ins[n+i]) - (ins[i]*ins[i]);
  }
}

void yin::cumnorm(float* difference, int n, float* d_primes)
{
  float denom = difference[0];

  d_primes[0] = 1;
	
  for (int i=1; i<n; i++)
   {
      denom += difference[i];
		
      if (denom == 0) d_primes[i] = 0;
      else d_primes[i] = (i * difference[i]) / denom;
   }
}

float yin::get_max_period(float* d_primes, int n)
{
  float max_period;
  float min = 100;       // initialize min to a value that is sure 
	                 //to be larger than the minimum value in d_primes

  //find fundamental period
  for (int i=45; i<540; i++)
    {
      if (d_primes[i] < min)
       {
	 min = d_primes[i];
	 max_period = i;
       }
    }
   
  return max_period;
}