Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : /*====================================================================================
34 : EVS Codec 3GPP TS26.443 Nov 04, 2021. Version 12.14.0 / 13.10.0 / 14.6.0 / 15.4.0 / 16.3.0
35 : ====================================================================================*/
36 :
37 : #include <stdint.h>
38 : #include "options.h"
39 : #include <math.h>
40 : #include "cnst.h"
41 : #include "prot.h"
42 : #include "rom_com.h"
43 : #include "wmc_auto.h"
44 :
45 : /*-------------------------------------------------------------------*
46 : * Local constants
47 : *-------------------------------------------------------------------*/
48 :
49 : #define WARP_OS_RATE 8
50 : #define LL 256
51 : #define LL_OS ( WARP_OS_RATE * LL )
52 : #define OSLENGTH 12
53 : #define WI_THRESHLD 0.8
54 : #define WI_SAMPLE_THLD 20
55 : #define ERB_CBSIZE1 64
56 : #define ERB_CBSIZE2 64
57 :
58 : #define _POLY1( x, c ) ( ( c )[0] * ( x ) + ( c )[1] )
59 : #define _POLY2( x, c ) ( _POLY1( ( x ), ( c ) ) * ( x ) + ( c )[2] )
60 : #define _POLY3( x, c ) ( _POLY2( ( x ), ( c ) ) * ( x ) + ( c )[3] )
61 :
62 :
63 : /*-------------------------------------------------------------------*
64 : * DTFS_new()
65 : *
66 : * DTFS structure initialization.
67 : *-------------------------------------------------------------------*/
68 :
69 : /*! r: DTFS structure */
70 0 : ivas_error DTFS_new(
71 : DTFS_STRUCTURE **dtfs_out )
72 : {
73 : int16_t i;
74 0 : DTFS_STRUCTURE *dtfs = NULL;
75 :
76 0 : if ( ( dtfs = (DTFS_STRUCTURE *) malloc( sizeof( DTFS_STRUCTURE ) ) ) == NULL )
77 : {
78 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for DTFS (SC-VBR) structure\n" ) );
79 : }
80 :
81 : #define WMC_TOOL_SKIP
82 : MOVE( 2 );
83 : LOOP( 1 );
84 : MOVE( 2 );
85 0 : dtfs->lag = 0;
86 0 : dtfs->nH = 0;
87 0 : dtfs->nH_4kHz = 0;
88 0 : dtfs->upper_cut_off_freq_of_interest = 3300.0;
89 0 : dtfs->upper_cut_off_freq = 4000.0;
90 :
91 0 : for ( i = 0; i < MAXLAG_WI; i++ )
92 : {
93 0 : dtfs->a[i] = dtfs->b[i] = 0.0;
94 : }
95 :
96 0 : dtfs->sampling_rate = -1;
97 :
98 0 : *dtfs_out = dtfs;
99 : #undef WMC_TOOL_SKIP
100 :
101 0 : return IVAS_ERR_OK;
102 : }
103 :
104 :
105 : /*-------------------------------------------------------------------*
106 : * DTFS_copy()
107 : *
108 : * Copy from one DTFS STRUCTURE to another.
109 : *-------------------------------------------------------------------*/
110 :
111 0 : void DTFS_copy(
112 : DTFS_STRUCTURE *Xout, /* o : DTFS structure */
113 : DTFS_STRUCTURE Xinp /* i : DTFS structure */
114 : )
115 : {
116 : int16_t k;
117 :
118 : #define WMC_TOOL_SKIP
119 : LOOP( 1 );
120 0 : for ( k = 0; k < MAXLAG_WI; k++ )
121 : {
122 : MOVE( 1 );
123 0 : Xout->a[k] = Xinp.a[k];
124 : }
125 : LOOP( 1 );
126 0 : for ( k = 0; k < MAXLAG_WI; k++ )
127 : {
128 : MOVE( 1 );
129 0 : Xout->b[k] = Xinp.b[k];
130 : }
131 : MOVE( 1 );
132 0 : Xout->lag = Xinp.lag;
133 0 : Xout->nH = Xinp.nH;
134 0 : Xout->nH_4kHz = Xinp.nH_4kHz;
135 0 : Xout->upper_cut_off_freq_of_interest = Xinp.upper_cut_off_freq_of_interest;
136 0 : Xout->upper_cut_off_freq = Xinp.upper_cut_off_freq;
137 : #undef WMC_TOOL_SKIP
138 :
139 0 : return;
140 : }
141 :
142 :
143 : /*-------------------------------------------------------------------*
144 : * DTFS_sub()
145 : *
146 : * Difference of A and B coefficients in cartesian domain.
147 : * Equivalent to time domain subtraction.
148 : *-------------------------------------------------------------------*/
149 :
150 0 : void DTFS_sub(
151 : DTFS_STRUCTURE *tmp, /* o : output DFTS */
152 : DTFS_STRUCTURE X1, /* i : DTFS input 1 */
153 : DTFS_STRUCTURE X2 /* i : DTFS input 2 */
154 : )
155 : {
156 : int16_t i;
157 :
158 : #define WMC_TOOL_SKIP
159 : MULT( 1 );
160 : LOOP( 1 );
161 0 : for ( i = 0; i <= X1.lag / 2; i++ )
162 : {
163 : MOVE( 1 );
164 : MULT( 1 );
165 0 : tmp->a[i] = X1.a[i];
166 : MOVE( 1 );
167 0 : tmp->b[i] = X1.b[i];
168 : }
169 : MULT( 1 );
170 : LOOP( 1 );
171 0 : for ( i = 0; i <= X2.lag / 2; i++ )
172 : {
173 : MAC( 1 );
174 0 : tmp->a[i] -= X2.a[i];
175 : ADD( 1 );
176 0 : tmp->b[i] -= X2.b[i];
177 : }
178 : MOVE( 1 );
179 0 : tmp->lag = max( X1.lag, X2.lag );
180 0 : tmp->nH = max( X1.nH, X2.nH );
181 0 : tmp->nH_4kHz = max( X1.nH_4kHz, X2.nH_4kHz );
182 0 : tmp->upper_cut_off_freq_of_interest = X1.upper_cut_off_freq_of_interest;
183 0 : tmp->upper_cut_off_freq = X1.upper_cut_off_freq;
184 : #undef WMC_TOOL_SKIP
185 :
186 0 : return;
187 : }
188 :
189 : /*-------------------------------------------------------------------*
190 : * DTFS_fast_fs_inv()
191 : *
192 : * DTFS inverse.
193 : *-------------------------------------------------------------------*/
194 :
195 0 : static void DTFS_fast_fs_inv(
196 : DTFS_STRUCTURE *X1_DTFS, /* i : DTFS */
197 : float *out, /* o : time domain output */
198 : int16_t N /* i : number of output samples */
199 : )
200 : {
201 0 : uint16_t i, M_2 = (uint16_t) min( X1_DTFS->lag >> 1, X1_DTFS->nH ), N_2 = (uint16_t) N >> 1;
202 : float dbuf[256 + 1]; /* N can't be > 256 */
203 :
204 0 : if ( N < X1_DTFS->lag )
205 : {
206 0 : N = X1_DTFS->lag;
207 : }
208 :
209 : #define WMC_TOOL_SKIP
210 : /* Populate the dbuf array */
211 0 : dbuf[1] = X1_DTFS->a[0];
212 0 : dbuf[2] = 0.0;
213 0 : for ( i = 1; i < M_2; i++ )
214 : {
215 : MAC( 1 );
216 0 : dbuf[2 * i + 1] = X1_DTFS->a[i] * N_2;
217 : MAC( 1 );
218 0 : dbuf[2 * i + 2] = X1_DTFS->b[i] * N_2;
219 : }
220 :
221 0 : if ( N_2 != M_2 )
222 : {
223 0 : dbuf[2 * i + 1] = X1_DTFS->a[i] * N_2;
224 0 : dbuf[2 * i + 2] = X1_DTFS->b[i] * N_2;
225 0 : i++;
226 : }
227 :
228 : /* Zero-padding in the frequency domain */
229 0 : for ( ; i < N_2; i++ )
230 : {
231 : MOVE( 1 );
232 : MAC( 2 );
233 0 : dbuf[2 * i + 1] = dbuf[2 * i + 2] = 0.0;
234 : }
235 :
236 : FUNC( 3 );
237 0 : realft( dbuf, N_2, -1 );
238 :
239 0 : for ( i = 1; i <= N; i++ )
240 : {
241 : MULT( 1 );
242 : ADD( 1 );
243 0 : out[i - 1] = dbuf[i] / N_2;
244 : }
245 : #undef WMC_TOOL_SKIP
246 :
247 0 : return;
248 : }
249 :
250 :
251 : /*-------------------------------------------------------------------*
252 : * DTFS_alignment_weight()
253 : *
254 : * Estimate the shift to find the best match between the reference
255 : * DTFS and the test DTFS.
256 : *-------------------------------------------------------------------*/
257 :
258 : /*! r: shift value to shift X2_DTFS by */
259 0 : static float DTFS_alignment_weight(
260 : DTFS_STRUCTURE refX1_DTFS, /* i : X1 the reference DTFS to keep fixed */
261 : DTFS_STRUCTURE X2_DTFS, /* i : X2 the test DTFS to shift to find best match */
262 : float Eshift, /* i : Expected shift - coarse value */
263 : const float *LPC1, /* i : LPC to filter to find correlation in spch */
264 : const float *LPC2 /* i : LPC to filter to find correlation in spch */
265 : )
266 : {
267 : /* Eshift is w.r.t X2 */
268 : int16_t k;
269 : float maxcorr, corr, Adiff, diff, tmp, tmp1, fshift, n, wcorr;
270 0 : float pwf = 0.7f, tmplpc[M + 1];
271 : DTFS_STRUCTURE X1_DTFS;
272 :
273 : #define WMC_TOOL_SKIP
274 : FUNC( 2 );
275 0 : DTFS_copy( &X1_DTFS, refX1_DTFS );
276 : FUNC( 2 );
277 0 : DTFS_adjustLag( &X1_DTFS, X2_DTFS.lag );
278 : FUNC( 3 );
279 : ADD( 1 );
280 0 : DTFS_poleFilter( &X1_DTFS, LPC1, M + 1 );
281 : MOVE( 1 );
282 0 : tmp = 1.0;
283 : LOOP( 1 );
284 0 : for ( k = 0, tmp = 1.0; k < M + 1; k++ )
285 : {
286 : MOVE( 1 );
287 : MULT( 2 );
288 0 : tmplpc[k] = LPC1[k] * ( tmp *= pwf );
289 : }
290 :
291 : FUNC( 3 );
292 0 : DTFS_zeroFilter( &X1_DTFS, tmplpc, M + 1 );
293 : FUNC( 3 );
294 0 : DTFS_poleFilter( &X2_DTFS, LPC2, M + 1 );
295 : MOVE( 1 );
296 : LOOP( 1 );
297 0 : for ( k = 0, tmp = 1.0; k < M + 1; k++ )
298 : {
299 : /* can be stored as a table */
300 : MULT( 1 );
301 0 : tmplpc[k] = LPC2[k] * ( tmp *= pwf );
302 : }
303 : FUNC( 3 );
304 0 : DTFS_zeroFilter( &X2_DTFS, tmplpc, M + 1 );
305 : MOVE( 1 );
306 : ADD( 1 );
307 0 : maxcorr = (float) -HUGE_VAL;
308 : MOVE( 1 );
309 0 : fshift = Eshift;
310 : MOVE( 1 );
311 : MULT( 1 );
312 0 : Adiff = max( 6, 0.15f * X2_DTFS.lag );
313 : LOGIC( 1 );
314 0 : if ( X2_DTFS.lag < 60 )
315 : {
316 : MOVE( 1 );
317 0 : diff = 0.25;
318 : }
319 : else
320 : {
321 : MOVE( 1 );
322 0 : diff = 0.5;
323 : }
324 : ADD( 1 );
325 : ADD( 1 );
326 : LOOP( 1 );
327 0 : for ( n = Eshift - Adiff; n <= Eshift + Adiff; n += diff )
328 : {
329 : MOVE( 2 );
330 : ADD( 1 );
331 0 : corr = tmp = 0.0f;
332 : /* bit-exact optimization - PI2/X2_DTFS.lag should be counted as a single divide */
333 : MULT( 1 );
334 0 : tmp1 = (float) ( PI2 * n / X2_DTFS.lag );
335 : ADD( 1 );
336 : LOOP( 1 );
337 0 : for ( k = 0; k <= min( X2_DTFS.lag >> 1, X2_DTFS.nH_4kHz ); k++, tmp += tmp1 )
338 : {
339 : /* Not counting math function cos and sin since they will be implemented as look-up tables */
340 : MAC( 1 );
341 : ADD( 1 );
342 0 : corr += (float) ( ( X1_DTFS.a[k] * X2_DTFS.a[k] + X1_DTFS.b[k] * X2_DTFS.b[k] ) * cos( tmp ) );
343 : MAC( 1 );
344 : ADD( 1 );
345 0 : corr += (float) ( ( X1_DTFS.b[k] * X2_DTFS.a[k] - X1_DTFS.a[k] * X2_DTFS.b[k] ) * sin( tmp ) );
346 : }
347 : MOVE( 1 );
348 : MAC( 2 );
349 : ABS( 1 );
350 0 : wcorr = (float) ( corr * ( 1.0f - 0.01f * fabs( n - Eshift ) ) );
351 : LOGIC( 1 );
352 0 : if ( wcorr > maxcorr )
353 : {
354 : MOVE( 1 );
355 0 : fshift = n;
356 : MOVE( 1 );
357 0 : maxcorr = wcorr;
358 : }
359 : }
360 : #undef WMC_TOOL_SKIP
361 :
362 0 : return fshift;
363 : }
364 :
365 :
366 : /*-------------------------------------------------------------------*
367 : * DTFS_alignment_full()
368 : *
369 : * Shift value for DTFS full alignment.
370 : *-------------------------------------------------------------------*/
371 :
372 0 : float DTFS_alignment_full(
373 : DTFS_STRUCTURE X1_DTFS, /* i : reference DTFS */
374 : DTFS_STRUCTURE X2_DTFS, /* i : DTFS to shift */
375 : const int16_t num_steps /* i : resolution */
376 : )
377 : {
378 : int16_t k;
379 : float maxcorr, corr, tmp, tmp1, fshift, n, diff;
380 :
381 : #define WMC_TOOL_SKIP
382 : LOGIC( 1 );
383 0 : if ( X1_DTFS.lag < X2_DTFS.lag )
384 : {
385 : FUNC( 2 );
386 0 : DTFS_zeroPadd( X2_DTFS.lag, &X1_DTFS );
387 : }
388 :
389 : MOVE( 1 );
390 : ADD( 1 );
391 0 : maxcorr = (float) -HUGE_VAL;
392 : /* bit-exact optimization - 1/num_steps can be constant => should be counted as a multiply */
393 : MOVE( 1 );
394 : MULT( 1 );
395 0 : diff = (float) X2_DTFS.lag / num_steps;
396 :
397 : LOOP( 1 );
398 0 : for ( fshift = n = 0.0; n < (float) X2_DTFS.lag; n += diff )
399 : {
400 : MOVE( 2 );
401 0 : corr = tmp = 0.0f;
402 : MOVE( 1 );
403 : MULT( 2 );
404 0 : tmp1 = (float) ( PI2 * n / X2_DTFS.lag );
405 : ADD( 1 );
406 :
407 : LOOP( 1 );
408 0 : for ( k = 0; k <= min( X2_DTFS.lag >> 1, X2_DTFS.nH_4kHz ); k++, tmp += tmp1 )
409 :
410 : {
411 : MAC( 1 );
412 : ADD( 2 );
413 0 : corr += (float) ( ( X1_DTFS.a[k] * X2_DTFS.a[k] + X1_DTFS.b[k] * X2_DTFS.b[k] ) * cos( tmp ) );
414 : MAC( 1 );
415 : ADD( 1 );
416 0 : corr += (float) ( ( X1_DTFS.b[k] * X2_DTFS.a[k] - X1_DTFS.a[k] * X2_DTFS.b[k] ) * sin( tmp ) );
417 : }
418 : LOGIC( 1 );
419 0 : if ( corr > maxcorr )
420 : {
421 : MOVE( 1 );
422 0 : fshift = n;
423 : MOVE( 1 );
424 0 : maxcorr = corr;
425 : }
426 : }
427 : #undef WMC_TOOL_SKIP
428 :
429 0 : return fshift;
430 : }
431 :
432 :
433 : /*-------------------------------------------------------------------*
434 : * DTFS_phaseShift()
435 : *
436 : * Phase shift the DTFS coefficients.
437 : * ph is the amount of phase shift (between 0 and 2pi), where positive
438 : * value indicates a shift to right, and a negative ph value indicates a
439 : * left shift.
440 : *-------------------------------------------------------------------*/
441 :
442 0 : void DTFS_phaseShift(
443 : DTFS_STRUCTURE *X, /* i/o: DTFS to shift */
444 : float ph /* i : phase to shift */
445 : )
446 : {
447 : int16_t k;
448 0 : float tmp, tmp2 = 0.0f;
449 :
450 : #define WMC_TOOL_SKIP
451 : ADD( 1 );
452 : LOOP( 1 );
453 0 : for ( k = 0; k <= min( X->lag >> 1, X->nH ); k++, tmp2 += ph )
454 : {
455 : MOVE( 1 );
456 : ADD( 1 );
457 0 : tmp = X->a[k];
458 : MOVE( 1 );
459 : MAC( 1 );
460 0 : X->a[k] = (float) ( tmp * cos( tmp2 ) - X->b[k] * sin( tmp2 ) );
461 : MOVE( 1 );
462 : MAC( 1 );
463 0 : X->b[k] = (float) ( tmp * sin( tmp2 ) + X->b[k] * cos( tmp2 ) );
464 : }
465 : #undef WMC_TOOL_SKIP
466 :
467 0 : return;
468 : }
469 :
470 :
471 : /*-------------------------------------------------------------------*
472 : * DTFS_zeroPadd()
473 : *
474 : * Zero-pad the DTFS coefficients.
475 : *-------------------------------------------------------------------*/
476 :
477 0 : void DTFS_zeroPadd(
478 : const int16_t N, /* i : Target lag */
479 : DTFS_STRUCTURE *X /* i/o: DTFS */
480 : )
481 : {
482 : int16_t i;
483 : float diff;
484 :
485 : #define WMC_TOOL_SKIP
486 : LOGIC( 1 );
487 0 : if ( N == X->lag )
488 : {
489 : #undef WMC_TOOL_SKIP
490 0 : return;
491 : #define WMC_TOOL_SKIP
492 : }
493 : ADD( 1 );
494 : LOOP( 1 );
495 0 : for ( i = ( X->lag >> 1 ) + 1; i <= N >> 1; i++ )
496 : {
497 : MOVE( 2 );
498 0 : X->a[i] = X->b[i] = 0.0;
499 : }
500 : MOVE( 1 );
501 0 : X->lag = N;
502 : /* recompute nH for new lag */
503 0 : X->nH = (int16_t) floor( X->upper_cut_off_freq / ( 12800.0 / X->lag ) );
504 : MOVE( 1 );
505 : MULT( 1 );
506 0 : diff = 12800.0f / X->lag;
507 : MULT( 1 );
508 : ADD( 1 );
509 : LOGIC( 1 );
510 0 : if ( X->upper_cut_off_freq - ( diff * X->nH ) >= diff )
511 : {
512 0 : X->nH++;
513 : }
514 : #undef WMC_TOOL_SKIP
515 :
516 0 : return;
517 : }
518 :
519 :
520 : /*-------------------------------------------------------------------*
521 : * DTFS_to_fs()
522 : *
523 : * DTFS to Fs conversion.
524 : *-------------------------------------------------------------------*/
525 :
526 0 : void DTFS_to_fs(
527 : const float *x, /* i : time domain signal */
528 : const int16_t N, /* i : Length of input vector */
529 : DTFS_STRUCTURE *X, /* o : DTFS structure with a, b, lag */
530 : const int32_t sampling_rate, /* i : sampling rate */
531 : const int16_t FR_flag /* i : FR flag */
532 : )
533 : {
534 : int16_t n;
535 : int16_t nH, k, nH_band, nH_4kHz;
536 : float sum, tmp, diff;
537 :
538 0 : if ( !FR_flag )
539 : {
540 0 : if ( sampling_rate == 16000 )
541 : {
542 0 : X->upper_cut_off_freq_of_interest = 4000.0;
543 0 : X->upper_cut_off_freq = 6400.0;
544 0 : X->sampling_rate = INT_FS_12k8;
545 : }
546 0 : else if ( sampling_rate == 8000 )
547 : {
548 0 : X->upper_cut_off_freq_of_interest = 3300.0;
549 0 : X->upper_cut_off_freq = 4000.0;
550 0 : X->sampling_rate = INT_FS_12k8;
551 : }
552 : }
553 : else
554 : {
555 0 : X->upper_cut_off_freq_of_interest = 8000.0;
556 0 : X->upper_cut_off_freq = 8000.0;
557 0 : X->sampling_rate = INT_FS_16k;
558 : }
559 :
560 : #define WMC_TOOL_SKIP
561 0 : X->lag = N;
562 : MOVE( 1 );
563 : MULT( 1 );
564 0 : nH_band = (int16_t) floor( X->upper_cut_off_freq / ( 12800.0 / X->lag ) );
565 :
566 0 : nH_4kHz = (int16_t) floor( 4000 / ( 12800.0 / X->lag ) );
567 : MOVE( 1 );
568 : MULT( 1 );
569 0 : diff = 12800.0f / X->lag;
570 : MULT( 1 );
571 : ADD( 1 );
572 : LOGIC( 1 );
573 0 : if ( X->upper_cut_off_freq - ( diff * nH_band ) >= diff )
574 : {
575 0 : nH_band++;
576 : }
577 0 : if ( 4000 - ( diff * nH_4kHz ) >= diff )
578 : {
579 0 : nH_4kHz++;
580 : }
581 : /* Number of harmonics excluding the ones at 0 and at pi */
582 : MOVE( 1 );
583 : MULT( 1 );
584 : ADD( 1 );
585 0 : nH = ( N - 1 ) >> 1;
586 : /* The DC component */
587 0 : X->a[0] = 0.0;
588 0 : X->b[0] = 0.0;
589 : LOOP( 1 );
590 0 : for ( n = 0; n < N; n++ )
591 : {
592 : ADD( 1 );
593 0 : X->a[0] += x[n];
594 : }
595 : MULT( 1 );
596 0 : X->a[0] /= N;
597 :
598 : /* Strictly set the DC componet to zero */
599 : MOVE( 1 );
600 0 : X->a[0] = 0.0;
601 :
602 : /* The harmonics excluding the one at pi */
603 : LOOP( 1 );
604 0 : for ( k = 1; k <= nH; k++ )
605 : {
606 0 : X->a[k] = x[0];
607 0 : X->b[k] = 0.0;
608 : MOVE( 2 );
609 : MULT( 1 );
610 0 : sum = tmp = (float) ( PI2 * k / N );
611 : LOOP( 1 );
612 0 : for ( n = 1; n < N; n++, sum += tmp )
613 : {
614 : ADD( 1 );
615 0 : X->a[k] += (float) ( x[n] * cos( sum ) );
616 : ADD( 1 );
617 0 : X->b[k] += (float) ( x[n] * sin( sum ) );
618 : }
619 : MULT( 2 );
620 0 : X->a[k] *= ( 2.0f / N );
621 : MULT( 2 );
622 0 : X->b[k] *= ( 2.0f / N );
623 : }
624 :
625 : /* The harmonic at 'pi' */
626 : LOGIC( 1 );
627 0 : if ( N % 2 == 0 )
628 : {
629 : MOVE( 1 );
630 0 : X->a[k] = 0.0;
631 : MOVE( 1 );
632 0 : tmp = 1.0;
633 : LOOP( 1 );
634 0 : for ( n = 0; n < N; n++, tmp *= -1.0 )
635 : {
636 : ADD( 1 );
637 0 : X->a[k] += x[n] * tmp;
638 : }
639 : MULT( 1 );
640 0 : X->a[k] /= N;
641 : MOVE( 1 );
642 0 : X->b[k] = 0.0;
643 : }
644 : ADD( 1 );
645 : LOOP( 1 );
646 0 : for ( k = nH_band + 1; k <= min( ( X->lag >> 1 ), ( MAXLAG_WI - 1 ) ); k++ )
647 : {
648 : MOVE( 1 );
649 0 : X->a[k] = 0.0;
650 : MOVE( 1 );
651 0 : X->b[k] = 0.0;
652 : }
653 0 : X->nH = nH_band;
654 0 : X->nH_4kHz = nH_4kHz;
655 : #undef WMC_TOOL_SKIP
656 :
657 0 : return;
658 : }
659 :
660 :
661 : /*-------------------------------------------------------------------*
662 : * DTFS_fs_inv()
663 : *
664 : * DTFS Inverse
665 : *-------------------------------------------------------------------*/
666 :
667 0 : void DTFS_fs_inv(
668 : DTFS_STRUCTURE *X, /* i : DTFS input */
669 : float *x, /* o : time domain sig */
670 : const int16_t N, /* i : Output length */
671 : float ph0 /* i : phase shift applied to the output */
672 : )
673 : {
674 : float phase, tmp;
675 : int16_t k, n;
676 :
677 : #define WMC_TOOL_SKIP
678 : LOOP( 1 );
679 0 : for ( n = 0; n < N; n++ )
680 : {
681 : MOVE( 1 );
682 0 : x[n] = X->a[0];
683 : MOVE( 2 );
684 : MAC( 1 );
685 0 : tmp = phase = (float) ( PI2 * n / X->lag + ph0 );
686 : ADD( 1 );
687 : LOOP( 1 );
688 0 : for ( k = 1; k <= min( X->lag >> 1, X->nH ); k++, tmp += phase )
689 : {
690 : MAC( 1 );
691 : ADD( 1 );
692 0 : x[n] += (float) ( X->a[k] * cos( tmp ) + X->b[k] * sin( tmp ) );
693 : }
694 : }
695 : #undef WMC_TOOL_SKIP
696 :
697 0 : return;
698 : }
699 :
700 :
701 : /*-------------------------------------------------------------------*
702 : * DTFS_transform()
703 : *
704 : * DTFS transform.
705 : *-------------------------------------------------------------------*/
706 :
707 0 : static void DTFS_transform(
708 : DTFS_STRUCTURE X, /* i : Starting DTFS to use in WI */
709 : DTFS_STRUCTURE X2, /* i : Ending DTFS to use in WI */
710 : const float *phase, /* i : Phase contour */
711 : float *out, /* o : Output time domain waveform */
712 : int16_t N, /* i : Number of samples to generate */
713 : const int16_t FR_flag /* i : Flag to indicate called in FR context */
714 : )
715 : {
716 : int16_t i, j, j1;
717 : float w, tmp;
718 : float x1_256[256], x2_256[256];
719 : float sum1, sum2;
720 : int16_t m, l1, k;
721 : int16_t N1;
722 0 : float nrg_flag = 0;
723 : float x_r_fx[L_FRAME];
724 : float temp_w;
725 : ivas_error error;
726 :
727 : DTFS_STRUCTURE *tmp1_dtfs;
728 : DTFS_STRUCTURE *tmp2_dtfs;
729 : DTFS_STRUCTURE *tmp3_dtfs;
730 :
731 0 : error = IVAS_ERR_OK;
732 :
733 0 : if ( ( error = DTFS_new( &tmp1_dtfs ) ) != IVAS_ERR_OK )
734 : {
735 0 : IVAS_ERROR( error, "Error creating DTFS structure 1" );
736 : }
737 0 : if ( ( error = DTFS_new( &tmp2_dtfs ) ) != IVAS_ERR_OK )
738 : {
739 0 : IVAS_ERROR( error, "Error creating DTFS structure 2" );
740 : }
741 0 : if ( ( error = DTFS_new( &tmp3_dtfs ) ) != IVAS_ERR_OK )
742 : {
743 0 : IVAS_ERROR( error, "Error creating DTFS structure 3" );
744 : }
745 :
746 : #define WMC_TOOL_SKIP
747 : FUNC( 2 );
748 0 : DTFS_copy( tmp1_dtfs, X );
749 : FUNC( 2 );
750 0 : DTFS_copy( tmp2_dtfs, X2 );
751 : FUNC( 3 );
752 0 : DTFS_fast_fs_inv( tmp1_dtfs, x1_256, 256 );
753 : FUNC( 3 );
754 0 : DTFS_fast_fs_inv( tmp2_dtfs, x2_256, 256 );
755 : MOVE( 1 );
756 : MAC( 1 );
757 : ADD( 1 );
758 : LOG( 1 );
759 0 : tmp = (float) ( log( 1.0 - WI_THRESHLD ) / ( X.lag - N ) );
760 : LOOP( 1 );
761 0 : for ( i = 0; i < N; i++ )
762 : {
763 0 : if ( FR_flag == 0 )
764 : {
765 : /* should not be counted inside the loop */
766 0 : if ( N - WI_SAMPLE_THLD > X.lag )
767 : {
768 : /* pre-computed and stored in a table */
769 : MOVE( 1 );
770 0 : w = (float) ( 1.0 - exp( -( i + 1 ) * tmp ) );
771 : }
772 : else
773 : {
774 : /* can be a look-up table */
775 : MOVE( 1 );
776 0 : w = (float) ( i + 1 ) / N;
777 : }
778 : }
779 : else
780 : {
781 0 : if ( nrg_flag )
782 : {
783 0 : w = (float) ( i + 1 ) / N;
784 : }
785 : else
786 : {
787 0 : if ( N <= tmp2_dtfs->lag )
788 : {
789 0 : N = tmp2_dtfs->lag + 1;
790 : }
791 :
792 0 : N1 = N - tmp2_dtfs->lag;
793 0 : if ( i < N1 )
794 : {
795 0 : w = (float) ( i + 1 ) / N1;
796 : }
797 : else
798 0 : w = 1.0;
799 : }
800 : }
801 :
802 : /* add sinc interpolation of two time domain waveforms at
803 : appropriate phase position */
804 : MULT( 1 );
805 : DIV( 1 );
806 : FUNC( 1 );
807 : ADD( 1 );
808 0 : j = ( LL_OS * 10 + (int) rint_new( phase[i] * LL_OS / PI2 ) ) % LL_OS;
809 :
810 0 : if ( j < 0 )
811 : {
812 0 : j = 0;
813 : }
814 :
815 : MOVE( 1 );
816 : DIV( 1 );
817 0 : k = j % WARP_OS_RATE;
818 : MOVE( 1 );
819 : MULT( 1 );
820 0 : l1 = j / WARP_OS_RATE;
821 : MOVE( 2 );
822 :
823 0 : set_f( x_r_fx, 0.0f, L_FRAME );
824 :
825 0 : temp_w = ( 1 - w );
826 :
827 0 : for ( j1 = 0; j1 < 12; j1++ )
828 : {
829 0 : m = ( 1000 * LL + l1 - OSLENGTH / 2 + j1 ) % LL;
830 :
831 0 : if ( m < 0 )
832 : {
833 0 : m = 0;
834 : }
835 :
836 0 : x_r_fx[m] = x1_256[m] * temp_w + x2_256[m] * w;
837 : }
838 :
839 : LOOP( 1 );
840 0 : for ( j1 = 0, sum1 = sum2 = 0.0; j1 < OSLENGTH; j1++ )
841 : {
842 : /* mult or div by constants should be done once outside the loop */
843 : DIV( 1 );
844 : ADD( 1 );
845 0 : m = ( 1000 * LL + l1 - OSLENGTH / 2 + j1 ) % LL;
846 :
847 0 : if ( m < 0 )
848 : {
849 0 : m = 0;
850 : }
851 :
852 0 : sum1 += x_r_fx[m] * sinc[k][j1];
853 : }
854 :
855 0 : out[i] = sum1;
856 : }
857 : #undef WMC_TOOL_SKIP
858 :
859 0 : free( tmp1_dtfs );
860 0 : free( tmp2_dtfs );
861 0 : free( tmp3_dtfs );
862 :
863 0 : return;
864 : }
865 :
866 :
867 : /*-------------------------------------------------------------------*
868 : * DTFS_zeroFilter()
869 : *
870 : * DTFS zero filter response.
871 : *-------------------------------------------------------------------*/
872 :
873 0 : void DTFS_zeroFilter(
874 : DTFS_STRUCTURE *X, /* i/o: DTFS to zeroFilter inplace */
875 : const float *LPC, /* i : LPCs */
876 : const int16_t N /* i : LPC order */
877 : )
878 : {
879 : float tmp, tmp1, tmp2, sum1, sum2;
880 : int16_t k, n;
881 :
882 : #define WMC_TOOL_SKIP
883 : MOVE( 1 );
884 : MULT( 1 );
885 0 : tmp1 = (float) ( PI2 / X->lag );
886 : LOOP( 1 );
887 0 : for ( k = 0; k <= min( X->lag >> 1, X->nH ); k++ )
888 : {
889 : MOVE( 2 );
890 : MULT( 1 );
891 0 : tmp = tmp2 = k * tmp1;
892 : /* Calculate sum1 and sum2 */
893 : MOVE( 1 );
894 0 : sum1 = 1.0;
895 : MOVE( 1 );
896 0 : sum2 = 0.0;
897 : ADD( 1 );
898 : LOOP( 1 );
899 0 : for ( n = 0; n < N; n++, tmp2 += tmp )
900 : {
901 : MULT( 1 );
902 : ADD( 2 );
903 0 : sum1 += (float) ( LPC[n] * cos( tmp2 ) );
904 : MULT( 1 );
905 : ADD( 1 );
906 0 : sum2 += (float) ( LPC[n] * sin( tmp2 ) );
907 : }
908 : /* Calculate the circular convolution */
909 : MOVE( 1 );
910 0 : tmp = X->a[k];
911 : MOVE( 1 );
912 : MAC( 1 );
913 0 : X->a[k] = tmp * sum1 - X->b[k] * sum2;
914 : MOVE( 1 );
915 : MAC( 1 );
916 0 : X->b[k] = X->b[k] * sum1 + tmp * sum2;
917 : }
918 : #undef WMC_TOOL_SKIP
919 :
920 0 : return;
921 : }
922 :
923 :
924 : /*-------------------------------------------------------------------*
925 : * DTFS_poleFilter()
926 : *
927 : * DTFS pole filter response.
928 : *-------------------------------------------------------------------*/
929 :
930 0 : void DTFS_poleFilter(
931 : DTFS_STRUCTURE *X, /* i/o: DTFS to poleFilter inplace */
932 : const float *LPC, /* i : LPCs */
933 : const int16_t N /* i : LPC order */
934 : )
935 : {
936 : float tmp, tmp1, tmp2, sum1, sum2;
937 : int16_t k, n;
938 :
939 : #define WMC_TOOL_SKIP
940 : MOVE( 1 );
941 : MULT( 1 );
942 0 : tmp1 = (float) ( PI2 / X->lag );
943 : LOOP( 1 );
944 0 : for ( k = 0; k <= min( X->lag >> 1, X->nH ); k++ )
945 : {
946 : MOVE( 2 );
947 : MULT( 1 );
948 0 : tmp = tmp2 = k * tmp1;
949 : /* Calculate sum1 and sum2 */
950 : MOVE( 1 );
951 0 : sum1 = 1.0;
952 : MOVE( 1 );
953 0 : sum2 = 0.0;
954 : ADD( 1 );
955 : LOOP( 1 );
956 0 : for ( n = 0; n < N; n++, tmp2 += tmp )
957 : {
958 : MULT( 1 );
959 : ADD( 2 );
960 0 : sum1 += (float) ( LPC[n] * cos( tmp2 ) );
961 : MULT( 1 );
962 : ADD( 1 );
963 0 : sum2 += (float) ( LPC[n] * sin( tmp2 ) );
964 : }
965 : /* Calculate the circular convolution */
966 : MOVE( 1 );
967 0 : tmp = X->a[k];
968 : MAC( 1 );
969 0 : tmp2 = sum1 * sum1 + sum2 * sum2;
970 : MAC( 1 );
971 : DIV( 1 );
972 0 : X->a[k] = ( tmp * sum1 + X->b[k] * sum2 ) / tmp2;
973 : MAC( 1 );
974 : ADD( 1 );
975 0 : X->b[k] = ( -tmp * sum2 + X->b[k] * sum1 ) / tmp2;
976 : }
977 : #undef WMC_TOOL_SKIP
978 :
979 0 : return;
980 : }
981 :
982 :
983 : /*-------------------------------------------------------------------*
984 : * DTFS_setEngy()
985 : *
986 : * Set DTFS energy.
987 : *-------------------------------------------------------------------*/
988 :
989 0 : static float DTFS_setEngy(
990 : DTFS_STRUCTURE *X_DTFS, /* i/o: DTFS structure to set engy */
991 : float en2 /* i : Energy to set to */
992 : )
993 : {
994 : int16_t k;
995 : float en1, tmp;
996 :
997 : #define WMC_TOOL_SKIP
998 : FUNC( 1 );
999 : MOVE( 1 );
1000 0 : en1 = DTFS_getEngy( *X_DTFS );
1001 : LOGIC( 1 );
1002 0 : if ( en1 == 0.0 )
1003 : {
1004 : #undef WMC_TOOL_SKIP
1005 0 : return 0.0;
1006 : #define WMC_TOOL_SKIP
1007 : }
1008 : MOVE( 1 );
1009 : DIV( 1 );
1010 : SQRT( 1 );
1011 0 : tmp = (float) sqrt( en2 / en1 );
1012 : LOOP( 1 );
1013 0 : for ( k = 0; k <= min( X_DTFS->lag >> 1, X_DTFS->nH ); k++ )
1014 : {
1015 : MULT( 1 );
1016 0 : X_DTFS->a[k] *= tmp;
1017 : MULT( 1 );
1018 0 : X_DTFS->b[k] *= tmp;
1019 : }
1020 : #undef WMC_TOOL_SKIP
1021 :
1022 0 : return en1;
1023 : }
1024 :
1025 :
1026 : /*-------------------------------------------------------------------*
1027 : * DTFS_adjustLag()
1028 : *
1029 : * Adjust DTFS lag based on a target lag.
1030 : *-------------------------------------------------------------------*/
1031 :
1032 0 : void DTFS_adjustLag(
1033 : DTFS_STRUCTURE *X_DTFS, /* i/o: DTFS to adjust lag for */
1034 : const int16_t N /* i : Target lag */
1035 : )
1036 : {
1037 : int16_t k;
1038 : float en, diff;
1039 :
1040 : #define WMC_TOOL_SKIP
1041 : LOGIC( 1 );
1042 0 : if ( N == X_DTFS->lag )
1043 : {
1044 : #undef WMC_TOOL_SKIP
1045 0 : return;
1046 : #define WMC_TOOL_SKIP
1047 : }
1048 :
1049 : LOGIC( 1 );
1050 0 : if ( N > X_DTFS->lag )
1051 : {
1052 :
1053 : FUNC( 2 );
1054 0 : DTFS_zeroPadd( N, X_DTFS );
1055 : }
1056 : else
1057 : {
1058 : FUNC( 1 );
1059 : MOVE( 1 );
1060 0 : en = DTFS_getEngy( *X_DTFS );
1061 : ADD( 1 );
1062 : LOOP( 1 );
1063 0 : for ( k = ( N >> 1 ) + 1; k <= min( X_DTFS->lag >> 1, X_DTFS->nH ); k++ )
1064 : {
1065 : MOVE( 1 );
1066 0 : X_DTFS->a[k] = 0.0;
1067 : MOVE( 1 );
1068 0 : X_DTFS->b[k] = 0.0;
1069 : }
1070 : FUNC( 2 );
1071 0 : DTFS_setEngy( X_DTFS, en );
1072 : MOVE( 1 );
1073 0 : X_DTFS->lag = N;
1074 : /* recompute nH for new lag */
1075 0 : X_DTFS->nH = (int16_t) floor( X_DTFS->upper_cut_off_freq / ( 12800.0 / X_DTFS->lag ) );
1076 :
1077 0 : X_DTFS->nH_4kHz = (int16_t) floor( 4000.0 / ( 12800.0 / X_DTFS->lag ) );
1078 : MOVE( 1 );
1079 : MULT( 1 );
1080 0 : diff = 12800.0f / X_DTFS->lag;
1081 : MULT( 1 );
1082 : ADD( 1 );
1083 : LOGIC( 1 );
1084 0 : if ( X_DTFS->upper_cut_off_freq - ( diff * X_DTFS->nH ) >= diff )
1085 : {
1086 0 : X_DTFS->nH++;
1087 : }
1088 0 : if ( 4000.0 - ( diff * X_DTFS->nH_4kHz ) >= diff )
1089 : {
1090 0 : X_DTFS->nH_4kHz++;
1091 : }
1092 : }
1093 : #undef WMC_TOOL_SKIP
1094 :
1095 0 : return;
1096 : }
1097 :
1098 :
1099 : /*-------------------------------------------------------------------*
1100 : * DTFS_getEngy()
1101 : *
1102 : * Get DTFS energy.
1103 : *-------------------------------------------------------------------*/
1104 :
1105 0 : float DTFS_getEngy(
1106 : DTFS_STRUCTURE X /* i : DTFS to compute energy of */
1107 : )
1108 : {
1109 : int16_t k;
1110 : float en;
1111 :
1112 0 : en = 0.0f;
1113 : #define WMC_TOOL_SKIP
1114 : LOOP( 1 );
1115 0 : for ( k = 1; k <= min( ( X.lag - 1 ) >> 1, X.nH ); k++ )
1116 : {
1117 : MAC( 1 );
1118 : ADD( 1 );
1119 0 : en += X.a[k] * X.a[k] + X.b[k] * X.b[k];
1120 : }
1121 : MULT( 1 );
1122 0 : en /= 2.0;
1123 : MULT( 1 );
1124 : ADD( 1 );
1125 0 : en += X.a[0] * X.a[0];
1126 : LOGIC( 1 );
1127 0 : if ( X.lag % 2 == 0 )
1128 : {
1129 : MAC( 1 );
1130 : ADD( 1 );
1131 0 : en += X.a[k] * X.a[k] + X.b[k] * X.b[k];
1132 : }
1133 : #undef WMC_TOOL_SKIP
1134 :
1135 0 : return en;
1136 : }
1137 :
1138 :
1139 : /*-------------------------------------------------------------------*
1140 : * DTFS_car2pol()
1141 : *
1142 : * DTFS cartesian to polar co-ordinates conversion.
1143 : *-------------------------------------------------------------------*/
1144 :
1145 0 : void DTFS_car2pol(
1146 : DTFS_STRUCTURE *X /* i/o: DTFS structure a, b, lag */
1147 : )
1148 : {
1149 : int16_t k;
1150 : float tmp;
1151 :
1152 : #define WMC_TOOL_SKIP
1153 : LOOP( 1 );
1154 0 : for ( k = 1; k <= min( ( X->lag - 1 ) >> 1, X->nH ); k++ )
1155 : {
1156 : MOVE( 1 );
1157 0 : tmp = X->a[k];
1158 : MOVE( 1 );
1159 : FUNC( 2 );
1160 : MULT( 1 );
1161 0 : X->a[k] = (float) ( 0.5f * sqrt( tmp * tmp + X->b[k] * X->b[k] ) );
1162 : MOVE( 1 );
1163 0 : X->b[k] = (float) atan2( X->b[k], tmp );
1164 : }
1165 : LOGIC( 1 );
1166 0 : if ( X->lag % 2 == 0 )
1167 : {
1168 : MOVE( 1 );
1169 0 : tmp = X->a[k];
1170 : FUNC( 2 );
1171 : MOVE( 1 );
1172 0 : X->a[k] = (float) sqrt( tmp * tmp + X->b[k] * X->b[k] );
1173 : MOVE( 1 );
1174 0 : X->b[k] = (float) atan2( X->b[k], tmp );
1175 : }
1176 : #undef WMC_TOOL_SKIP
1177 :
1178 0 : return;
1179 : }
1180 :
1181 :
1182 : /*-------------------------------------------------------------------*
1183 : * DTFS_pol2car()
1184 : *
1185 : * DTFS polar to cartesian co-ordinates conversion.
1186 : *-------------------------------------------------------------------*/
1187 :
1188 0 : void DTFS_pol2car(
1189 : DTFS_STRUCTURE *X /* i/o: DTFS structure a, b, lag */
1190 : )
1191 : {
1192 : int16_t k;
1193 : float tmp;
1194 :
1195 : #define WMC_TOOL_SKIP
1196 : LOOP( 1 );
1197 0 : for ( k = 1; k <= min( ( X->lag - 1 ) >> 1, X->nH ); k++ )
1198 : {
1199 : MOVE( 1 );
1200 0 : tmp = X->b[k];
1201 : MOVE( 1 );
1202 : MULT( 2 );
1203 0 : X->b[k] = (float) ( 2.0f * X->a[k] * sin( tmp ) );
1204 : MOVE( 1 );
1205 : MULT( 2 );
1206 0 : X->a[k] = (float) ( 2.0f * X->a[k] * cos( tmp ) );
1207 : }
1208 : LOGIC( 1 );
1209 0 : if ( X->lag % 2 == 0 )
1210 : {
1211 : MOVE( 1 );
1212 0 : tmp = X->b[k];
1213 : MOVE( 1 );
1214 : MULT( 1 );
1215 0 : X->b[k] = (float) ( X->a[k] * sin( tmp ) );
1216 : MOVE( 1 );
1217 : MULT( 1 );
1218 0 : X->a[k] = (float) ( X->a[k] * cos( tmp ) );
1219 : }
1220 : #undef WMC_TOOL_SKIP
1221 :
1222 0 : return;
1223 : }
1224 :
1225 :
1226 : /*-------------------------------------------------------------------*
1227 : * DTFS_setEngyHarm()
1228 : *
1229 : * Adjust DTFS energy based on the input target energy.
1230 : *-------------------------------------------------------------------*/
1231 :
1232 : /*! r: Input RMS between f1 and f2 before scaling */
1233 0 : float DTFS_setEngyHarm(
1234 : float f1, /* i : lower band freq of input to control energy */
1235 : float f2, /* i : upper band freq of input to control energy */
1236 : float g1, /* i : lower band freq of output to control energy */
1237 : float g2, /* i : upper band freq of output to control energy */
1238 : float en2, /* i : Target Energy to set the DTFS to */
1239 : DTFS_STRUCTURE *X /* i/o: DTFS to adjust the energy of */
1240 : )
1241 : {
1242 : int16_t k, count;
1243 : float en1, tmp, factor, diff;
1244 :
1245 0 : diff = (float) INT_FS_12k8 / X->lag;
1246 :
1247 0 : en1 = 0.0f;
1248 0 : count = 0;
1249 :
1250 : #define WMC_TOOL_SKIP
1251 : LOGIC( 1 );
1252 0 : if ( f1 == 0.0 )
1253 : {
1254 : MULT( 1 );
1255 : ADD( 1 );
1256 0 : en1 += X->a[0] * X->a[0];
1257 0 : count++;
1258 : }
1259 : MOVE( 1 );
1260 : ADD( 1 );
1261 : ADD( 1 );
1262 : LOOP( 1 );
1263 0 : for ( k = 1, tmp = diff; k <= min( ( X->lag - 1 ) >> 1, X->nH ); k++, tmp += diff )
1264 : {
1265 0 : if ( X->a[k] < EPSILON )
1266 : {
1267 0 : X->a[k] = 0;
1268 : }
1269 :
1270 : LOGIC( 1 );
1271 0 : if ( tmp > f1 && tmp <= f2 )
1272 : {
1273 : MULT( 1 );
1274 : ADD( 1 );
1275 0 : en1 += X->a[k] * X->a[k];
1276 0 : count++;
1277 : }
1278 : }
1279 :
1280 0 : if ( count <= 0.0 )
1281 : {
1282 0 : count = 1;
1283 : }
1284 :
1285 :
1286 : DIV( 1 );
1287 0 : en1 /= count;
1288 :
1289 0 : if ( en2 < 0.0 )
1290 : {
1291 0 : en2 = 0.0;
1292 : }
1293 :
1294 : LOGIC( 1 );
1295 0 : if ( en1 > 0.0 )
1296 : {
1297 : MOVE( 1 );
1298 : DIV( 1 );
1299 : SQRT( 1 );
1300 0 : factor = (float) sqrt( en2 / en1 );
1301 : }
1302 : else
1303 : {
1304 : MOVE( 1 );
1305 0 : factor = 0.0f;
1306 : }
1307 : LOGIC( 1 );
1308 0 : if ( g1 == 0.0 )
1309 : {
1310 : MULT( 1 );
1311 0 : X->a[k] *= factor;
1312 : }
1313 : MOVE( 1 );
1314 : ADD( 1 );
1315 : ADD( 1 );
1316 : LOOP( 1 );
1317 0 : for ( k = 1, tmp = diff; k <= min( ( X->lag - 1 ) >> 1, X->nH ); k++, tmp += diff )
1318 : {
1319 : ADD( 1 );
1320 : ADD( 1 );
1321 : LOGIC( 1 );
1322 0 : if ( tmp > g1 && tmp <= g2 )
1323 : {
1324 : MULT( 1 );
1325 0 : X->a[k] *= factor;
1326 : }
1327 : }
1328 : #undef WMC_TOOL_SKIP
1329 :
1330 0 : return (float) ( en1 + 1e-20 );
1331 : }
1332 :
1333 :
1334 : /*-------------------------------------------------------------------*
1335 : * cubicPhase()
1336 : *
1337 : * Compute coefficients of cubic phase function
1338 : *-------------------------------------------------------------------*/
1339 :
1340 0 : static void cubicPhase(
1341 : float ph1, /* i : phase offset */
1342 : float ph2, /* i : phase 2 */
1343 : const float L1, /* i : previous lag */
1344 : const float L2, /* i : current lag */
1345 : int16_t N, /* i : input length */
1346 : float *phOut /* o : cubic phase output */
1347 : )
1348 : {
1349 : float coef[4], f1, f2, c1, c2, factor;
1350 : int16_t n;
1351 : double diff;
1352 :
1353 : #define WMC_TOOL_SKIP
1354 : ADD( 1 );
1355 0 : N -= (int16_t) L2;
1356 :
1357 0 : if ( N <= 0 )
1358 : {
1359 0 : N = 1;
1360 : }
1361 :
1362 : /* Computation of the coefficients of the cubic phase function */
1363 : MOVE( 1 );
1364 : DIV( 1 );
1365 0 : f1 = (float) ( PI2 / L1 );
1366 : MOVE( 1 );
1367 : DIV( 1 );
1368 0 : f2 = (float) ( PI2 / L2 );
1369 : MOVE( 1 );
1370 : DIV( 1 );
1371 0 : ph1 = (float) fmod( (double) ( ph1 ), PI2 );
1372 : MOVE( 1 );
1373 : DIV( 1 );
1374 0 : ph2 = (float) fmod( (double) ( ph2 ), PI2 );
1375 : MOVE( 1 );
1376 0 : coef[3] = ph1;
1377 : MOVE( 1 );
1378 0 : coef[2] = f1;
1379 : MOVE( 1 );
1380 : MAC( 1 );
1381 : ADD( 1 );
1382 : FUNC( 1 );
1383 : ADD( 1 );
1384 0 : factor = (float) ( anint( ( ph1 - ph2 + 0.5 * N * ( f2 + f1 ) ) / PI2 ) );
1385 : MOVE( 1 );
1386 : ADD( 1 );
1387 0 : c1 = f2 - f1;
1388 : MOVE( 1 );
1389 : MAC( 1 );
1390 : ADD( 2 );
1391 0 : c2 = (float) ( ph2 - ph1 - N * f1 + PI2 * factor );
1392 : MOVE( 1 );
1393 : MAC( 1 );
1394 : DIV( 1 );
1395 0 : coef[0] = ( N * c1 - 2 * c2 ) / ( N * N * N );
1396 : MOVE( 1 );
1397 : MULT( 4 );
1398 : DIV( 1 );
1399 : ADD( 1 );
1400 0 : coef[1] = ( c1 - 3 * N * N * coef[0] ) / ( 2 * N );
1401 : /* Computation of the phase value at each sample point */
1402 : MOVE( 1 );
1403 0 : phOut[0] = ph1;
1404 : LOOP( 1 );
1405 0 : for ( n = 1; n < N; n++ )
1406 : {
1407 : MOVE( 1 );
1408 0 : phOut[n] = _POLY3( n, coef );
1409 : }
1410 : MOVE( 1 );
1411 : MULT( 1 );
1412 0 : diff = (float) ( PI2 / L2 );
1413 : ADD( 1 );
1414 : LOOP( 1 );
1415 0 : for ( ; n < N + (int) L2; n++ )
1416 : {
1417 : MOVE( 1 );
1418 : ADD( 2 );
1419 0 : phOut[n] = (float) ( phOut[n - 1] + diff );
1420 : }
1421 : #undef WMC_TOOL_SKIP
1422 :
1423 0 : return;
1424 : }
1425 :
1426 :
1427 : /*-------------------------------------------------------------------*
1428 : * DTFS_to_erb()
1429 : *
1430 : * DTFS to ERB conversion
1431 : *-------------------------------------------------------------------*/
1432 :
1433 0 : void DTFS_to_erb(
1434 : DTFS_STRUCTURE X, /* i : DTFS input */
1435 : float *out /* o : ERB output */
1436 : )
1437 : {
1438 : int16_t num_erb;
1439 : uint16_t i, j, count[NUM_ERB_WB];
1440 : float freq, diff;
1441 :
1442 0 : const float *erb = NULL;
1443 0 : num_erb = NUM_ERB_NB;
1444 0 : if ( X.upper_cut_off_freq == 4000.0 )
1445 : {
1446 0 : num_erb = NUM_ERB_NB;
1447 0 : erb = &( erb_NB[0] );
1448 : }
1449 0 : else if ( X.upper_cut_off_freq == 6400.0 )
1450 : {
1451 0 : num_erb = NUM_ERB_WB;
1452 0 : erb = &( erb_WB[0] );
1453 : }
1454 :
1455 : #define WMC_TOOL_SKIP
1456 : LOOP( 1 );
1457 0 : for ( i = 0; i < num_erb; i++ )
1458 : {
1459 : MOVE( 1 );
1460 0 : count[i] = 0;
1461 : MOVE( 1 );
1462 0 : out[i] = 0.0;
1463 : }
1464 : MOVE( 1 );
1465 : MULT( 1 );
1466 0 : diff = 12800.0f / X.lag;
1467 : MOVE( 2 );
1468 : ADD( 1 );
1469 : LOOP( 1 );
1470 0 : for ( i = j = 0, freq = 0.0; i <= min( X.lag >> 1, X.nH ); i++, freq += diff )
1471 : {
1472 0 : if ( !( freq <= erb[num_erb] ) )
1473 : {
1474 0 : freq = erb[num_erb];
1475 : }
1476 :
1477 : LOOP( 1 );
1478 0 : for ( ; j < num_erb; j++ )
1479 : {
1480 : LOGIC( 1 );
1481 0 : if ( freq < erb[j + 1] )
1482 : {
1483 0 : if ( X.a[i] < 0.0f )
1484 : {
1485 0 : X.a[i] = 0.0f;
1486 : }
1487 :
1488 : ADD( 1 );
1489 0 : out[j] += X.a[i];
1490 0 : count[j]++;
1491 0 : break;
1492 : }
1493 : }
1494 : }
1495 : LOOP( 1 );
1496 0 : for ( i = 0; i < num_erb; i++ )
1497 : {
1498 : LOGIC( 1 );
1499 0 : if ( count[i] > 1 )
1500 : {
1501 : DIV( 1 );
1502 0 : out[i] /= count[i];
1503 : }
1504 : }
1505 : #undef WMC_TOOL_SKIP
1506 :
1507 0 : return;
1508 : }
1509 :
1510 :
1511 : /*-------------------------------------------------------------------*
1512 : * erb_slot()
1513 : *
1514 : * Estimation of ERB slots.
1515 : *-------------------------------------------------------------------*/
1516 :
1517 0 : void erb_slot(
1518 : int16_t lag, /* i : input lag */
1519 : int16_t *out, /* o : ERB slots */
1520 : float *mfreq, /* i : ERB frequencies */
1521 : int16_t num_erb /* i : number of ERBs */
1522 : )
1523 : {
1524 : uint16_t i, j;
1525 : float freq, diff;
1526 : int16_t upper_cut_off_freq;
1527 0 : const float *erb = NULL;
1528 : int16_t nH_band;
1529 :
1530 0 : upper_cut_off_freq = 4000;
1531 0 : if ( num_erb == NUM_ERB_NB )
1532 : {
1533 0 : upper_cut_off_freq = 4000;
1534 0 : erb = &( erb_NB[0] );
1535 : }
1536 0 : else if ( num_erb == NUM_ERB_WB )
1537 : {
1538 0 : upper_cut_off_freq = 6400;
1539 0 : erb = &( erb_WB[0] );
1540 : }
1541 :
1542 : #define WMC_TOOL_SKIP
1543 : MOVE( 1 );
1544 : MULT( 1 );
1545 0 : nH_band = (int16_t) floor( upper_cut_off_freq / ( 12800.0 / lag ) );
1546 :
1547 : LOOP( 1 );
1548 0 : for ( i = 0; i < num_erb; i++ )
1549 : {
1550 : MOVE( 1 );
1551 0 : out[i] = 0;
1552 : MOVE( 1 );
1553 0 : mfreq[i] = 0.0;
1554 : }
1555 : MOVE( 1 );
1556 : MULT( 1 );
1557 0 : diff = 12800.0f / lag;
1558 : MULT( 1 );
1559 : ADD( 1 );
1560 : LOGIC( 1 );
1561 0 : if ( upper_cut_off_freq - ( diff * nH_band ) >= diff )
1562 : {
1563 0 : nH_band++;
1564 : }
1565 : MOVE( 2 );
1566 : ADD( 1 );
1567 : LOOP( 1 );
1568 0 : for ( i = j = 0, freq = 0.0; i <= min( lag >> 1, nH_band ); i++, freq += diff )
1569 : {
1570 :
1571 0 : if ( !( freq <= erb[num_erb] ) )
1572 : {
1573 0 : freq = erb[num_erb];
1574 : }
1575 :
1576 : MOVE( 1 );
1577 0 : freq = min( freq, upper_cut_off_freq );
1578 :
1579 : LOOP( 1 );
1580 0 : for ( ; j < num_erb; j++ )
1581 : {
1582 : LOGIC( 1 );
1583 0 : if ( freq < erb[j + 1] )
1584 : {
1585 : ADD( 1 );
1586 0 : mfreq[j] += freq;
1587 0 : out[j]++;
1588 0 : break;
1589 : }
1590 : }
1591 : }
1592 : LOOP( 1 );
1593 0 : for ( j = 0; j < num_erb; j++ )
1594 : {
1595 : LOGIC( 1 );
1596 0 : if ( out[j] > 1 )
1597 : {
1598 : DIV( 1 );
1599 0 : mfreq[j] /= out[j];
1600 : }
1601 : }
1602 : #undef WMC_TOOL_SKIP
1603 :
1604 0 : return;
1605 : }
1606 :
1607 :
1608 : /*-------------------------------------------------------------------*
1609 : * DTFS_erb_inv()
1610 : *
1611 : * .DTFS after ERB inverse
1612 : *-------------------------------------------------------------------*/
1613 :
1614 0 : void DTFS_erb_inv(
1615 : float *in, /* i : ERB inpt */
1616 : int16_t *slot, /* i : ERB slots filled based on lag */
1617 : float *mfreq, /* i : erb frequence edges */
1618 : DTFS_STRUCTURE *X, /* o : DTFS after erb-inv */
1619 : const int16_t num_erb /* i : Number of ERB bands */
1620 : )
1621 : {
1622 0 : int16_t i, j, m = 0;
1623 : float diff;
1624 : float freq, f[NUM_ERB_WB + 2], amp[NUM_ERB_WB + 2];
1625 0 : int16_t upper_cut_off_freq = 0;
1626 :
1627 0 : const float *erb = NULL;
1628 :
1629 0 : if ( num_erb == NUM_ERB_NB )
1630 : {
1631 0 : upper_cut_off_freq = 4000;
1632 0 : erb = &( erb_NB[0] );
1633 : }
1634 0 : else if ( num_erb == NUM_ERB_WB )
1635 : {
1636 0 : upper_cut_off_freq = 6400;
1637 0 : erb = &( erb_WB[0] );
1638 : }
1639 :
1640 : #define WMC_TOOL_SKIP
1641 : MOVE( 1 );
1642 0 : f[m] = 0.0;
1643 : MOVE( 1 );
1644 0 : amp[m] = 0.0;
1645 0 : m++;
1646 : LOOP( 1 );
1647 0 : for ( i = 0; i < num_erb; i++ )
1648 : {
1649 : LOGIC( 1 );
1650 0 : if ( slot[i] != 0 )
1651 : {
1652 : MOVE( 1 );
1653 0 : f[m] = mfreq[i];
1654 : MOVE( 1 );
1655 0 : amp[m] = in[i];
1656 0 : m++;
1657 : }
1658 : }
1659 : MOVE( 1 );
1660 0 : f[m] = upper_cut_off_freq;
1661 : MOVE( 1 );
1662 0 : amp[m] = 0.0;
1663 0 : m++;
1664 :
1665 : MOVE( 1 );
1666 : MULT( 1 );
1667 0 : diff = 12800.0f / X->lag;
1668 :
1669 : MOVE( 2 );
1670 : ADD( 1 );
1671 : LOOP( 1 );
1672 0 : for ( i = 0, j = 1, freq = 0.0; i <= min( X->lag >> 1, X->nH ); i++, freq += diff )
1673 : {
1674 0 : if ( !( freq <= erb[num_erb] ) )
1675 : {
1676 0 : freq = erb[num_erb];
1677 : }
1678 :
1679 0 : if ( !( m <= num_erb + 2 ) )
1680 : {
1681 0 : m = num_erb + 2;
1682 : }
1683 :
1684 : LOGIC( 1 );
1685 0 : if ( freq > upper_cut_off_freq )
1686 : {
1687 : MOVE( 1 );
1688 0 : freq = upper_cut_off_freq;
1689 : }
1690 : LOOP( 1 );
1691 0 : for ( ; j < m; j++ )
1692 : {
1693 : LOGIC( 1 );
1694 0 : if ( freq <= f[j] )
1695 : {
1696 : MOVE( 1 );
1697 : MAC( 2 );
1698 : ADD( 1 );
1699 0 : X->a[i] = amp[j] * ( freq - f[j - 1] ) + amp[j - 1] * ( f[j] - freq );
1700 : LOGIC( 1 );
1701 0 : if ( f[j] != f[j - 1] )
1702 : {
1703 : DIV( 1 );
1704 : ADD( 1 );
1705 0 : X->a[i] /= ( f[j] - f[j - 1] );
1706 : }
1707 0 : break;
1708 : }
1709 : }
1710 :
1711 0 : X->a[0] = 0.0f;
1712 : }
1713 : #undef WMC_TOOL_SKIP
1714 :
1715 0 : return;
1716 : }
1717 :
1718 :
1719 : /*-------------------------------------------------------------------*
1720 : * LPCPowSpect()
1721 : *
1722 : * LPC power spectrum
1723 : *-------------------------------------------------------------------*/
1724 :
1725 0 : static void LPCPowSpect(
1726 : const float *freq, /* i : ERB frequencies */
1727 : const int16_t Nf, /* i : Number of ERBs */
1728 : const float *LPC, /* i : LPC coefficients */
1729 : const int16_t Np, /* i : Number of LPCs */
1730 : float *out /* o : LPC power spectrum */
1731 : )
1732 : {
1733 : float w, tmp, Re, Im;
1734 : int16_t i, k;
1735 :
1736 : #define WMC_TOOL_SKIP
1737 : LOOP( 1 );
1738 0 : for ( k = 0; k < Nf; k++ )
1739 : {
1740 : MOVE( 1 );
1741 0 : Re = 1.0;
1742 : MOVE( 1 );
1743 0 : Im = 0.0;
1744 : /* Note that freq ranges between [0 UPPER_CUT_OFF_FREQ] */
1745 : MOVE( 1 );
1746 : MULT( 2 );
1747 0 : tmp = (float) ( freq[k] / 12800.0f * PI2 );
1748 : MOVE( 1 );
1749 : ADD( 1 );
1750 : LOOP( 1 );
1751 0 : for ( i = 0, w = tmp; i < Np; i++, w += tmp )
1752 : {
1753 : MULT( 1 );
1754 : ADD( 2 );
1755 0 : Re += (float) ( LPC[i] * cos( w ) );
1756 : MULT( 1 );
1757 : ADD( 1 );
1758 0 : Im -= (float) ( LPC[i] * sin( w ) );
1759 : }
1760 : MOVE( 1 );
1761 : MAC( 1 );
1762 : DIV( 1 );
1763 0 : out[k] = 1.0f / ( Re * Re + Im * Im );
1764 : }
1765 : #undef WMC_TOOL_SKIP
1766 :
1767 0 : return;
1768 : }
1769 :
1770 :
1771 : /*-------------------------------------------------------------------*
1772 : * erb_diff()
1773 : *
1774 : * ERB difference
1775 : *-------------------------------------------------------------------*/
1776 :
1777 0 : void erb_diff(
1778 : const float *prev_erb, /* i : previous ERB */
1779 : const int16_t pl, /* i : previous lag */
1780 : const float *curr_erb, /* i : current ERB */
1781 : const int16_t l, /* i : current lag */
1782 : const float *curr_lsp, /* i : current LSP coefficients */
1783 : float *out, /* o : ERB difference */
1784 : int16_t *index, /* i : ERB index */
1785 : const int16_t num_erb /* i : Number of ERBs */
1786 : )
1787 : {
1788 : int16_t i, j;
1789 : int16_t pslot[NUM_ERB_WB], cslot[NUM_ERB_WB], mmseindex;
1790 : float tmp, tmp1, t_prev_erb[NUM_ERB_WB], LPC[M + 1], mfreq[NUM_ERB_WB], PowSpect[NUM_ERB_WB], mmse;
1791 0 : const float( *AmpCB1 )[10] = 0;
1792 :
1793 0 : if ( num_erb == NUM_ERB_NB )
1794 : {
1795 0 : AmpCB1 = AmpCB1_NB;
1796 : }
1797 0 : else if ( num_erb == NUM_ERB_WB )
1798 : {
1799 0 : AmpCB1 = AmpCB1_WB;
1800 : }
1801 :
1802 : #define WMC_TOOL_SKIP
1803 : FUNC( 3 );
1804 0 : erb_slot( l, cslot, mfreq, num_erb );
1805 : FUNC( 3 );
1806 0 : erb_slot( pl, pslot, t_prev_erb, num_erb );
1807 : MOVE( 1 );
1808 : ADD( 1 );
1809 : LOOP( 1 );
1810 0 : for ( i = 0, tmp = 1.0f; i < M + 1; i++ )
1811 : {
1812 : MOVE( 1 );
1813 : MULT( 2 );
1814 : ADD( 1 );
1815 0 : LPC[i] = curr_lsp[i] * ( tmp *= 0.78f );
1816 : }
1817 : FUNC( 5 );
1818 : ADD( 1 );
1819 0 : LPCPowSpect( mfreq, num_erb, LPC, M + 1, PowSpect );
1820 :
1821 : LOOP( 1 );
1822 0 : for ( i = 0; i < num_erb; i++ )
1823 : {
1824 : MOVE( 1 );
1825 0 : t_prev_erb[i] = prev_erb[i];
1826 : }
1827 :
1828 : LOGIC( 1 );
1829 0 : if ( pl > l )
1830 : {
1831 : MOVE( 1 );
1832 0 : tmp = t_prev_erb[0];
1833 : LOOP( 1 );
1834 0 : for ( i = 0; i < num_erb; i++ )
1835 : {
1836 0 : if ( pslot[i] < 0 )
1837 : {
1838 0 : pslot[i] = 0;
1839 : }
1840 :
1841 : LOGIC( 1 );
1842 0 : if ( pslot[i] != 0 )
1843 : {
1844 : MOVE( 1 );
1845 0 : tmp = t_prev_erb[i];
1846 : }
1847 : else
1848 : {
1849 : MOVE( 1 );
1850 0 : t_prev_erb[i] = tmp;
1851 : }
1852 : }
1853 : }
1854 0 : else if ( l > pl )
1855 : {
1856 : MOVE( 1 );
1857 0 : tmp = t_prev_erb[num_erb - 1];
1858 : ADD( 1 );
1859 : LOOP( 1 );
1860 0 : for ( i = num_erb - 1; i >= 0; i-- )
1861 : {
1862 : LOGIC( 1 );
1863 0 : if ( pslot[i] != 0 )
1864 : {
1865 : MOVE( 1 );
1866 0 : tmp = t_prev_erb[i];
1867 : }
1868 : else
1869 : {
1870 : MOVE( 1 );
1871 0 : t_prev_erb[i] = tmp;
1872 : }
1873 : }
1874 : }
1875 :
1876 : LOOP( 1 );
1877 0 : for ( i = 0; i < num_erb; i++ )
1878 : {
1879 : MOVE( 1 );
1880 : ADD( 1 );
1881 0 : out[i] = curr_erb[i] - t_prev_erb[i];
1882 : }
1883 :
1884 : /* First Band Amplitude Search */
1885 : MOVE( 1 );
1886 0 : mmse = (float) HUGE_VAL;
1887 : MOVE( 1 );
1888 0 : mmseindex = -1;
1889 : LOOP( 1 );
1890 0 : for ( j = 0; j < ERB_CBSIZE1; j++ )
1891 : {
1892 : MOVE( 1 );
1893 0 : tmp = 0.0;
1894 : LOOP( 1 );
1895 0 : for ( i = 1; i < 11; i++ )
1896 : {
1897 : LOGIC( 1 );
1898 0 : if ( cslot[i] != 0 )
1899 : {
1900 : ADD( 1 );
1901 : LOGIC( 1 );
1902 0 : if ( AmpCB1[j][i - 1] < -t_prev_erb[i] )
1903 : {
1904 : MOVE( 1 );
1905 : MULT( 1 );
1906 : MULT( 1 );
1907 0 : tmp1 = PowSpect[i] * SQR( curr_erb[i] );
1908 : }
1909 : else
1910 : {
1911 : MOVE( 1 );
1912 : MAC( 1 );
1913 : MULT( 1 );
1914 0 : tmp1 = (float) ( PowSpect[i] * SQR( out[i] - AmpCB1[j][i - 1] ) );
1915 : }
1916 : LOGIC( 1 );
1917 0 : if ( AmpCB1[j][i - 1] < out[i] )
1918 : {
1919 : MULT( 1 );
1920 0 : tmp1 *= 0.9f;
1921 : }
1922 : ADD( 1 );
1923 0 : tmp += tmp1;
1924 : }
1925 : }
1926 :
1927 : LOGIC( 1 );
1928 0 : if ( tmp < mmse )
1929 : {
1930 : MOVE( 1 );
1931 0 : mmse = tmp;
1932 : MOVE( 1 );
1933 0 : mmseindex = j;
1934 : }
1935 : }
1936 :
1937 0 : if ( !( mmseindex < ERB_CBSIZE1 && mmseindex >= 0 ) )
1938 : {
1939 0 : mmseindex = 0;
1940 : }
1941 :
1942 : MOVE( 1 );
1943 0 : index[0] = mmseindex;
1944 :
1945 : /* Second Band Amplitude Search */
1946 : MOVE( 1 );
1947 0 : mmse = (float) HUGE_VAL;
1948 : MOVE( 1 );
1949 0 : mmseindex = -1;
1950 : LOOP( 1 );
1951 0 : for ( j = 0; j < ERB_CBSIZE2; j++ )
1952 : {
1953 : MOVE( 1 );
1954 0 : tmp = 0.0;
1955 0 : for ( i = 11; i < num_erb; i++ )
1956 : {
1957 0 : if ( num_erb == NUM_ERB_NB )
1958 : {
1959 : LOGIC( 1 );
1960 0 : if ( cslot[i] != 0 )
1961 : {
1962 : ADD( 1 );
1963 : LOGIC( 1 );
1964 0 : if ( AmpCB2_NB[j][i - 11] < -t_prev_erb[i] )
1965 : {
1966 : MOVE( 1 );
1967 : MULT( 1 );
1968 : MULT( 1 );
1969 0 : tmp1 = PowSpect[i] * SQR( curr_erb[i] );
1970 : }
1971 : else
1972 : {
1973 : MOVE( 1 );
1974 : MAC( 1 );
1975 : MULT( 1 );
1976 0 : tmp1 = (float) ( PowSpect[i] * SQR( out[i] - AmpCB2_NB[j][i - 11] ) );
1977 : }
1978 :
1979 : LOGIC( 1 );
1980 0 : if ( AmpCB2_NB[j][i - 11] < out[i] )
1981 : {
1982 : MULT( 1 );
1983 0 : tmp1 *= 0.9f;
1984 : }
1985 : ADD( 1 );
1986 0 : tmp += tmp1;
1987 : }
1988 : }
1989 0 : else if ( num_erb == NUM_ERB_WB )
1990 : {
1991 0 : if ( cslot[i] != 0 )
1992 : {
1993 : ADD( 1 );
1994 : LOGIC( 1 );
1995 0 : if ( AmpCB2_WB[j][i - 11] < -t_prev_erb[i] )
1996 : {
1997 : MOVE( 1 );
1998 : MULT( 1 );
1999 : MULT( 1 );
2000 0 : tmp1 = PowSpect[i] * SQR( curr_erb[i] );
2001 : }
2002 : else
2003 : {
2004 : MOVE( 1 );
2005 : MAC( 1 );
2006 : MULT( 1 );
2007 0 : tmp1 = (float) ( PowSpect[i] * SQR( out[i] - AmpCB2_WB[j][i - 11] ) );
2008 : }
2009 :
2010 : LOGIC( 1 );
2011 0 : if ( AmpCB2_WB[j][i - 11] < out[i] )
2012 : {
2013 : MULT( 1 );
2014 0 : tmp1 *= 0.9f;
2015 : }
2016 : ADD( 1 );
2017 0 : tmp += tmp1;
2018 : }
2019 : }
2020 : }
2021 :
2022 : LOGIC( 1 );
2023 0 : if ( tmp < mmse )
2024 : {
2025 : MOVE( 1 );
2026 0 : mmse = tmp;
2027 : MOVE( 1 );
2028 0 : mmseindex = j;
2029 : }
2030 : }
2031 :
2032 0 : if ( !( mmseindex < ERB_CBSIZE2 && mmseindex >= 0 ) )
2033 : {
2034 0 : mmseindex = 0;
2035 : }
2036 :
2037 : MOVE( 1 );
2038 0 : index[1] = mmseindex;
2039 : #undef WMC_TOOL_SKIP
2040 :
2041 0 : return;
2042 : }
2043 :
2044 : /*-------------------------------------------------------------------*
2045 : * erb_add()
2046 : *
2047 : * Add current and past ERB
2048 : *-------------------------------------------------------------------*/
2049 :
2050 0 : void erb_add(
2051 : float *curr_erb, /* i/o: current ERB */
2052 : const int16_t l, /* i : current lag */
2053 : const float *prev_erb, /* i : previous ERB */
2054 : const int16_t pl, /* i : previous lag */
2055 : const int16_t *index, /* i : ERB index */
2056 : const int16_t num_erb /* i : number of ERBs */
2057 : )
2058 : {
2059 : int16_t i, pslot[NUM_ERB_WB], cslot[NUM_ERB_WB];
2060 : float tmp, t_prev_erb[NUM_ERB_WB];
2061 0 : const float( *AmpCB1 )[10] = 0;
2062 :
2063 0 : if ( num_erb == NUM_ERB_NB )
2064 : {
2065 0 : AmpCB1 = AmpCB1_NB;
2066 : }
2067 0 : else if ( num_erb == NUM_ERB_WB )
2068 : {
2069 0 : AmpCB1 = AmpCB1_WB;
2070 : }
2071 :
2072 : #define WMC_TOOL_SKIP
2073 : FUNC( 3 );
2074 0 : erb_slot( l, cslot, t_prev_erb, num_erb );
2075 : FUNC( 3 );
2076 0 : erb_slot( pl, pslot, t_prev_erb, num_erb );
2077 :
2078 : LOOP( 1 );
2079 0 : for ( i = 0; i < num_erb; i++ )
2080 : {
2081 : MOVE( 1 );
2082 0 : t_prev_erb[i] = prev_erb[i];
2083 : }
2084 :
2085 : LOGIC( 1 );
2086 0 : if ( pl > l )
2087 : {
2088 : MOVE( 1 );
2089 0 : tmp = t_prev_erb[0];
2090 : LOOP( 1 );
2091 0 : for ( i = 0; i < num_erb; i++ )
2092 : {
2093 0 : if ( !( pslot[i] >= 0 ) )
2094 : {
2095 0 : pslot[i] = 0;
2096 : }
2097 :
2098 : LOGIC( 1 );
2099 0 : if ( pslot[i] != 0 )
2100 : {
2101 : MOVE( 1 );
2102 0 : tmp = t_prev_erb[i];
2103 : }
2104 : else
2105 : {
2106 : MOVE( 1 );
2107 0 : t_prev_erb[i] = tmp;
2108 : }
2109 : }
2110 : }
2111 0 : else if ( l > pl )
2112 : {
2113 : MOVE( 1 );
2114 0 : tmp = t_prev_erb[num_erb - 1];
2115 : ADD( 1 );
2116 : LOOP( 1 );
2117 0 : for ( i = num_erb - 1; i >= 0; i-- )
2118 : {
2119 : LOGIC( 1 );
2120 0 : if ( pslot[i] != 0 )
2121 : {
2122 : MOVE( 1 );
2123 0 : tmp = t_prev_erb[i];
2124 : }
2125 : else
2126 : {
2127 : MOVE( 1 );
2128 0 : t_prev_erb[i] = tmp;
2129 : }
2130 : }
2131 : }
2132 :
2133 : LOOP( 1 );
2134 0 : for ( i = 1; i < 11; i++ )
2135 : {
2136 : LOGIC( 1 );
2137 0 : if ( cslot[i] != 0 )
2138 : {
2139 : MOVE( 1 );
2140 : ADD( 1 );
2141 0 : curr_erb[i] = (float) ( AmpCB1[index[0]][i - 1] + t_prev_erb[i] );
2142 : MOVE( 1 );
2143 0 : curr_erb[i] = max( 0.0f, curr_erb[i] );
2144 : }
2145 : else
2146 : {
2147 : MOVE( 1 );
2148 0 : curr_erb[i] = 0.0;
2149 : }
2150 : }
2151 : LOOP( 1 );
2152 0 : for ( i = 11; i < ( num_erb - 2 ); i++ )
2153 : {
2154 : LOGIC( 1 );
2155 0 : if ( cslot[i] != 0 )
2156 : {
2157 : MOVE( 1 );
2158 0 : if ( num_erb == NUM_ERB_NB )
2159 : {
2160 : ADD( 1 );
2161 0 : curr_erb[i] = (float) ( AmpCB2_NB[index[1]][i - 11] + t_prev_erb[i] );
2162 : MOVE( 1 );
2163 0 : curr_erb[i] = max( 0.0f, curr_erb[i] );
2164 : }
2165 0 : else if ( num_erb == NUM_ERB_WB )
2166 : {
2167 : ADD( 1 );
2168 0 : curr_erb[i] = (float) ( AmpCB2_WB[index[1]][i - 11] + t_prev_erb[i] );
2169 : MOVE( 1 );
2170 0 : curr_erb[i] = max( 0.0f, curr_erb[i] );
2171 : }
2172 : }
2173 : else
2174 : {
2175 : MOVE( 1 );
2176 0 : curr_erb[i] = 0.0f;
2177 : }
2178 : }
2179 : #undef WMC_TOOL_SKIP
2180 :
2181 0 : return;
2182 : }
2183 :
2184 :
2185 : /*-------------------------------------------------------------------*
2186 : * WIsyn()
2187 : *
2188 : * Synthesis using waveform interpolation
2189 : *-------------------------------------------------------------------*/
2190 :
2191 0 : ivas_error WIsyn(
2192 : DTFS_STRUCTURE PREVCW, /* i : Prev frame DTFS */
2193 : DTFS_STRUCTURE *CURRCW_DTFS_out, /* i/o: Curr frame DTFS */
2194 : const float *curr_lpc, /* i : LPC */
2195 : float *ph_offset, /* i/o: Phase offset to line up at end of frame */
2196 : float *out, /* o : Waveform Interpolated time domain signal */
2197 : const int16_t N, /* i : Number of output samples to generate */
2198 : const int16_t FR_flag /* i : called for post-smoothing in FR */
2199 : )
2200 : {
2201 : DTFS_STRUCTURE *CURRCW_DTFS;
2202 0 : uint16_t I = 1, flag = 0;
2203 : float alignment, tmp, *phase;
2204 : ivas_error error;
2205 :
2206 0 : error = IVAS_ERR_OK;
2207 :
2208 0 : if ( ( phase = (float *) malloc( N * sizeof( float ) ) ) == NULL )
2209 : {
2210 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for WI structure\n" ) );
2211 : }
2212 :
2213 0 : if ( ( error = DTFS_new( &CURRCW_DTFS ) ) != IVAS_ERR_OK )
2214 : {
2215 0 : return IVAS_ERROR( error, "Error creating new DTFS structure\n" );
2216 : }
2217 :
2218 : #define WMC_TOOL_SKIP
2219 : FUNC( 2 );
2220 0 : DTFS_copy( CURRCW_DTFS, *CURRCW_DTFS_out );
2221 :
2222 : /* Calculating the expected alignment shift */
2223 : MOVE( 1 );
2224 : MULT( 2 );
2225 : INDIRECT( 1 );
2226 : INDIRECT( 1 );
2227 0 : alignment = (float) ( *ph_offset / PI2 * PREVCW.lag );
2228 : LOGIC( 1 );
2229 0 : if ( flag == 1 )
2230 : {
2231 : MULT( 1 );
2232 0 : alignment *= I;
2233 : }
2234 : /* Calculating the expected alignment shift */
2235 : MOVE( 1 );
2236 : DIV( 1 );
2237 : ADD( 2 );
2238 : DIV( 1 );
2239 0 : tmp = (float) fmod( ( N % ( ( PREVCW.lag + CURRCW_DTFS->lag ) >> 1 ) + alignment ), CURRCW_DTFS->lag );
2240 :
2241 : /* Compute the alignment shift */
2242 0 : if ( FR_flag == 0 )
2243 : {
2244 : FUNC( 5 );
2245 : MOVE( 1 );
2246 0 : alignment = DTFS_alignment_weight( PREVCW, *CURRCW_DTFS, tmp, curr_lpc, curr_lpc );
2247 : }
2248 : else /* FR case */
2249 : {
2250 : FUNC( 5 );
2251 : MOVE( 1 );
2252 0 : alignment = DTFS_alignment_full( PREVCW, *CURRCW_DTFS, CURRCW_DTFS->lag * 2 );
2253 : }
2254 :
2255 : MOVE( 1 );
2256 : MULT( 2 );
2257 0 : tmp = (float) ( PI2 * alignment / CURRCW_DTFS->lag );
2258 : FUNC( 2 );
2259 0 : DTFS_phaseShift( CURRCW_DTFS, tmp );
2260 : FUNC( 2 );
2261 : MULT( 2 );
2262 0 : DTFS_phaseShift( CURRCW_DTFS_out, (float) ( PI2 * alignment / CURRCW_DTFS->lag ) );
2263 :
2264 : /* Compute the cubic phase track and transform to 1-D signal */
2265 : FUNC( 6 );
2266 0 : cubicPhase( *ph_offset, tmp, (float) PREVCW.lag, (float) CURRCW_DTFS->lag, N, phase );
2267 :
2268 0 : if ( FR_flag == 0 )
2269 : {
2270 : FUNC( 6 );
2271 0 : DTFS_transform( PREVCW, *CURRCW_DTFS, phase, out, N, 0 );
2272 : }
2273 : else
2274 : {
2275 : FUNC( 6 );
2276 0 : DTFS_transform( PREVCW, *CURRCW_DTFS, phase, out, N, 1 );
2277 : }
2278 :
2279 : /* Adjust the phase offset and wrap it between 0 and 2pi */
2280 : LOGIC( 1 );
2281 0 : if ( flag == 2 )
2282 : {
2283 : MULT( 1 );
2284 0 : tmp *= I;
2285 : }
2286 : MOVE( 1 );
2287 : DIV( 1 );
2288 0 : *ph_offset = (float) fmod( (double) ( tmp ), PI2 );
2289 : #undef WMC_TOOL_SKIP
2290 :
2291 0 : free( phase );
2292 0 : free( CURRCW_DTFS );
2293 :
2294 0 : return error;
2295 : }
|
