1/* Copyright (C) 2006-2008 CSIRO, Jean-Marc Valin, Xiph.Org Foundation
2
3   File: scal.c
4   Shaped comb-allpass filter for channel decorrelation
5
6   Redistribution and use in source and binary forms, with or without
7   modification, are permitted provided that the following conditions are
8   met:
9
10   1. Redistributions of source code must retain the above copyright notice,
11   this list of conditions and the following disclaimer.
12
13   2. Redistributions in binary form must reproduce the above copyright
14   notice, this list of conditions and the following disclaimer in the
15   documentation and/or other materials provided with the distribution.
16
17   3. The name of the author may not be used to endorse or promote products
18   derived from this software without specific prior written permission.
19
20   THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
21   IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
22   OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23   DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
24   INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27   HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28   STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30   POSSIBILITY OF SUCH DAMAGE.
31*/
32
33/*
34The algorithm implemented here is described in:
35
36* J.-M. Valin, Perceptually-Motivated Nonlinear Channel Decorrelation For
37  Stereo Acoustic Echo Cancellation, Accepted for Joint Workshop on
38  Hands­free Speech Communication and Microphone Arrays (HSCMA), 2008.
39  http://people.xiph.org/~jm/papers/valin_hscma2008.pdf
40
41*/
42
43#ifdef HAVE_CONFIG_H
44#include "config.h"
45#endif
46
47#include "speex/speex_echo.h"
48#include "vorbis_psy.h"
49#include "arch.h"
50#include "os_support.h"
51#include "smallft.h"
52#include <math.h>
53#include <stdlib.h>
54
55#ifndef M_PI
56#define M_PI           3.14159265358979323846  /* pi */
57#endif
58
59#define ALLPASS_ORDER 20
60
61struct SpeexDecorrState_ {
62   int rate;
63   int channels;
64   int frame_size;
65#ifdef VORBIS_PSYCHO
66   VorbisPsy *psy;
67   struct drft_lookup lookup;
68   float *wola_mem;
69   float *curve;
70#endif
71   float *vorbis_win;
72   int    seed;
73   float *y;
74
75   /* Per-channel stuff */
76   float *buff;
77   float (*ring)[ALLPASS_ORDER];
78   int *ringID;
79   int *order;
80   float *alpha;
81};
82
83
84
85EXPORT SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
86{
87   int i, ch;
88   SpeexDecorrState *st = speex_alloc(sizeof(SpeexDecorrState));
89   st->rate = rate;
90   st->channels = channels;
91   st->frame_size = frame_size;
92#ifdef VORBIS_PSYCHO
93   st->psy = vorbis_psy_init(rate, 2*frame_size);
94   spx_drft_init(&st->lookup, 2*frame_size);
95   st->wola_mem = speex_alloc(frame_size*sizeof(float));
96   st->curve = speex_alloc(frame_size*sizeof(float));
97#endif
98   st->y = speex_alloc(frame_size*sizeof(float));
99
100   st->buff = speex_alloc(channels*2*frame_size*sizeof(float));
101   st->ringID = speex_alloc(channels*sizeof(int));
102   st->order = speex_alloc(channels*sizeof(int));
103   st->alpha = speex_alloc(channels*sizeof(float));
104   st->ring = speex_alloc(channels*ALLPASS_ORDER*sizeof(float));
105
106   /*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
107   st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
108   for (i=0;i<2*frame_size;i++)
109      st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
110   st->seed = rand();
111
112   for (ch=0;ch<channels;ch++)
113   {
114      for (i=0;i<ALLPASS_ORDER;i++)
115         st->ring[ch][i] = 0;
116      st->ringID[ch] = 0;
117      st->alpha[ch] = 0;
118      st->order[ch] = 10;
119   }
120   return st;
121}
122
123static float uni_rand(int *seed)
124{
125   const unsigned int jflone = 0x3f800000;
126   const unsigned int jflmsk = 0x007fffff;
127   union {int i; float f;} ran;
128   *seed = 1664525 * *seed + 1013904223;
129   ran.i = jflone | (jflmsk & *seed);
130   ran.f -= 1.5;
131   return 2*ran.f;
132}
133
134static unsigned int irand(int *seed)
135{
136   *seed = 1664525 * *seed + 1013904223;
137   return ((unsigned int)*seed)>>16;
138}
139
140
141EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
142{
143   int ch;
144   float amount;
145
146   if (strength<0)
147      strength = 0;
148   if (strength>100)
149      strength = 100;
150
151   amount = .01*strength;
152   for (ch=0;ch<st->channels;ch++)
153   {
154      int i;
155      int N=2*st->frame_size;
156      float beta, beta2;
157      float *x;
158      float max_alpha = 0;
159
160      float *buff;
161      float *ring;
162      int ringID;
163      int order;
164      float alpha;
165
166      buff = st->buff+ch*2*st->frame_size;
167      ring = st->ring[ch];
168      ringID = st->ringID[ch];
169      order = st->order[ch];
170      alpha = st->alpha[ch];
171
172      for (i=0;i<st->frame_size;i++)
173         buff[i] = buff[i+st->frame_size];
174      for (i=0;i<st->frame_size;i++)
175         buff[i+st->frame_size] = in[i*st->channels+ch];
176
177      x = buff+st->frame_size;
178
179      if (amount>1)
180         beta = 1-sqrt(.4*amount);
181      else
182         beta = 1-0.63246*amount;
183      if (beta<0)
184         beta = 0;
185
186      beta2 = beta;
187      for (i=0;i<st->frame_size;i++)
188      {
189         st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
190               + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
191               - alpha*(ring[ringID]
192               - beta*ring[ringID+1>=order?0:ringID+1]);
193         ring[ringID++]=st->y[i];
194         st->y[i] *= st->vorbis_win[st->frame_size+i];
195         if (ringID>=order)
196            ringID=0;
197      }
198      order = order+(irand(&st->seed)%3)-1;
199      if (order < 5)
200         order = 5;
201      if (order > 10)
202         order = 10;
203      /*order = 5+(irand(&st->seed)%6);*/
204      max_alpha = pow(.96+.04*(amount-1),order);
205      if (max_alpha > .98/(1.+beta2))
206         max_alpha = .98/(1.+beta2);
207
208      alpha = alpha + .4*uni_rand(&st->seed);
209      if (alpha > max_alpha)
210         alpha = max_alpha;
211      if (alpha < -max_alpha)
212         alpha = -max_alpha;
213      for (i=0;i<ALLPASS_ORDER;i++)
214         ring[i] = 0;
215      ringID = 0;
216      for (i=0;i<st->frame_size;i++)
217      {
218         float tmp =  alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
219               + x[i-ALLPASS_ORDER]*st->vorbis_win[i]
220               - alpha*(ring[ringID]
221               - beta*ring[ringID+1>=order?0:ringID+1]);
222         ring[ringID++]=tmp;
223         tmp *= st->vorbis_win[i];
224         if (ringID>=order)
225            ringID=0;
226         st->y[i] += tmp;
227      }
228
229#ifdef VORBIS_PSYCHO
230      float frame[N];
231      float scale = 1./N;
232      for (i=0;i<2*st->frame_size;i++)
233         frame[i] = buff[i];
234   //float coef = .5*0.78130;
235      float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
236      compute_curve(st->psy, buff, st->curve);
237      for (i=1;i<st->frame_size;i++)
238      {
239         float x1,x2;
240         float gain;
241         do {
242            x1 = uni_rand(&st->seed);
243            x2 = uni_rand(&st->seed);
244         } while (x1*x1+x2*x2 > 1.);
245         gain = coef*sqrt(.1+st->curve[i]);
246         frame[2*i-1] = gain*x1;
247         frame[2*i] = gain*x2;
248      }
249      frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
250      frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
251      spx_drft_backward(&st->lookup,frame);
252      for (i=0;i<2*st->frame_size;i++)
253         frame[i] *= st->vorbis_win[i];
254#endif
255
256      for (i=0;i<st->frame_size;i++)
257      {
258#ifdef VORBIS_PSYCHO
259         float tmp = st->y[i] + frame[i] + st->wola_mem[i];
260         st->wola_mem[i] = frame[i+st->frame_size];
261#else
262         float tmp = st->y[i];
263#endif
264         if (tmp>32767)
265            tmp = 32767;
266         if (tmp < -32767)
267            tmp = -32767;
268         out[i*st->channels+ch] = tmp;
269      }
270
271      st->ringID[ch] = ringID;
272      st->order[ch] = order;
273      st->alpha[ch] = alpha;
274
275   }
276}
277
278EXPORT void speex_decorrelate_destroy(SpeexDecorrState *st)
279{
280#ifdef VORBIS_PSYCHO
281   vorbis_psy_destroy(st->psy);
282   speex_free(st->wola_mem);
283   speex_free(st->curve);
284#endif
285   speex_free(st->buff);
286   speex_free(st->ring);
287   speex_free(st->ringID);
288   speex_free(st->alpha);
289   speex_free(st->vorbis_win);
290   speex_free(st->order);
291   speex_free(st->y);
292   speex_free(st);
293}
294