b37169378b1f525660fccef65f8a2fb431ecc809
[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, are permitted provided that the following conditions
5 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 Internet Society, IETF or IETF Trust, nor the
12 names of specific contributors, may be used to endorse or promote
13 products derived from this software without specific prior written
14 permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 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 #include "pitch.h"
39
40 #define SCRATCH_SIZE        22
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     int                 arch                /* I Run-time architecture                                          */
53 );
54
55 static void silk_P_Ana_calc_energy_st3(
56     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
57     const silk_float    frame[],            /* I vector to correlate                                            */
58     opus_int            start_lag,          /* I start lag                                                      */
59     opus_int            sf_length,          /* I sub frame length                                               */
60     opus_int            nb_subfr,           /* I number of subframes                                            */
61     opus_int            complexity          /* I Complexity setting                                             */
62 );
63
64 /************************************************************/
65 /* CORE PITCH ANALYSIS FUNCTION                             */
66 /************************************************************/
67 opus_int silk_pitch_analysis_core_FLP(      /* O    Voicing estimate: 0 voiced, 1 unvoiced                      */
68     const silk_float    *frame,             /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz                  */
69     opus_int            *pitch_out,         /* O    Pitch lag values [nb_subfr]                                 */
70     opus_int16          *lagIndex,          /* O    Lag Index                                                   */
71     opus_int8           *contourIndex,      /* O    Pitch contour Index                                         */
72     silk_float          *LTPCorr,           /* I/O  Normalized correlation; input: value from previous frame    */
73     opus_int            prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */
74     const silk_float    search_thres1,      /* I    First stage threshold for lag candidates 0 - 1              */
75     const silk_float    search_thres2,      /* I    Final threshold for lag candidates 0 - 1                    */
76     const opus_int      Fs_kHz,             /* I    sample frequency (kHz)                                      */
77     const opus_int      complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */
78     const opus_int      nb_subfr,           /* I    Number of 5 ms subframes                                    */
79     int                 arch                /* I    Run-time architecture                                       */
80 )
81 {
82     opus_int   i, k, d, j;
83     silk_float frame_8kHz[  PE_MAX_FRAME_LENGTH_MS * 8 ];
84     silk_float frame_4kHz[  PE_MAX_FRAME_LENGTH_MS * 4 ];
85     opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ];
86     opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ];
87     opus_int32 filt_state[ 6 ];
88     silk_float threshold, contour_bias;
89     silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ];
90     opus_val32 xcorr[ PE_MAX_LAG_MS * 4 - PE_MIN_LAG_MS * 4 + 1 ];
91     silk_float CC[ PE_NB_CBKS_STAGE2_EXT ];
92     const silk_float *target_ptr, *basis_ptr;
93     double    cross_corr, normalizer, energy, energy_tmp;
94     opus_int   d_srch[ PE_D_SRCH_LENGTH ];
95     opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ];
96     opus_int   length_d_srch, length_d_comp;
97     silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;
98     opus_int   CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;
99     opus_int   cbk_size;
100     silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr;
101     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
102     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
103     opus_int   lag_counter;
104     opus_int   frame_length, frame_length_8kHz, frame_length_4kHz;
105     opus_int   sf_length, sf_length_8kHz, sf_length_4kHz;
106     opus_int   min_lag, min_lag_8kHz, min_lag_4kHz;
107     opus_int   max_lag, max_lag_8kHz, max_lag_4kHz;
108     opus_int   nb_cbk_search;
109     const opus_int8 *Lag_CB_ptr;
110
111     /* Check for valid sampling frequency */
112     silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
113
114     /* Check for valid complexity setting */
115     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
116     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
117
118     silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );
119     silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );
120
121     /* Set up frame lengths max / min lag for the sampling frequency */
122     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
123     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
124     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
125     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;
126     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;
127     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;
128     min_lag           = PE_MIN_LAG_MS * Fs_kHz;
129     min_lag_4kHz      = PE_MIN_LAG_MS * 4;
130     min_lag_8kHz      = PE_MIN_LAG_MS * 8;
131     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;
132     max_lag_4kHz      = PE_MAX_LAG_MS * 4;
133     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;
134
135     /* Resample from input sampled at Fs_kHz to 8 kHz */
136     if( Fs_kHz == 16 ) {
137         /* Resample to 16 -> 8 khz */
138         opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ];
139         silk_float2short_array( frame_16_FIX, frame, frame_length );
140         silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
141         silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length );
142         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
143     } else if( Fs_kHz == 12 ) {
144         /* Resample to 12 -> 8 khz */
145         opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ];
146         silk_float2short_array( frame_12_FIX, frame, frame_length );
147         silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
148         silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length );
149         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
150     } else {
151         silk_assert( Fs_kHz == 8 );
152         silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz );
153     }
154
155     /* Decimate again to 4 kHz */
156     silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
157     silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz );
158     silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz );
159
160     /* Low-pass filter */
161     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
162         frame_4kHz[ i ] = silk_ADD_SAT16( frame_4kHz[ i ], frame_4kHz[ i - 1 ] );
163     }
164
165     /******************************************************************************
166     * FIRST STAGE, operating in 4 khz
167     ******************************************************************************/
168     silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));
169     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
170     for( k = 0; k < nb_subfr >> 1; k++ ) {
171         /* Check that we are within range of the array */
172         silk_assert( target_ptr >= frame_4kHz );
173         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
174
175         basis_ptr = target_ptr - min_lag_4kHz;
176
177         /* Check that we are within range of the array */
178         silk_assert( basis_ptr >= frame_4kHz );
179         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
180
181         celt_pitch_xcorr( target_ptr, target_ptr-max_lag_4kHz, xcorr, sf_length_8kHz, max_lag_4kHz - min_lag_4kHz + 1, arch );
182
183         /* Calculate first vector products before loop */
184         cross_corr = xcorr[ max_lag_4kHz - min_lag_4kHz ];
185         normalizer = silk_energy_FLP( target_ptr, sf_length_8kHz ) +
186                      silk_energy_FLP( basis_ptr,  sf_length_8kHz ) +
187                      sf_length_8kHz * 4000.0f;
188
189         C[ 0 ][ min_lag_4kHz ] += (silk_float)( 2 * cross_corr / normalizer );
190
191         /* From now on normalizer is computed recursively */
192         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
193             basis_ptr--;
194
195             /* Check that we are within range of the array */
196             silk_assert( basis_ptr >= frame_4kHz );
197             silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
198
199             cross_corr = xcorr[ max_lag_4kHz - d ];
200
201             /* Add contribution of new sample and remove contribution from oldest sample */
202             normalizer +=
203                 basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] -
204                 basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ];
205             C[ 0 ][ d ] += (silk_float)( 2 * cross_corr / normalizer );
206         }
207         /* Update target pointer */
208         target_ptr += sf_length_8kHz;
209     }
210
211     /* Apply short-lag bias */
212     for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
213         C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;
214     }
215
216     /* Sort */
217     length_d_srch = 4 + 2 * complexity;
218     silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
219     silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
220
221     /* Escape if correlation is very low already here */
222     Cmax = C[ 0 ][ min_lag_4kHz ];
223     if( Cmax < 0.2f ) {
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));
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 ) + 1.0;
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             if( cross_corr > 0.0f ) {
296                 energy = silk_energy_FLP( basis_ptr, sf_length_8kHz );
297                 C[ k ][ d ] = (silk_float)( 2 * cross_corr / ( energy + energy_tmp ) );
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     /* Set up 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
360         /* Bias towards shorter lags */
361         lag_log2 = silk_log2( (silk_float)d );
362         CCmax_new_b = CCmax_new - PE_SHORTLAG_BIAS * nb_subfr * lag_log2;
363
364         /* Bias towards previous lag */
365         if( prevLag > 0 ) {
366             delta_lag_log2_sqr = lag_log2 - prevLag_log2;
367             delta_lag_log2_sqr *= delta_lag_log2_sqr;
368             CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / ( delta_lag_log2_sqr + 0.5f );
369         }
370
371         if( CCmax_new_b > CCmax_b &&                /* Find maximum biased correlation                  */
372             CCmax_new > nb_subfr * search_thres2    /* Correlation needs to be high enough to be voiced */
373         ) {
374             CCmax_b = CCmax_new_b;
375             CCmax   = CCmax_new;
376             lag     = d;
377             CBimax  = CBimax_new;
378         }
379     }
380
381     if( lag == -1 ) {
382         /* No suitable candidate found */
383         silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) );
384         *LTPCorr      = 0.0f;
385         *lagIndex     = 0;
386         *contourIndex = 0;
387         return 1;
388     }
389
390     /* Output normalized correlation */
391     *LTPCorr = (silk_float)( CCmax / nb_subfr );
392     silk_assert( *LTPCorr >= 0.0f );
393
394     if( Fs_kHz > 8 ) {
395         /* Search in original signal */
396
397         /* Compensate for decimation */
398         silk_assert( lag == silk_SAT16( lag ) );
399         if( Fs_kHz == 12 ) {
400             lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 );
401         } else { /* Fs_kHz == 16 */
402             lag = silk_LSHIFT( lag, 1 );
403         }
404
405         lag = silk_LIMIT_int( lag, min_lag, max_lag );
406         start_lag = silk_max_int( lag - 2, min_lag );
407         end_lag   = silk_min_int( lag + 2, max_lag );
408         lag_new   = lag;                                    /* to avoid undefined lag */
409         CBimax    = 0;                                      /* to avoid undefined lag */
410
411         CCmax = -1000.0f;
412
413         /* Calculate the correlations and energies needed in stage 3 */
414         silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity, arch );
415         silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity );
416
417         lag_counter = 0;
418         silk_assert( lag == silk_SAT16( lag ) );
419         contour_bias = PE_FLATCONTOUR_BIAS / lag;
420
421         /* Set up cbk parameters according to complexity setting and frame length */
422         if( nb_subfr == PE_MAX_NB_SUBFR ) {
423             nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
424             cbk_size      = PE_NB_CBKS_STAGE3_MAX;
425             Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
426         } else {
427             nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
428             cbk_size      = PE_NB_CBKS_STAGE3_10MS;
429             Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
430         }
431
432         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ];
433         energy_tmp = silk_energy_FLP( target_ptr, nb_subfr * sf_length ) + 1.0;
434         for( d = start_lag; d <= end_lag; d++ ) {
435             for( j = 0; j < nb_cbk_search; j++ ) {
436                 cross_corr = 0.0;
437                 energy = energy_tmp;
438                 for( k = 0; k < nb_subfr; k++ ) {
439                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
440                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];
441                 }
442                 if( cross_corr > 0.0 ) {
443                     CCmax_new = (silk_float)( 2 * cross_corr / energy );
444                     /* Reduce depending on flatness of contour */
445                     CCmax_new *= 1.0f - contour_bias * j;
446                 } else {
447                     CCmax_new = 0.0f;
448                 }
449
450                 if( CCmax_new > CCmax && ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) {
451                     CCmax   = CCmax_new;
452                     lag_new = d;
453                     CBimax  = j;
454                 }
455             }
456             lag_counter++;
457         }
458
459         for( k = 0; k < nb_subfr; k++ ) {
460             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
461             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
462         }
463         *lagIndex = (opus_int16)( lag_new - min_lag );
464         *contourIndex = (opus_int8)CBimax;
465     } else {        /* Fs_kHz == 8 */
466         /* Save Lags */
467         for( k = 0; k < nb_subfr; k++ ) {
468             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
469             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 );
470         }
471         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
472         *contourIndex = (opus_int8)CBimax;
473     }
474     silk_assert( *lagIndex >= 0 );
475     /* return as voiced */
476     return 0;
477 }
478
479 /***********************************************************************
480  * Calculates the correlations used in stage 3 search. In order to cover
481  * the whole lag codebook for all the searched offset lags (lag +- 2),
482  * the following correlations are needed in each sub frame:
483  *
484  * sf1: lag range [-8,...,7] total 16 correlations
485  * sf2: lag range [-4,...,4] total 9 correlations
486  * sf3: lag range [-3,....4] total 8 correltions
487  * sf4: lag range [-6,....8] total 15 correlations
488  *
489  * In total 48 correlations. The direct implementation computed in worst
490  * case 4*12*5 = 240 correlations, but more likely around 120.
491  ***********************************************************************/
492 static void silk_P_Ana_calc_corr_st3(
493     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
494     const silk_float    frame[],            /* I vector to correlate                                            */
495     opus_int            start_lag,          /* I start lag                                                      */
496     opus_int            sf_length,          /* I sub frame length                                               */
497     opus_int            nb_subfr,           /* I number of subframes                                            */
498     opus_int            complexity,         /* I Complexity setting                                             */
499     int                 arch                /* I Run-time architecture                                          */
500 )
501 {
502     const silk_float *target_ptr;
503     opus_int   i, j, k, lag_counter, lag_low, lag_high;
504     opus_int   nb_cbk_search, delta, idx, cbk_size;
505     silk_float scratch_mem[ SCRATCH_SIZE ];
506     opus_val32 xcorr[ SCRATCH_SIZE ];
507     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
508
509     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
510     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
511
512     if( nb_subfr == PE_MAX_NB_SUBFR ) {
513         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
514         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
515         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
516         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
517     } else {
518         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
519         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
520         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
521         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
522         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
523     }
524
525     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
526     for( k = 0; k < nb_subfr; k++ ) {
527         lag_counter = 0;
528
529         /* Calculate the correlations for each subframe */
530         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
531         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
532         silk_assert(lag_high-lag_low+1 <= SCRATCH_SIZE);
533         celt_pitch_xcorr( target_ptr, target_ptr - start_lag - lag_high, xcorr, sf_length, lag_high - lag_low + 1, arch );
534         for( j = lag_low; j <= lag_high; j++ ) {
535             silk_assert( lag_counter < SCRATCH_SIZE );
536             scratch_mem[ lag_counter ] = xcorr[ lag_high - j ];
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 /********************************************************************/
556 /* Calculate the energies for first two subframes. The energies are */
557 /* calculated recursively.                                          */
558 /********************************************************************/
559 static void silk_P_Ana_calc_energy_st3(
560     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
561     const silk_float    frame[],            /* I vector to correlate                                            */
562     opus_int            start_lag,          /* I start lag                                                      */
563     opus_int            sf_length,          /* I sub frame length                                               */
564     opus_int            nb_subfr,           /* I number of subframes                                            */
565     opus_int            complexity          /* I Complexity setting                                             */
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 }