Reformatting changes with an update to the MSVC project files
[opus.git] / silk / float / pitch_analysis_core_FLP.c
1 /***********************************************************************
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 Redistribution and use in source and binary forms, with or without
4 modification, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions are met:
6 - Redistributions of source code must retain the above copyright notice,
7 this list of conditions and the following disclaimer.
8 - Redistributions in binary form must reproduce the above copyright
9 notice, this list of conditions and the following disclaimer in the
10 documentation and/or other materials provided with the distribution.
11 - Neither the name of Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 /*****************************************************************************
33 * Pitch analyser function
34 ******************************************************************************/
35 #include "SigProc_FLP.h"
36 #include "SigProc_FIX.h"
37 #include "pitch_est_defines.h"
38
39 #define SCRATCH_SIZE        22
40 #define eps                 1.192092896e-07f
41
42 /************************************************************/
43 /* Internally used functions                                */
44 /************************************************************/
45 static void silk_P_Ana_calc_corr_st3(
46     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
47     const silk_float    frame[],            /* I vector to correlate                                            */
48     opus_int            start_lag,          /* I start lag                                                      */
49     opus_int            sf_length,          /* I sub frame length                                               */
50     opus_int            nb_subfr,           /* I number of subframes                                            */
51     opus_int            complexity          /* I Complexity setting                                             */
52 );
53
54 static void silk_P_Ana_calc_energy_st3(
55     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
56     const silk_float    frame[],            /* I vector to correlate                                            */
57     opus_int            start_lag,          /* I start lag                                                      */
58     opus_int            sf_length,          /* I sub frame length                                               */
59     opus_int            nb_subfr,           /* I number of subframes                                            */
60     opus_int            complexity          /* I Complexity setting                                             */
61 );
62
63 /************************************************************/
64 /* CORE PITCH ANALYSIS FUNCTION                             */
65 /************************************************************/
66 opus_int silk_pitch_analysis_core_FLP(      /* O    Voicing estimate: 0 voiced, 1 unvoiced                      */
67     const silk_float    *frame,             /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz                  */
68     opus_int            *pitch_out,         /* O    Pitch lag values [nb_subfr]                                 */
69     opus_int16          *lagIndex,          /* O    Lag Index                                                   */
70     opus_int8           *contourIndex,      /* O    Pitch contour Index                                         */
71     silk_float          *LTPCorr,           /* I/O  Normalized correlation; input: value from previous frame    */
72     opus_int            prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */
73     const silk_float    search_thres1,      /* I    First stage threshold for lag candidates 0 - 1              */
74     const silk_float    search_thres2,      /* I    Final threshold for lag candidates 0 - 1                    */
75     const opus_int      Fs_kHz,             /* I    sample frequency (kHz)                                      */
76     const opus_int      complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */
77     const opus_int      nb_subfr            /* I    Number of 5 ms subframes                                    */
78 )
79 {
80     opus_int   i, k, d, j;
81     silk_float frame_8kHz[  PE_MAX_FRAME_LENGTH_MS * 8 ];
82     silk_float frame_4kHz[  PE_MAX_FRAME_LENGTH_MS * 4 ];
83     opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ];
84     opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ];
85     opus_int32 filt_state[ 6 ];
86     silk_float threshold, contour_bias;
87     silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ];
88     silk_float CC[ PE_NB_CBKS_STAGE2_EXT ];
89     const silk_float *target_ptr, *basis_ptr;
90     double    cross_corr, normalizer, energy, energy_tmp;
91     opus_int   d_srch[ PE_D_SRCH_LENGTH ];
92     opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ];
93     opus_int   length_d_srch, length_d_comp;
94     silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;
95     opus_int   CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;
96     opus_int   cbk_size;
97     silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr;
98     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
99     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
100     opus_int   lag_counter;
101     opus_int   frame_length, frame_length_8kHz, frame_length_4kHz;
102     opus_int   sf_length, sf_length_8kHz, sf_length_4kHz;
103     opus_int   min_lag, min_lag_8kHz, min_lag_4kHz;
104     opus_int   max_lag, max_lag_8kHz, max_lag_4kHz;
105     opus_int   nb_cbk_search;
106     const opus_int8 *Lag_CB_ptr;
107
108     /* Check for valid sampling frequency */
109     silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
110
111     /* Check for valid complexity setting */
112     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
113     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
114
115     silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );
116     silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );
117
118     /* Setup frame lengths max / min lag for the sampling frequency */
119     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
120     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
121     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
122     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;
123     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;
124     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;
125     min_lag           = PE_MIN_LAG_MS * Fs_kHz;
126     min_lag_4kHz      = PE_MIN_LAG_MS * 4;
127     min_lag_8kHz      = PE_MIN_LAG_MS * 8;
128     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;
129     max_lag_4kHz      = PE_MAX_LAG_MS * 4;
130     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;
131
132     silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));
133
134     /* Resample from input sampled at Fs_kHz to 8 kHz */
135     if( Fs_kHz == 16 ) {
136         /* Resample to 16 -> 8 khz */
137         opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ];
138         silk_float2short_array( frame_16_FIX, frame, frame_length );
139         silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
140         silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length );
141         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
142     } else if( Fs_kHz == 12 ) {
143         /* Resample to 12 -> 8 khz */
144         opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ];
145         silk_float2short_array( frame_12_FIX, frame, frame_length );
146         silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
147         silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length );
148         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
149     } else {
150         silk_assert( Fs_kHz == 8 );
151         silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz );
152     }
153
154     /* Decimate again to 4 kHz */
155     silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
156     silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz );
157     silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz );
158
159     /* Low-pass filter */
160     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
161         frame_4kHz[ i ] += frame_4kHz[ i - 1 ];
162     }
163
164     /******************************************************************************
165     * FIRST STAGE, operating in 4 khz
166     ******************************************************************************/
167     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
168     for( k = 0; k < nb_subfr >> 1; k++ ) {
169         /* Check that we are within range of the array */
170         silk_assert( target_ptr >= frame_4kHz );
171         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
172
173         basis_ptr = target_ptr - min_lag_4kHz;
174
175         /* Check that we are within range of the array */
176         silk_assert( basis_ptr >= frame_4kHz );
177         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
178
179         /* Calculate first vector products before loop */
180         cross_corr = silk_inner_product_FLP( target_ptr, basis_ptr, sf_length_8kHz );
181         normalizer = silk_energy_FLP( basis_ptr, sf_length_8kHz ) + sf_length_8kHz * 4000.0f;
182
183         C[ 0 ][ min_lag_4kHz ] += (silk_float)(cross_corr / sqrt(normalizer));
184
185         /* From now on normalizer is computed recursively */
186         for(d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++) {
187             basis_ptr--;
188
189             /* Check that we are within range of the array */
190             silk_assert( basis_ptr >= frame_4kHz );
191             silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
192
193             cross_corr = silk_inner_product_FLP(target_ptr, basis_ptr, sf_length_8kHz);
194
195             /* Add contribution of new sample and remove contribution from oldest sample */
196             normalizer +=
197                 basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] -
198                 basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ];
199             C[ 0 ][ d ] += (silk_float)(cross_corr / sqrt( normalizer ));
200         }
201         /* Update target pointer */
202         target_ptr += sf_length_8kHz;
203     }
204
205     /* Apply short-lag bias */
206     for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
207         C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;
208     }
209
210     /* Sort */
211     length_d_srch = 4 + 2 * complexity;
212     silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
213     silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
214
215     /* Escape if correlation is very low already here */
216     Cmax = C[ 0 ][ min_lag_4kHz ];
217     target_ptr = &frame_4kHz[ silk_SMULBB( sf_length_4kHz, nb_subfr ) ];
218     energy = 1000.0f;
219     for( i = 0; i < silk_LSHIFT( sf_length_4kHz, 2 ); i++ ) {
220         energy += target_ptr[i] * (double)target_ptr[i];
221     }
222     threshold = Cmax * Cmax;
223     if( energy / 16.0f > threshold ) {
224         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
225         *LTPCorr      = 0.0f;
226         *lagIndex     = 0;
227         *contourIndex = 0;
228         return 1;
229     }
230
231     threshold = search_thres1 * Cmax;
232     for( i = 0; i < length_d_srch; i++ ) {
233         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
234         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
235             d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
236         } else {
237             length_d_srch = i;
238             break;
239         }
240     }
241     silk_assert( length_d_srch > 0 );
242
243     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
244         d_comp[ i ] = 0;
245     }
246     for( i = 0; i < length_d_srch; i++ ) {
247         d_comp[ d_srch[ i ] ] = 1;
248     }
249
250     /* Convolution */
251     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
252         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
253     }
254
255     length_d_srch = 0;
256     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
257         if( d_comp[ i + 1 ] > 0 ) {
258             d_srch[ length_d_srch ] = i;
259             length_d_srch++;
260         }
261     }
262
263     /* Convolution */
264     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
265         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
266     }
267
268     length_d_comp = 0;
269     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
270         if( d_comp[ i ] > 0 ) {
271             d_comp[ length_d_comp ] = (opus_int16)( i - 2 );
272             length_d_comp++;
273         }
274     }
275
276     /**********************************************************************************
277     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
278     *************************************************************************************/
279     /*********************************************************************************
280     * Find energy of each subframe projected onto its history, for a range of delays
281     *********************************************************************************/
282     silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float)); /* Is this needed?*/
283
284     if( Fs_kHz == 8 ) {
285         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ];
286     } else {
287         target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
288     }
289     for( k = 0; k < nb_subfr; k++ ) {
290         energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz );
291         for( j = 0; j < length_d_comp; j++ ) {
292             d = d_comp[ j ];
293             basis_ptr = target_ptr - d;
294             cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );
295             energy     = silk_energy_FLP( basis_ptr, sf_length_8kHz );
296             if( cross_corr > 0.0f ) {
297                 C[ k ][ d ] = (silk_float)(cross_corr * cross_corr / (energy * energy_tmp + eps));
298             } else {
299                 C[ k ][ d ] = 0.0f;
300             }
301         }
302         target_ptr += sf_length_8kHz;
303     }
304
305     /* search over lag range and lags codebook */
306     /* scale factor for lag codebook, as a function of center lag */
307
308     CCmax   = 0.0f; /* This value doesn't matter */
309     CCmax_b = -1000.0f;
310
311     CBimax = 0; /* To avoid returning undefined lag values */
312     lag = -1;   /* To check if lag with strong enough correlation has been found */
313
314     if( prevLag > 0 ) {
315         if( Fs_kHz == 12 ) {
316             prevLag = silk_LSHIFT( prevLag, 1 ) / 3;
317         } else if( Fs_kHz == 16 ) {
318             prevLag = silk_RSHIFT( prevLag, 1 );
319         }
320         prevLag_log2 = silk_log2((silk_float)prevLag);
321     } else {
322         prevLag_log2 = 0;
323     }
324
325     /* Setup stage 2 codebook based on number of subframes */
326     if( nb_subfr == PE_MAX_NB_SUBFR ) {
327         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
328         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
329         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
330             /* If input is 8 khz use a larger codebook here because it is last stage */
331             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
332         } else {
333             nb_cbk_search = PE_NB_CBKS_STAGE2;
334         }
335     } else {
336         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
337         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
338         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
339     }
340
341     for( k = 0; k < length_d_srch; k++ ) {
342         d = d_srch[ k ];
343         for( j = 0; j < nb_cbk_search; j++ ) {
344             CC[j] = 0.0f;
345             for( i = 0; i < nb_subfr; i++ ) {
346                 /* Try all codebooks */
347                 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
348             }
349         }
350         /* Find best codebook */
351         CCmax_new  = -1000.0f;
352         CBimax_new = 0;
353         for( i = 0; i < nb_cbk_search; i++ ) {
354             if( CC[ i ] > CCmax_new ) {
355                 CCmax_new = CC[ i ];
356                 CBimax_new = i;
357             }
358         }
359         CCmax_new = silk_max_float(CCmax_new, 0.0f); /* To avoid taking square root of negative number later */
360         CCmax_new_b = CCmax_new;
361
362         /* Bias towards shorter lags */
363         lag_log2 = silk_log2((silk_float)d);
364         CCmax_new_b -= PE_SHORTLAG_BIAS * nb_subfr * lag_log2;
365
366         /* Bias towards previous lag */
367         if( prevLag > 0 ) {
368             delta_lag_log2_sqr = lag_log2 - prevLag_log2;
369             delta_lag_log2_sqr *= delta_lag_log2_sqr;
370             CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / (delta_lag_log2_sqr + 0.5f);
371         }
372
373         if( CCmax_new_b > CCmax_b                                   &&  /* Find maximum biased correlation                  */
374             CCmax_new > nb_subfr * search_thres2 * search_thres2    &&  /* Correlation needs to be high enough to be voiced */
375             silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz      /* Lag must be in range                             */
376         ) {
377             CCmax_b = CCmax_new_b;
378             CCmax   = CCmax_new;
379             lag     = d;
380             CBimax  = CBimax_new;
381         }
382     }
383
384     if( lag == -1 ) {
385         /* No suitable candidate found */
386         silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) );
387         *LTPCorr      = 0.0f;
388         *lagIndex     = 0;
389         *contourIndex = 0;
390         return 1;
391     }
392
393     if( Fs_kHz > 8 ) {
394         /* Search in original signal */
395
396         /* Compensate for decimation */
397         silk_assert( lag == silk_SAT16( lag ) );
398         if( Fs_kHz == 12 ) {
399             lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 );
400         } else { /* Fs_kHz == 16 */
401             lag = silk_LSHIFT( lag, 1 );
402         }
403
404         lag = silk_LIMIT_int( lag, min_lag, max_lag );
405         start_lag = silk_max_int( lag - 2, min_lag );
406         end_lag   = silk_min_int( lag + 2, max_lag );
407         lag_new   = lag;                                    /* to avoid undefined lag */
408         CBimax    = 0;                                      /* to avoid undefined lag */
409         silk_assert( CCmax >= 0.0f );
410         *LTPCorr = (silk_float)sqrt( CCmax / nb_subfr );    /* Output normalized correlation */
411
412         CCmax = -1000.0f;
413
414         /* Calculate the correlations and energies needed in stage 3 */
415         silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity );
416         silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity );
417
418         lag_counter = 0;
419         silk_assert( lag == silk_SAT16( lag ) );
420         contour_bias = PE_FLATCONTOUR_BIAS / lag;
421
422         /* Setup cbk parameters acording to complexity setting and frame length */
423         if( nb_subfr == PE_MAX_NB_SUBFR ) {
424             nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
425             cbk_size      = PE_NB_CBKS_STAGE3_MAX;
426             Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
427         } else {
428             nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
429             cbk_size      = PE_NB_CBKS_STAGE3_10MS;
430             Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
431         }
432
433         for( d = start_lag; d <= end_lag; d++ ) {
434             for( j = 0; j < nb_cbk_search; j++ ) {
435                 cross_corr = 0.0;
436                 energy = eps;
437                 for( k = 0; k < nb_subfr; k++ ) {
438                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];
439                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
440                 }
441                 if( cross_corr > 0.0 ) {
442                     CCmax_new = (silk_float)(cross_corr * cross_corr / energy);
443                     /* Reduce depending on flatness of contour */
444                     CCmax_new *= 1.0f - contour_bias * j;
445                 } else {
446                     CCmax_new = 0.0f;
447                 }
448
449                 if( CCmax_new > CCmax &&
450                    ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag
451                    ) {
452                     CCmax   = CCmax_new;
453                     lag_new = d;
454                     CBimax  = j;
455                 }
456             }
457             lag_counter++;
458         }
459
460         for( k = 0; k < nb_subfr; k++ ) {
461             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
462             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
463         }
464         *lagIndex = (opus_int16)( lag_new - min_lag );
465         *contourIndex = (opus_int8)CBimax;
466     } else {        /* Fs_kHz == 8 */
467         /* Save Lags and correlation */
468         silk_assert( CCmax >= 0.0f );
469         *LTPCorr = (silk_float)sqrt( CCmax / nb_subfr ); /* Output normalized correlation */
470         for( k = 0; k < nb_subfr; k++ ) {
471             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
472             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * Fs_kHz );
473         }
474         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
475         *contourIndex = (opus_int8)CBimax;
476     }
477     silk_assert( *lagIndex >= 0 );
478     /* return as voiced */
479     return 0;
480 }
481
482 static void silk_P_Ana_calc_corr_st3(
483     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
484     const silk_float    frame[],            /* I vector to correlate                                            */
485     opus_int            start_lag,          /* I start lag                                                      */
486     opus_int            sf_length,          /* I sub frame length                                               */
487     opus_int            nb_subfr,           /* I number of subframes                                            */
488     opus_int            complexity          /* I Complexity setting                                             */
489 )
490     /***********************************************************************
491      Calculates the correlations used in stage 3 search. In order to cover
492      the whole lag codebook for all the searched offset lags (lag +- 2),
493      the following correlations are needed in each sub frame:
494
495      sf1: lag range [-8,...,7] total 16 correlations
496      sf2: lag range [-4,...,4] total 9 correlations
497      sf3: lag range [-3,....4] total 8 correltions
498      sf4: lag range [-6,....8] total 15 correlations
499
500      In total 48 correlations. The direct implementation computed in worst case
501      4*12*5 = 240 correlations, but more likely around 120.
502      **********************************************************************/
503 {
504     const silk_float *target_ptr, *basis_ptr;
505     opus_int   i, j, k, lag_counter, lag_low, lag_high;
506     opus_int   nb_cbk_search, delta, idx, cbk_size;
507     silk_float scratch_mem[ SCRATCH_SIZE ];
508     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
509
510     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
511     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
512
513     if( nb_subfr == PE_MAX_NB_SUBFR ) {
514         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
515         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
516         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
517         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
518     } else {
519         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
520         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
521         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
522         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
523         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
524     }
525
526     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
527     for( k = 0; k < nb_subfr; k++ ) {
528         lag_counter = 0;
529
530         /* Calculate the correlations for each subframe */
531         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
532         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
533         for( j = lag_low; j <= lag_high; j++ ) {
534             basis_ptr = target_ptr - ( start_lag + j );
535             silk_assert( lag_counter < SCRATCH_SIZE );
536             scratch_mem[ lag_counter ] = (silk_float)silk_inner_product_FLP( target_ptr, basis_ptr, sf_length );
537             lag_counter++;
538         }
539
540         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
541         for( i = 0; i < nb_cbk_search; i++ ) {
542             /* Fill out the 3 dim array that stores the correlations for */
543             /* each code_book vector for each start lag */
544             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
545             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
546                 silk_assert( idx + j < SCRATCH_SIZE );
547                 silk_assert( idx + j < lag_counter );
548                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
549             }
550         }
551         target_ptr += sf_length;
552     }
553 }
554
555 static void silk_P_Ana_calc_energy_st3(
556     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
557     const silk_float    frame[],            /* I vector to correlate                                            */
558     opus_int            start_lag,          /* I start lag                                                      */
559     opus_int            sf_length,          /* I sub frame length                                               */
560     opus_int            nb_subfr,           /* I number of subframes                                            */
561     opus_int            complexity          /* I Complexity setting                                             */
562 )
563 /****************************************************************
564 Calculate the energies for first two subframes. The energies are
565 calculated recursively.
566 ****************************************************************/
567 {
568     const silk_float *target_ptr, *basis_ptr;
569     double    energy;
570     opus_int   k, i, j, lag_counter;
571     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
572     silk_float scratch_mem[ SCRATCH_SIZE ];
573     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
574
575     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
576     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
577
578     if( nb_subfr == PE_MAX_NB_SUBFR ) {
579         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
580         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
581         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
582         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
583     } else {
584         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
585         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
586         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
587         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
588         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
589     }
590
591     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
592     for( k = 0; k < nb_subfr; k++ ) {
593         lag_counter = 0;
594
595         /* Calculate the energy for first lag */
596         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
597         energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;
598         silk_assert( energy >= 0.0 );
599         scratch_mem[lag_counter] = (silk_float)energy;
600         lag_counter++;
601
602         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
603         for( i = 1; i < lag_diff; i++ ) {
604             /* remove part outside new window */
605             energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i];
606             silk_assert( energy >= 0.0 );
607
608             /* add part that comes into window */
609             energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ];
610             silk_assert( energy >= 0.0 );
611             silk_assert( lag_counter < SCRATCH_SIZE );
612             scratch_mem[lag_counter] = (silk_float)energy;
613             lag_counter++;
614         }
615
616         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
617         for( i = 0; i < nb_cbk_search; i++ ) {
618             /* Fill out the 3 dim array that stores the correlations for    */
619             /* each code_book vector for each start lag                     */
620             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
621             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
622                 silk_assert( idx + j < SCRATCH_SIZE );
623                 silk_assert( idx + j < lag_counter );
624                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
625                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );
626             }
627         }
628         target_ptr += sf_length;
629     }
630 }