/*
	$Id: eff.c,v 1.4 2000/12/30 01:24:28 rogue Exp $

	Copyright (c) 1999, 2000 Xforge project
*/

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

void clamp(struct buffer *buf) {
	int i, datasize;
	float s, *p;

	datasize = buf->len * buf->channels;
	p = buf->data;

	for (i = 0; i < datasize; i++) {
		s = *p;
		/* FIXME: should stand 1.0f, but overflows! */
		if (s > 0.9999f)
			s = 0.9999f;
		if (s < -0.9999f)
			s = -0.9999f;
		*p++ = s;
	}
}

void eff_volume(struct buffer *buf, float v1, float v2) {
	float v0, vs, *p;
	int i, j, sellen, mask = 0x1;

	if (undo_create(buf))
		return;

	sellen = buf->selend - buf->selstart + 1;

	/* handle the special case of length 1 selection */
	if (sellen == 1)
		vs = 0.0f;
	else
		vs = (v2 - v1) / (float)(sellen - 1);

	for (i = 0; i < buf->channels; i++) {
		if (mask & buf->selmask) {
			p = buf->data + buf->len * i + buf->selstart;
			v0 = v1;
			for (j = 0; j < sellen; j++) {
				*p++ *= v0;
				v0 += vs;
			}
		}
		mask = mask << 1;
	}

	clamp(buf);
	buf->mod = 1;
}

void eff_echo(struct buffer *buf, float gain, float feedback, float delay) {
	int idelay, i, j, mask = 0x1;
	float *p, *q, l;

	delay *= buf->rate;
	idelay = (int)delay;
	if (buf->len < idelay)
		return;

	if (!(p = malloc(idelay * sizeof(float)))) {
		xferr(EMEM_MSG);
		return;
	}

	if (undo_create(buf)) {
		free(p);
		return;
	}

	for (i = 0; i < buf->channels; i++) {
		q = buf->data + i * buf->len;
		if (mask & buf->selmask) {
			for (j = 0; j < idelay; j++)
				p[j] = *q++;
			for (; j < buf->len; j++) {
				l = p[j % idelay];
				p[j % idelay] = (l + *q) * feedback;
				*q++ += l * gain;
			}
		}
		mask = mask << 1;
	}

	free(p);
	clamp(buf);
	buf->mod = 1;
}

/*
	bsynth(buf, w1, w2, order, type, a, b)

	will synthesize IIR coefficients of order order butterworth filter
	of type type, cutoff frequencies w1 and w2 to a and b. Clear a and
	b to 0.0 before call.
*/

static void bsynth(struct buffer *buf, float w1, float w2, int order,
	int type, float *a, float *b) {
	/* prewarp frequencies */
	w1 = tan(M_PI * w1 / (float)buf->rate);
	w2 = tan(M_PI * w2 / (float)buf->rate);

	/* calculate IIR coefficients */
	switch (type) {
		case F_LP:
			switch (order) {
				case 1:
					a[0] = w1;
					a[1] = w1;
					b[0] = w1 + 1.0f;
					b[1] = 1.0f - w1;
					break;
				case 2:
					a[0] = w1 * w1;
					a[1] = 2.0f * w1*w1;
					a[2] = w1 * w1;
					b[0] = 1.0f + M_SQRT2 * w1 + w1*w1;
					b[1] = 2.0f * (1.0f - w1 * w1);
					b[2] = M_SQRT2 * w1 - 1.0f - w1*w1;
					break;
			}
			break;
		case F_HP:
			switch (order) {
				case 1:
					a[0] = 1.0f;
					a[1] = -1.0f;
					b[0] = w1 + 1.0f;
					b[1] = 1.0f - w1;
					break;
				case 2:
					a[0] = 1.0f;
					a[1] = -2.0f;
					a[2] = 1.0f;
					b[0] = 1.0f + M_SQRT2 * w1 + w1*w1;
					b[1] = 2.0f * (1.0f - w1 * w1);
					b[2] = M_SQRT2 * w1 - 1.0f - w1*w1;
					break;
			}
			break;
		case F_BP:
			switch (order) {
				case 1:
					a[0] = w2 - w1;
					a[2] = w1 - w2;
					b[0] = w1*w2 + w2 - w1 + 1.0f;
					b[1] = 2.0f * (1.0f - w1 * w2);
					b[2] = w2 - w1 - 1.0f - w1 * w2;
					break;
				case 2:
					a[0] = (w2 - w1) * (w2 - w1);
					a[2] = -2.0f * (w2 - w1) * (w2 - w1);
					a[4] = (w2 - w1) * (w2 - w1);
					b[0] = M_SQRT2 * (w2 - w1) *
						(1.0f + w1*w2) +
						w1*w1 * w2*w2 +
						w1*w1 + w2*w2 + 1.0f;
					b[1] = 4.0f * (1.0f - w1*w1 * w2*w2) -
						2.0f * M_SQRT2 *
						(w2 - w1) * (w1*w2 - 1.0f);
					b[2] = 2.0f * (w1*w1 + w2*w2) -
						6.0f * (w1*w1 * w2*w2 + 1.0f);
					b[3] = 4.0f * (1.0f - w1*w1 * w2*w2) -
						2.0f * M_SQRT2 *
						(w2 - w1) * (1.0f - w1*w2);
					b[4] = M_SQRT2 * (w2 - w1) *
						(1.0f + w1*w2) -
						w1*w1 * w2*w2 -
						w1*w1 - w2*w2 - 1.0f;
					break;
			}
			break;
		case F_BR:
			switch (order) {
				case 1:
					a[0] = w1*w2 + 1.0f;
					a[1] = 2.0f * (w2*w1 - 1.0f);
					a[2] = w1*w2 + 1.0f;
					b[0] = w1*w2 + w2 - w1 + 1.0f;
					b[1] = 2.0f * (1.0f - w1*w2);
					b[2] = w2 - w1 - 1.0f - w1 * w2;
					break;
				case 2:
					a[0] = w1*w1 * w2*w2 + 2.0f * w1*w2 +
						1.0f;
					a[1] = 4.0f * (w1*w1 * w2*w2 - 1.0f);
					a[2] = 6.0f * w1*w1 * w2*w2 -
						4.0f * w1*w2 + 6.0f;
					a[3] = 4.0f * (w1*w1 * w2*w2 - 1.0f);
					a[4] = w1*w1 * w2*w2 + 2.0f * w1*w2 +
						1.0f;
					b[0] = M_SQRT2 * (w2 - w1) *
						(1.0f + w1*w2) +
						w1*w1 * w2*w2 +
						w1*w1 + w2*w2 + 1.0f;
					b[1] = 4.0f * (1.0f - w1*w1 * w2*w2) -
						2.0f * M_SQRT2 *
						(w2 - w1) * (w1*w2 - 1.0f);
					b[2] = 2.0f * (w1*w1 + w2*w2) -
						6.0f * (w1*w1 * w2*w2 + 1.0f);
					b[3] = 4.0f * (1.0f - w1*w1 * w2*w2) -
						2.0f * M_SQRT2 *
						(w2 - w1) * (1.0f - w1*w2);
					b[4] = M_SQRT2 * (w2 - w1) *
						(1.0f + w1*w2) -
						w1*w1 * w2*w2 -
						w1*w1 - w2*w2 - 1.0f;
					break;
			}
			break;
	}
}

