/* DSPFFTFrequency.m -- copyright 1992, 1993, 1994 by C.D.Lane */

#import <musickit/musickit.h>
#import <dsp/arrayproc.h>

#import "DSPFFTFrequency.h"

#define FFT_SIZE (1024)
#define FREQ_ADJUST (2)

#define FFT_DATA DSPAPGetLowestAddressXY()
#define FFT_COEF (FFT_DATA + FFT_SIZE)

#define FFT_IMAG DSPMapPMemY(FFT_DATA)
#define FFT_REAL DSPMapPMemX(FFT_DATA)

#define SIN_TABLE DSPMapPMemY(FFT_COEF)
#define COS_TABLE DSPMapPMemX(FFT_COEF)

typedef enum {LINEAR = -1, INDEXED = 0} ADDR_MODES;

@implementation DSPFFTFrequency : Frequency

- init
{
#ifdef DEBUG	
	(void) DSPSetErrorFP(stderr);
	(void) DSPEnableErrorLog();
#endif
	return [super init];
}

- free
{
#ifdef DEBUG	
	(void) DSPDisableErrorLog();
#endif
	return [super free];
}

- (double) computeFrequency:sample
{
	unsigned int i;
	MKKeyNum note, maximum = c00k;
	double pitch, power, pitches[c7k], hits[c7k];
	short data[FFT_SIZE], *pointer = (short *) [(Sound *) sample data];
	float spectrum[FFT_SIZE], rate = ([sample samplingRate] / FREQ_ADJUST) / FFT_SIZE;

	if([sample sampleCount] < (FFT_SIZE * FREQ_ADJUST)) return FREQUENCY_UNSTABLE;

	for(i = 0; i < FFT_SIZE; i++) data[i] = pointer[i * FREQ_ADJUST];

	for(note = c00k; note < b6k; note++) pitches[note] = hits[note] = 0.0;

	if(DSPAPInit() == 0) {
		(void) DSPAPWriteFloatArray(DSPAPSinTable(FFT_SIZE), SIN_TABLE, 1, FFT_SIZE/2);
		(void) DSPAPWriteFloatArray(DSPAPCosTable(FFT_SIZE), COS_TABLE, 1, FFT_SIZE/2);

		(void) DSPAPWriteShortArray(data, FFT_REAL, 1, FFT_SIZE);

		(void) DSPAPvclear(FFT_IMAG, 1, FFT_SIZE);

		(void) DSPAPfftr2a(FFT_SIZE, FFT_DATA, FFT_COEF);

		(void) DSPSetDMAReadMReg(INDEXED); {
			(void) DSPAPReadFloatArray(spectrum, FFT_IMAG, FFT_SIZE/2, FFT_SIZE);
			} (void) DSPSetDMAReadMReg(LINEAR);

		(void) DSPAPFree();
		}
	else return FREQUENCY_ERROR;

	for(i = 0; i < ((FFT_SIZE/2)/FREQ_ADJUST); i++) {
		note = MKFreqToKeyNum(pitch = (i * rate), NULL, 0.0);
		hits[note] += (power = fabs(spectrum[i]));
		pitches[note] += (pitch * power);
		}

	for(note = c00k; note < b6k; note++) if(hits[note] > hits[maximum]) maximum = note;

	if(hits[maximum] == 0.0) return FREQUENCY_UNSTABLE;

	return pitches[maximum] / hits[maximum];
}

@end
