1/*
2 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
3 *
4 *  Use of this source code is governed by a BSD-style license
5 *  that can be found in the LICENSE file in the root of the source
6 *  tree. An additional intellectual property rights grant can be found
7 *  in the file PATENTS.  All contributing project authors may
8 *  be found in the AUTHORS file in the root of the source tree.
9 */
10
11#include <memory.h>
12#include <string.h>
13#ifdef WEBRTC_ANDROID
14#include <stdlib.h>
15#endif
16#include "pitch_estimator.h"
17#include "lpc_analysis.h"
18#include "codec.h"
19
20
21
22void WebRtcIsac_AllPoleFilter(double *InOut, double *Coef, int lengthInOut, int orderCoef){
23
24  /* the state of filter is assumed to be in InOut[-1] to InOut[-orderCoef] */
25  double scal;
26  double sum;
27  int n,k;
28
29  //if (fabs(Coef[0]-1.0)<0.001) {
30  if ( (Coef[0] > 0.9999) && (Coef[0] < 1.0001) )
31  {
32    for(n = 0; n < lengthInOut; n++)
33    {
34      sum = Coef[1] * InOut[-1];
35      for(k = 2; k <= orderCoef; k++){
36        sum += Coef[k] * InOut[-k];
37      }
38      *InOut++ -= sum;
39    }
40  }
41  else
42  {
43    scal = 1.0 / Coef[0];
44    for(n=0;n<lengthInOut;n++)
45    {
46      *InOut *= scal;
47      for(k=1;k<=orderCoef;k++){
48        *InOut -= scal*Coef[k]*InOut[-k];
49      }
50      InOut++;
51    }
52  }
53}
54
55
56void WebRtcIsac_AllZeroFilter(double *In, double *Coef, int lengthInOut, int orderCoef, double *Out){
57
58  /* the state of filter is assumed to be in In[-1] to In[-orderCoef] */
59
60  int n, k;
61  double tmp;
62
63  for(n = 0; n < lengthInOut; n++)
64  {
65    tmp = In[0] * Coef[0];
66
67    for(k = 1; k <= orderCoef; k++){
68      tmp += Coef[k] * In[-k];
69    }
70
71    *Out++ = tmp;
72    In++;
73  }
74}
75
76
77
78void WebRtcIsac_ZeroPoleFilter(double *In, double *ZeroCoef, double *PoleCoef, int lengthInOut, int orderCoef, double *Out){
79
80  /* the state of the zero section is assumed to be in In[-1] to In[-orderCoef] */
81  /* the state of the pole section is assumed to be in Out[-1] to Out[-orderCoef] */
82
83  WebRtcIsac_AllZeroFilter(In,ZeroCoef,lengthInOut,orderCoef,Out);
84  WebRtcIsac_AllPoleFilter(Out,PoleCoef,lengthInOut,orderCoef);
85}
86
87
88void WebRtcIsac_AutoCorr(
89    double *r,
90    const double *x,
91    int N,
92    int order
93                        )
94{
95  int  lag, n;
96  double sum, prod;
97  const double *x_lag;
98
99  for (lag = 0; lag <= order; lag++)
100  {
101    sum = 0.0f;
102    x_lag = &x[lag];
103    prod = x[0] * x_lag[0];
104    for (n = 1; n < N - lag; n++) {
105      sum += prod;
106      prod = x[n] * x_lag[n];
107    }
108    sum += prod;
109    r[lag] = sum;
110  }
111
112}
113
114
115void WebRtcIsac_BwExpand(double *out, double *in, double coef, short length) {
116  int i;
117  double  chirp;
118
119  chirp = coef;
120
121  out[0] = in[0];
122  for (i = 1; i < length; i++) {
123    out[i] = chirp * in[i];
124    chirp *= coef;
125  }
126}
127
128void WebRtcIsac_WeightingFilter(const double *in, double *weiout, double *whiout, WeightFiltstr *wfdata) {
129
130  double  tmpbuffer[PITCH_FRAME_LEN + PITCH_WLPCBUFLEN];
131  double  corr[PITCH_WLPCORDER+1], rc[PITCH_WLPCORDER+1];
132  double apol[PITCH_WLPCORDER+1], apolr[PITCH_WLPCORDER+1];
133  double  rho=0.9, *inp, *dp, *dp2;
134  double  whoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
135  double  weoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
136  double  *weo, *who, opol[PITCH_WLPCORDER+1], ext[PITCH_WLPCWINLEN];
137  int     k, n, endpos, start;
138
139  /* Set up buffer and states */
140  memcpy(tmpbuffer, wfdata->buffer, sizeof(double) * PITCH_WLPCBUFLEN);
141  memcpy(tmpbuffer+PITCH_WLPCBUFLEN, in, sizeof(double) * PITCH_FRAME_LEN);
142  memcpy(wfdata->buffer, tmpbuffer+PITCH_FRAME_LEN, sizeof(double) * PITCH_WLPCBUFLEN);
143
144  dp=weoutbuf;
145  dp2=whoutbuf;
146  for (k=0;k<PITCH_WLPCORDER;k++) {
147    *dp++ = wfdata->weostate[k];
148    *dp2++ = wfdata->whostate[k];
149    opol[k]=0.0;
150  }
151  opol[0]=1.0;
152  opol[PITCH_WLPCORDER]=0.0;
153  weo=dp;
154  who=dp2;
155
156  endpos=PITCH_WLPCBUFLEN + PITCH_SUBFRAME_LEN;
157  inp=tmpbuffer + PITCH_WLPCBUFLEN;
158
159  for (n=0; n<PITCH_SUBFRAMES; n++) {
160    /* Windowing */
161    start=endpos-PITCH_WLPCWINLEN;
162    for (k=0; k<PITCH_WLPCWINLEN; k++) {
163      ext[k]=wfdata->window[k]*tmpbuffer[start+k];
164    }
165
166    /* Get LPC polynomial */
167    WebRtcIsac_AutoCorr(corr, ext, PITCH_WLPCWINLEN, PITCH_WLPCORDER);
168    corr[0]=1.01*corr[0]+1.0; /* White noise correction */
169    WebRtcIsac_LevDurb(apol, rc, corr, PITCH_WLPCORDER);
170    WebRtcIsac_BwExpand(apolr, apol, rho, PITCH_WLPCORDER+1);
171
172    /* Filtering */
173    WebRtcIsac_ZeroPoleFilter(inp, apol, apolr, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, weo);
174    WebRtcIsac_ZeroPoleFilter(inp, apolr, opol, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, who);
175
176    inp+=PITCH_SUBFRAME_LEN;
177    endpos+=PITCH_SUBFRAME_LEN;
178    weo+=PITCH_SUBFRAME_LEN;
179    who+=PITCH_SUBFRAME_LEN;
180  }
181
182  /* Export filter states */
183  for (k=0;k<PITCH_WLPCORDER;k++) {
184    wfdata->weostate[k]=weoutbuf[PITCH_FRAME_LEN+k];
185    wfdata->whostate[k]=whoutbuf[PITCH_FRAME_LEN+k];
186  }
187
188  /* Export output data */
189  memcpy(weiout, weoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
190  memcpy(whiout, whoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
191}
192
193
194static const double APupper[ALLPASSSECTIONS] = {0.0347, 0.3826};
195static const double APlower[ALLPASSSECTIONS] = {0.1544, 0.744};
196
197
198
199void WebRtcIsac_AllpassFilterForDec(double *InOut,
200                                   const double *APSectionFactors,
201                                   int lengthInOut,
202                                   double *FilterState)
203{
204  //This performs all-pass filtering--a series of first order all-pass sections are used
205  //to filter the input in a cascade manner.
206  int n,j;
207  double temp;
208  for (j=0; j<ALLPASSSECTIONS; j++){
209    for (n=0;n<lengthInOut;n+=2){
210      temp = InOut[n]; //store input
211      InOut[n] = FilterState[j] + APSectionFactors[j]*temp;
212      FilterState[j] = -APSectionFactors[j]*InOut[n] + temp;
213    }
214  }
215}
216
217void WebRtcIsac_DecimateAllpass(const double *in,
218                                double *state_in,        /* array of size: 2*ALLPASSSECTIONS+1 */
219                                int N,                   /* number of input samples */
220                                double *out)             /* array of size N/2 */
221{
222  int n;
223  double data_vec[PITCH_FRAME_LEN];
224
225  /* copy input */
226  memcpy(data_vec+1, in, sizeof(double) * (N-1));
227
228  data_vec[0] = state_in[2*ALLPASSSECTIONS];   //the z^(-1) state
229  state_in[2*ALLPASSSECTIONS] = in[N-1];
230
231  WebRtcIsac_AllpassFilterForDec(data_vec+1, APupper, N, state_in);
232  WebRtcIsac_AllpassFilterForDec(data_vec, APlower, N, state_in+ALLPASSSECTIONS);
233
234  for (n=0;n<N/2;n++)
235    out[n] = data_vec[2*n] + data_vec[2*n+1];
236
237}
238
239
240
241/* create high-pass filter ocefficients
242 * z = 0.998 * exp(j*2*pi*35/8000);
243 * p = 0.94 * exp(j*2*pi*140/8000);
244 * HP_b = [1, -2*real(z), abs(z)^2];
245 * HP_a = [1, -2*real(p), abs(p)^2]; */
246static const double a_coef[2] = { 1.86864659625574, -0.88360000000000};
247static const double b_coef[2] = {-1.99524591718270,  0.99600400000000};
248static const float a_coef_float[2] = { 1.86864659625574f, -0.88360000000000f};
249static const float b_coef_float[2] = {-1.99524591718270f,  0.99600400000000f};
250
251/* second order high-pass filter */
252void WebRtcIsac_Highpass(const double *in, double *out, double *state, int N)
253{
254  int k;
255
256  for (k=0; k<N; k++) {
257    *out = *in + state[1];
258    state[1] = state[0] + b_coef[0] * *in + a_coef[0] * *out;
259    state[0] = b_coef[1] * *in++ + a_coef[1] * *out++;
260  }
261}
262
263void WebRtcIsac_Highpass_float(const float *in, double *out, double *state, int N)
264{
265  int k;
266
267  for (k=0; k<N; k++) {
268    *out = (double)*in + state[1];
269    state[1] = state[0] + b_coef_float[0] * *in + a_coef_float[0] * *out;
270    state[0] = b_coef_float[1] * (double)*in++ + a_coef_float[1] * *out++;
271  }
272}
273