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/*
12 * lattice.c
13 *
14 * contains the normalized lattice filter routines (MA and AR) for iSAC codec
15 *
16 */
17#include "settings.h"
18#include "codec.h"
19
20#include <math.h>
21#include <memory.h>
22#include <string.h>
23#ifdef WEBRTC_ANDROID
24#include <stdlib.h>
25#endif
26
27/* filter the signal using normalized lattice filter */
28/* MA filter */
29void WebRtcIsac_NormLatticeFilterMa(int orderCoef,
30                                     float *stateF,
31                                     float *stateG,
32                                     float *lat_in,
33                                     double *filtcoeflo,
34                                     double *lat_out)
35{
36  int n,k,i,u,temp1;
37  int ord_1 = orderCoef+1;
38  float sth[MAX_AR_MODEL_ORDER];
39  float cth[MAX_AR_MODEL_ORDER];
40  float inv_cth[MAX_AR_MODEL_ORDER];
41  double a[MAX_AR_MODEL_ORDER+1];
42  float f[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN], g[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN];
43  float gain1;
44
45  for (u=0;u<SUBFRAMES;u++)
46  {
47    /* set the Direct Form coefficients */
48    temp1 = u*ord_1;
49    a[0] = 1;
50    memcpy(a+1, filtcoeflo+temp1+1, sizeof(double) * (ord_1-1));
51
52    /* compute lattice filter coefficients */
53    WebRtcIsac_Dir2Lat(a,orderCoef,sth,cth);
54
55    /* compute the gain */
56    gain1 = (float)filtcoeflo[temp1];
57    for (k=0;k<orderCoef;k++)
58    {
59      gain1 *= cth[k];
60      inv_cth[k] = 1/cth[k];
61    }
62
63    /* normalized lattice filter */
64    /*****************************/
65
66    /* initial conditions */
67    for (i=0;i<HALF_SUBFRAMELEN;i++)
68    {
69      f[0][i] = lat_in[i + u * HALF_SUBFRAMELEN];
70      g[0][i] = lat_in[i + u * HALF_SUBFRAMELEN];
71    }
72
73    /* get the state of f&g for the first input, for all orders */
74    for (i=1;i<ord_1;i++)
75    {
76      f[i][0] = inv_cth[i-1]*(f[i-1][0] + sth[i-1]*stateG[i-1]);
77      g[i][0] = cth[i-1]*stateG[i-1] + sth[i-1]* f[i][0];
78    }
79
80    /* filtering */
81    for(k=0;k<orderCoef;k++)
82    {
83      for(n=0;n<(HALF_SUBFRAMELEN-1);n++)
84      {
85        f[k+1][n+1] = inv_cth[k]*(f[k][n+1] + sth[k]*g[k][n]);
86        g[k+1][n+1] = cth[k]*g[k][n] + sth[k]* f[k+1][n+1];
87      }
88    }
89
90    for(n=0;n<HALF_SUBFRAMELEN;n++)
91    {
92      lat_out[n + u * HALF_SUBFRAMELEN] = gain1 * f[orderCoef][n];
93    }
94
95    /* save the states */
96    for (i=0;i<ord_1;i++)
97    {
98      stateF[i] = f[i][HALF_SUBFRAMELEN-1];
99      stateG[i] = g[i][HALF_SUBFRAMELEN-1];
100    }
101    /* process next frame */
102  }
103
104  return;
105}
106
107
108/*///////////////////AR filter ///////////////////////////////*/
109/* filter the signal using normalized lattice filter */
110void WebRtcIsac_NormLatticeFilterAr(int orderCoef,
111                                     float *stateF,
112                                     float *stateG,
113                                     double *lat_in,
114                                     double *lo_filt_coef,
115                                     float *lat_out)
116{
117  int n,k,i,u,temp1;
118  int ord_1 = orderCoef+1;
119  float sth[MAX_AR_MODEL_ORDER];
120  float cth[MAX_AR_MODEL_ORDER];
121  double a[MAX_AR_MODEL_ORDER+1];
122  float ARf[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN], ARg[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN];
123  float gain1,inv_gain1;
124
125  for (u=0;u<SUBFRAMES;u++)
126  {
127    /* set the denominator and numerator of the Direct Form */
128    temp1 = u*ord_1;
129    a[0] = 1;
130
131    memcpy(a+1, lo_filt_coef+temp1+1, sizeof(double) * (ord_1-1));
132
133    WebRtcIsac_Dir2Lat(a,orderCoef,sth,cth);
134
135    gain1 = (float)lo_filt_coef[temp1];
136    for (k=0;k<orderCoef;k++)
137    {
138      gain1 = cth[k]*gain1;
139    }
140
141    /* initial conditions */
142    inv_gain1 = 1/gain1;
143    for (i=0;i<HALF_SUBFRAMELEN;i++)
144    {
145      ARf[orderCoef][i] = (float)lat_in[i + u * HALF_SUBFRAMELEN]*inv_gain1;
146    }
147
148
149    for (i=orderCoef-1;i>=0;i--) //get the state of f&g for the first input, for all orders
150    {
151      ARf[i][0] = cth[i]*ARf[i+1][0] - sth[i]*stateG[i];
152      ARg[i+1][0] = sth[i]*ARf[i+1][0] + cth[i]* stateG[i];
153    }
154    ARg[0][0] = ARf[0][0];
155
156    for(n=0;n<(HALF_SUBFRAMELEN-1);n++)
157    {
158      for(k=orderCoef-1;k>=0;k--)
159      {
160        ARf[k][n+1] = cth[k]*ARf[k+1][n+1] - sth[k]*ARg[k][n];
161        ARg[k+1][n+1] = sth[k]*ARf[k+1][n+1] + cth[k]* ARg[k][n];
162      }
163      ARg[0][n+1] = ARf[0][n+1];
164    }
165
166    memcpy(lat_out+u * HALF_SUBFRAMELEN, &(ARf[0][0]), sizeof(float) * HALF_SUBFRAMELEN);
167
168    /* cannot use memcpy in the following */
169    for (i=0;i<ord_1;i++)
170    {
171      stateF[i] = ARf[i][HALF_SUBFRAMELEN-1];
172      stateG[i] = ARg[i][HALF_SUBFRAMELEN-1];
173    }
174
175  }
176
177  return;
178}
179
180
181/* compute the reflection coefficients using the step-down procedure*/
182/* converts the direct form parameters to lattice form.*/
183/* a and b are vectors which contain the direct form coefficients,
184   according to
185   A(z) = a(1) + a(2)*z + a(3)*z^2 + ... + a(M+1)*z^M
186   B(z) = b(1) + b(2)*z + b(3)*z^2 + ... + b(M+1)*z^M
187*/
188
189void WebRtcIsac_Dir2Lat(double *a,
190                        int orderCoef,
191                        float *sth,
192                        float *cth)
193{
194  int m, k;
195  float tmp[MAX_AR_MODEL_ORDER];
196  float tmp_inv, cth2;
197
198  sth[orderCoef-1] = (float)a[orderCoef];
199  cth2 = 1.0f - sth[orderCoef-1] * sth[orderCoef-1];
200  cth[orderCoef-1] = (float)sqrt(cth2);
201  for (m=orderCoef-1; m>0; m--)
202  {
203    tmp_inv = 1.0f / cth2;
204    for (k=1; k<=m; k++)
205    {
206      tmp[k] = ((float)a[k] - sth[m] * (float)a[m-k+1]) * tmp_inv;
207    }
208
209    for (k=1; k<m; k++)
210    {
211      a[k] = tmp[k];
212    }
213
214    sth[m-1] = tmp[m];
215    cth2 = 1 - sth[m-1] * sth[m-1];
216    cth[m-1] = (float)sqrt(cth2);
217  }
218}
219