#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "sfft.h"
#include "misc.h"

#define	STABCOEFF	0.9999

/* ---------------------------------------------------------------------- */

struct sfft *init_sfft(int len, int first, int last)
{
	struct sfft *s;
	int i;

	if ((s = calloc(1, sizeof(struct sfft))) == NULL)
		return NULL;

	s->twiddles = calloc(len, sizeof(complex));
	s->history = calloc(len, sizeof(complex));
	s->bins = calloc(len, sizeof(complex));

	if (!s->twiddles || !s->history || !s->bins) {
		clear_sfft(s);
		return NULL;
	}

	s->fftlen = len;
	s->first = first;
	s->last = last;

	for (i = 0; i < len; i++) {
		s->twiddles[i].re = cos(i * 2.0 * M_PI / len) * STABCOEFF;
		s->twiddles[i].im = sin(i * 2.0 * M_PI / len) * STABCOEFF;
	}

	s->corr = pow(STABCOEFF, len);

	return s;
}

void clear_sfft(struct sfft *s)
{
	if (s) {
		free(s->twiddles);
		free(s->history);
		free(s->bins);
		free(s);
	}
}

/*
 * Sliding FFT, complex input, complex output
 */
complex *sfft(struct sfft *s, complex new)
{
	complex old, z;
	int i;

	/* restore the sample fftlen samples back */
	old = s->history[s->ptr];
	old.re *= s->corr;
	old.im *= s->corr;

	/* save the new sample */
	s->history[s->ptr] = new;

	/* advance the history pointer */
	s->ptr = (s->ptr + 1) % s->fftlen;

	/* calculate the wanted bins */
	for (i = s->first; i < s->last; i++) {
		z = s->bins[i];
		z = csub(z, old);
		z = cadd(z, new);
		s->bins[i] = cmul(z, s->twiddles[i]);
	}

	return s->bins;
}

/* ---------------------------------------------------------------------- */