void eff_reverb(struct buffer *buf, float dry, float wet, float damp,
	float fb) {
	/* comb filters */
	float *c[8];			/* buffer pointers */
	int ci[8];			/* buffer indices */
	int cs[8];			/* buffer sizes */
	/* allpass filters */
	float *a[4];			/* buffer pointers */
	int ai[4];			/* buffer indices */
	int as[4];			/* buffer sizes */
	float afb = 0.5f;		/* feedback */
	float *fmem;
	float s, t, in, fs, *p, *q;
	int flen, i, j, k, sellen, mask = 0x1;

	/* these are freeverb default values scaled by sampling rate */
	cs[0] = 1116 * buf->rate / 44100;
	cs[1] = 1188 * buf->rate / 44100;
	cs[2] = 1277 * buf->rate / 44100;
	cs[3] = 1356 * buf->rate / 44100;
	cs[4] = 1422 * buf->rate / 44100;
	cs[5] = 1491 * buf->rate / 44100;
	cs[6] = 1557 * buf->rate / 44100;
	cs[7] = 1617 * buf->rate / 44100;
	as[0] = 556 * buf->rate / 44100;
	as[1] = 441 * buf->rate / 44100;
	as[2] = 341 * buf->rate / 44100;
	as[3] = 225 * buf->rate / 44100;

	flen = 0;
	for (i = 0; i < 8; i++)
		flen += cs[i];
	for (i = 0; i < 4; i++)
		flen += as[i];

	if (!(fmem = malloc(flen * sizeof(float)))) {
		xferr(EMEM_MSG);
		return;
	}

	j = 0;
	for (i = 0; i < 8; i++) {
		c[i] = fmem + j;
		j += cs[i];
	}
	for (i = 0; i < 4; i++) {
		a[i] = fmem + j;
		j += as[i];
	}

	if (undo_create(buf)) {
		free(fmem);
		return;
	}

	sellen = buf->selend - buf->selstart + 1;

	for (i = 0; i < buf->channels; i++) {
		if (mask & buf->selmask) {
			p = buf->data + buf->len * i + buf->selstart;
			/* clear the filters */
			for (j = 0; j < 8; j++)
				ci[j] = 0;
			for (j = 0; j < 4; j++)
				ai[j] = 0;
			for (j = 0; j < flen; j++)
				fmem[j] = 0.0f;
			for (j = 0; j < sellen; j++) {
				s = 0.0f;
				in = 0.015 * *p;
				/* process the comb filters in parallel */
				for (k = 0; k < 8; k++) {
					q = c[k] + ci[k];
					s += *q;
					fs = *q * (1.0f - damp) + in * damp;
					*q = in + fs * fb;
					if (++ci[k] == cs[k])
						ci[k] = 0;
				}
				/* process the allpass filters in series */
				for (k = 0; k < 4; k++) {
					q = a[k] + ai[k];
					t = s;
					s = *q - s;
					*q = t + afb * *q;
					if (++ai[k] == as[k])
						ai[k] = 0;
				}
				*p = dry * *p + wet * s;
				p++;
			}
		}
		mask = mask << 1;
	}

	free(fmem);
	clamp(buf);
	buf->mod = 1;
}

