// FreqSpec.m

#include	<stdio.h>
#include	<stdlib.h>
#include	<math.h>
#include	<limits.h>
#include	<errno.h>
#include	<objpak.h>
#include	"FreqSpec.h"

#ifndef M_PI
#define	M_PI	3.14159265358979323846
#endif

// for computing hanning window on FFT input data
#define	hanning(n, N)	(0.5 - 0.5 * cos((2 * M_PI * n) / N))

// error macros
#define	PROGNAME	"FreqSpec"
#define fatal_error(s) do {				\
    fprintf(stderr, PROGNAME ":%s Error: %s: %s\n",	\
	    __FUNCTION__,				\
	    s,						\
	    strerror(errno));				\
    exit(EXIT_FAILURE);					\
} while (0)

//  static inline int log2(double x);

@implementation FreqSpec {}

- init
{
				// buggish: must be called after "N:"

    plan	 = rfftw_create_plan(N,
				     FFTW_REAL_TO_COMPLEX,
				     FFTW_ESTIMATE);

    if (! (in = malloc(N * sizeof(fftw_real))) )
      fatal_error("malloc");
    
    if (! (out = malloc(N * sizeof(fftw_real))) ) 
      fatal_error("malloc");
    
    n_spectra	 = N / 2 + 1;
    if (! (spectrum = malloc(n_spectra * sizeof(fftw_real))) )
      fatal_error("malloc");

    // normalize the magnitudes by dividing by N / 2
    normalize		 = YES;
    normalize_scale	 = N / 2;

    return self;
}

+ newWithN: (size_t) N
{
    self	 = [ super new ];
    [ self N: N ];
    [ self init ];
    return self;
}

- (void)N: (size_t) n
{
    N	 = n;
}

- use_hanning: (BOOL) yesno
{
    use_hanning	 = yesno;
    if (use_hanning)
      normalize_scale	 = 1.3 * (N / 2);
    return self;
}

- analyze
{
    int		i, end;
    fftw_real	magnitude;

    if (use_hanning) {
      // apply Hanning window to input data
      for (i = 0; i < N; ++i)
	in[i]	 *= hanning(i, N);
    }

    rfftw_one(plan, in, out);
    spectrum[0]	 = sqrt(out[0] * out[0]); /* DC component */
    end		 = (N + 1) / 2;	/* i < N/2 rounded up */
    for (i = 1; i < end; ++i) {
      magnitude		 = sqrt(out[i] * out[i] + out[N - i] * out[N - i]);
      if (normalize)
	magnitude	 /= normalize_scale;
      spectrum[i]	 = magnitude;
    }
    if ((N % 2) == 0) {		/* for even N */
      magnitude		 = sqrt(out[N/2] * out[N/2]);	/* Nyquist freq */
      if (normalize)
	magnitude	 /= normalize_scale;
      spectrum[N/2]	 = magnitude;
    }

    return self;
}

- shortAnalyze: (short *) samples
{
    int		i;

    /* copy from shorts into floats */
    for (i = 0; i < N; ++i)
      in[i]	 = samples[i];
    return [ self analyze ];
}

- intAnalyze: (int *) samples
{
    int		i;

    /* copy from shorts into floats */
    for (i = 0; i < N; ++i)
      in[i]	 = samples[i];
    return [ self analyze ];
}

- (BOOL) freqs: (fftw_real *) frequencies
  withBaseFreq: (fftw_real) freq_spacing
     intoBands: (fftw_real *) bands
{
    fftw_real	avg;
    int		n_bands	 = 0;
    int		i, j, start, end, distance;

    // ---------count the number of requested frequencies in the 
    //          zero-terminated array
    while (frequencies[n_bands] != 0)
      ++n_bands;

    for (i = 0; i < n_bands - 1; ++i) {
      // start	 = log2(frequencies[i] * freq_spacing);
      start	 = frequencies[i] / freq_spacing;
      end	 = frequencies[i + 1] / freq_spacing;
      distance	 = end - start;

      if (distance < 0) {
	fprintf(stderr, "FreqSpec Error: malformed frequency array\n");
	return NO;
      }
      for (j = start, avg = 0; j < end; ++j) 
	  avg	 += spectrum[j] / distance; // hopefully avoid overflow
      bands[i]	 = avg;
    }

    return YES;
}

- destroy
{
    rfftw_destroy_plan(plan);
    if (in)		free(in);
    if (out)		free(out);
    if (spectrum)	free(spectrum);
    
    return nil;
}

@end

//  static inline int log2(double x)
//  {
//      int		exp;
//      frexp(x, &exp);
//      printf("log2 of %f is %d\n", x, exp - 1);
//      return --exp;
//  }

/*
- (fftw_real *)NBand: (int) n_bands
{
    // pedants say don't cast malloc's return value in C, only C++
    fftw_real	*bands		 = xmalloc(n_bands * sizeof(fftw_real));
    int		per_band	 = n_spectra / n_bands;	// spectra per band
    fftw_real	sum, avg;
    int		i, j;

    for (i = 0; i < n_bands; ++i) {
      for (j = 0, sum = 0; j < per_band; ++j) 
	  sum	 += spectrum[i * per_band + j];
      avg	 = sum / per_band;
      bands[i]	 = avg;
    }

    return bands;
}

*/