1/*
2 *  Bandpass filter sweep effect
3 *  Copyright (c) Maarten de Boer <mdeboer@iua.upf.es>
4 *
5 *   This program is free software; you can redistribute it and/or modify
6 *   it under the terms of the GNU General Public License as published by
7 *   the Free Software Foundation; either version 2 of the License, or
8 *   (at your option) any later version.
9 *
10 *   This program is distributed in the hope that it will be useful,
11 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
12 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 *   GNU General Public License for more details.
14 *
15 *   You should have received a copy of the GNU General Public License
16 *   along with this program; if not, write to the Free Software
17 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
18 *
19 */
20
21#include "aconfig.h"
22#include <math.h>
23#include <alsa/asoundlib.h>
24
25struct effect_private {
26	/* filter the sweep variables */
27	float lfo,dlfo,fs,fc,BW,C,D,a0,a1,a2,b1,b2,*x[3],*y[3];
28	float lfo_depth, lfo_center;
29	unsigned int channels;
30};
31
32static int effect_init(struct lookback *loopback,
33		       void *private_data,
34		       snd_pcm_access_t access,
35		       unsigned int channels,
36		       unsigned int rate,
37		       snd_pcm_format_t format)
38{
39	struct effect_private *priv = private_data;
40	int i;
41
42#if __BYTE_ORDER == __LITTLE_ENDIAN
43	if (format != SND_PCM_FORMAT_S16_LE)
44		return -EIO;
45#elif __BYTE_ORDER == __BIG_ENDIAN
46	if (format != SND_PCM_FORMAT_S16_BE)
47		return -EIO;
48#else
49	return -EIO;
50#endif
51	priv->fs = (float) rate;
52	priv->channels = channels;
53	for (i = 0; i < 3; i++) {
54		priv->x[i] = calloc(channels * sizeof(float));
55		priv->y[i] = calloc(channels * sizeof(float));
56	}
57	return 0;
58}
59
60static int effect_done(struct loopback *loopback,
61		       void *private_data)
62{
63	struct effect_private *priv = private_data;
64	int i;
65
66	for (i = 0; i < 3; i++) {
67		free(priv->x[i]);
68		free(priv->y[i]);
69	}
70	return 0;
71}
72
73static int effect_apply(struct loopback *loopback,
74			void *private_data,
75			const snd_pcm_channel_area_t *areas,
76			snd_uframes_t offset,
77			snd_uframes_t frames)
78{
79	struct effect_private *priv = private_data;
80	short *samples = (short*)areas[0].addr + offset*priv->channels;
81	snd_uframes_t i;
82
83	for (i=0; i < frames; i++) {
84		int chn;
85
86		fc = sin(priv->lfo)*priv->lfo_depth+priv->lfo_center;
87		priv->lfo += priv->dlfo;
88		if (priv->lfo>2.*M_PI) priv->lfo -= 2.*M_PI;
89		priv->C = 1./tan(M_PI*priv->BW/priv->fs);
90		priv->D = 2.*cos(2*M_PI*fc/fs);
91		priv->a0 = 1./(1.+priv->C);
92		priv->a1 = 0;
93		priv->a2 = -priv->a0;
94		priv->b1 = -priv->C*priv->D*a0;
95		priv->b2 = (priv->C-1)*priv->a0;
96
97		for (chn=0; chn < priv->channels; chn++)
98		{
99			priv->x[chn][2] = priv->x[chn][1];
100			priv->x[chn][1] = priv->x[chn][0];
101
102			priv->y[chn][2] = priv->y[chn][1];
103			priv->y[chn][1] = priv->y[chn][0];
104
105			priv->x[chn][0] = samples[i*channels+chn];
106			priv->y[chn][0] = priv->a0*priv->x[0][chn]
107				+ priv->a1*priv->x[1][chn] + priv->a2*x[2][chn]
108				- priv->b1*priv->y[1][chn] - priv->b2*y[2][chn];
109			samples[i*channels+chn] = priv->y[chn][0];
110		}
111	}
112	return 0;
113}
114
115void effect_init_sweep(void)
116{
117	struct effect_private *priv;
118
119	priv = register_effect(effect_init,
120			       effect_apply,
121			       effect_done,
122			       sizeof(struct effectprivate));
123	if (priv) {
124		priv->lfo_center = 2000.;
125		priv->lfo_depth = 1800.;
126		priv->lfo_freq = 0.2;
127		priv->BW = 50;
128	}
129}
130