void eff_bfilter(struct buffer *buf, float w1, float w2, int order, int type) {
	float a[5], b[5];
	float x[4], y[4];		/* delay buffer */
	float s, *p;
	int i, j, sellen, mask = 0x1;

	if (undo_create(buf))
		return;

	/* initialize coefficients to zero */
	for (i = 0; i < 5; i++)
		a[i] = b[i] = 0.0f;

	bsynth(buf, w1, w2, order, type, a, b);

	/* calculate reciprocal of b[0] so we can multiply */
	b[0] = 1.0f / b[0];

	/* do the filtering */
	sellen = buf->selend - buf->selstart + 1;

	for (i = 0; i < buf->channels; i++) {
		if (mask & buf->selmask) {
			p = buf->data + buf->len * i + buf->selstart;
			x[0] = x[1] = x[2] = x[3] = 0.0f;
			y[0] = y[1] = y[2] = y[3] = 0.0f;
			for (j = 0; j < sellen; j++) {
				s = a[0]*(*p) + a[1]*x[0] + a[2]*x[1] +
					a[3]*x[2] + a[4]*x[3] + b[1]*y[0] +
					b[2]*y[1] + b[3]*y[2] + b[4]*y[3];
				s *= b[0];
				x[3] = x[2];
				x[2] = x[1];
				x[1] = x[0];
				x[0] = *p;
				y[3] = y[2];
				y[2] = y[1];
				y[1] = y[0];
				y[0] = s;
				*p++ = s;
			}
		}
		mask = mask << 1;
	}

	clamp(buf);
	buf->mod = 1;
}

void eff_compress(struct buffer *buf, float dynamics, float velocity) {
	float s;				/* lpf output */
	float x[2], y[2];			/* lpf memory */
	float a[3], b[3];			/* lpf coefficients */
	float *p, h;
	int sellen, i, j, mask = 0x1;

	if (undo_create(buf))
		return;

	sellen = buf->selend - buf->selstart + 1;

	/* generate second order low pass filter */
	for (i = 0; i < 3; i++)
		a[i] = b[i] = 0.0f;
	bsynth(buf, velocity, 0.0f, 2, F_LP, a, b);

	/* calculate reciprocal of b[0] so we can multiply */
	b[0] = 1.0f / b[0];

	for (i = 0; i < buf->channels; i++) {
		if (mask & buf->selmask) {
			p = buf->data + buf->len * i + buf->selstart;
			x[0] = x[1] = y[0] = y[1] = 0.5f;
			for (j = 0; j < sellen; j++) {
				s = a[0]*fabs(*p) + a[1]*x[0] + a[2]*x[1] +
					b[1]*y[0] + b[2]*y[1];
				s *= b[0];
				x[1] = x[0];
				x[0] = fabs(*p);
				y[1] = y[0];
				y[0] = s;
				h = s + s;
				/* FIXME: noise gate hack, consider */
				/* selectable */
				if (h < 0.001f)
					*p++ = 0.0f;
				else
					*p++ *= pow(h, dynamics);
			}
		}
		mask = mask << 1;
	}

	clamp(buf);
	buf->mod = 1;
}

void eff_flange(struct buffer *buf, float delay, float depth, float freq) {
	int sellen, i, j, mask = 0x1;
	float phase, phinc;
	float *p, s, s1, s2;
	float d, w;

	if (undo_create(buf))
		return;

	sellen = buf->selend - buf->selstart + 1;

	phinc = 2.0f * M_PI * freq / (float)(buf->rate);
	/* convert delay and depth ms -> samples */
	delay = delay * (float)(buf->rate) / 1000.0f;
	depth = depth * (float)(buf->rate) / 1000.0f;

	for (i = 0; i < buf->channels; i++) {
		if (mask & buf->selmask) {
			p = buf->data + buf->len * i + buf->selstart +
				(int)(delay + depth) + 1;
			phase = 0.0f;
			for (j = (int)(delay + depth) + 1; j < sellen; j++) {
				phase += phinc;
				d = delay + depth * sin(phase);
				s = 0.5f * *p;
				s1 = *(p - (int)d);
				s2 = *(p - (int)(d + 1));
				w = d - floor(d);
				s += 0.5f * ((1.0f - w) * s1 + w * s2);
				*p++ = s;
			}
		}
		mask = mask << 1;
	}

	clamp(buf);
	buf->mod = 1;
}
