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