/*
	$Id: resample.c,v 1.4 2001/09/03 18:53:43 rogue Exp $

	Copyright (c) 1999, 2000 Xforge project
*/

#include <Xm/Xm.h>
#include <math.h>
#include <stdlib.h>
#include "xforge.h"
#include "bufutil.h"
#include "glob.h"

static void window(float *p, int m, int wfunc) {
	int i;

	switch (wfunc) {
		case WFUNC_RECTANGULAR:
			for (i = 0; i < m; i++)
				p[i] = 1.0f;
			break;
		case WFUNC_BARTLETT:
			for (i = 0; i < m; i++)
				p[i] = 1.0f - 2.0f * (float)abs(i - (m - 1)/2)
					/ (float)(m - 1);
			break;
		case WFUNC_BLACKMAN:
			for (i = 0; i < m; i++)
				p[i] = 0.42f - 0.5f * cos((2.0f * M_PI * i) /
					(float)(m - 1)) + 0.08f *
					cos((4.0f * M_PI * i) /
					(float)(m - 1));
			break;
		case WFUNC_HAMMING:
			for (i = 0; i < m; i++)
				p[i] = 0.54f - 0.46 * cos((2.0f * M_PI * i) /
					(float)(m - 1));
			break;
		case WFUNC_HANNING:
			for (i = 0; i < m; i++)
				p[i] = 0.5f * (1.0f - cos((2.0f * M_PI * i) /
					(float)(m - 1)));
			break;
	}
}

/*
	sinc(x)

	calculates the sinc function of x
*/

static float sinc(float x) {
	/* to avoid 0/0 divisions, we make a small discontinuity to our */
	/* sinc() implementation. You should not be able to hear this. :-) */
	if (fabs(x) < 0.0001f)
		return 1.0f;
	else
		return (float)(sin(x) / x);
}

void resample(struct buffer *buf, int newrate, int wlen, int wfunc) {
	float *w, *p, *q, *newdata, *start, *stop;
	float s, offset;
	float rc, ph;
	int newlen, i, j, ch;

	if (!(buf->data))
		return;

	if (wlen < 1) {
		xferr("Resampling window must be at least 1 sample. Aborted.");
		return;
	}

	newlen = (int)(((float)newrate / (float)buf->rate) * (float)buf->len);

	if (!(w = malloc(wlen * sizeof(float)))) {
		syserr(EMEM_MSG);
		return;
	}

	if (!(newdata = malloc(buf->channels * newlen * sizeof(float)))) {
		syserr(EMEM_MSG);
		free(w);
		return;
	}

	if (undo_create(buf)) {
		free(w);
		free(newdata);
		return;
	}

	window(w, wlen, wfunc);

	/* relative sample cycle length */
	rc = (float)buf->rate / (float)newrate;

	for (ch = 0; ch < buf->channels; ch++) {
		/* phase in old time */
		ph = 0.0f;
		q = newdata + newlen * ch;
		start = buf->data + buf->len * ch;
		stop = buf->data + buf->len * ch + buf->len - 1;
		for (i = 0; i < newlen; i++) {
			s = 0;
			offset = ph - floor(ph + 0.5f) - (float)(wlen - 1) /
				2.0f;
			p = &(buf->data[buf->len * ch + (int)(ph + offset)]);
			for (j = 0; j < wlen; j++) {
				if (p >= start && p < stop)
					s += w[j] * sinc(offset * M_PI) * *p;
				p++;
				offset += 1.0f;
			}
			*q++ = s;
			ph += rc;
		}
	}

	free(buf->data);
	buf->data = newdata;
	buf->rate = newrate;
	buf->len = newlen;
	/* just in case, clamp the signal */
	clamp(buf);
	free(w);
	buf->mod = 1;
}
