pitch analysis (in FIX) now scales the input down at the start, instead of at every...
[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         silk_assert( (frame_length_4kHz & 3) == 0 );
162     for( i = frame_length_4kHz - 1; i > 4; i -= 4 ) {
163         frame_4kHz[ i - 0 ] = silk_ADD_SAT16( frame_4kHz[ i - 0 ], frame_4kHz[ i - 1 ] );
164         frame_4kHz[ i - 1 ] = silk_ADD_SAT16( frame_4kHz[ i - 1 ], frame_4kHz[ i - 2 ] );
165         frame_4kHz[ i - 2 ] = silk_ADD_SAT16( frame_4kHz[ i - 2 ], frame_4kHz[ i - 3 ] );
166         frame_4kHz[ i - 3 ] = silk_ADD_SAT16( frame_4kHz[ i - 3 ], frame_4kHz[ i - 4 ] );
167     }
168     frame_4kHz[ i - 0 ] = silk_ADD_SAT16( frame_4kHz[ i - 0 ], frame_4kHz[ i - 1 ] );
169     frame_4kHz[ i - 1 ] = silk_ADD_SAT16( frame_4kHz[ i - 1 ], frame_4kHz[ i - 2 ] );
170     frame_4kHz[ i - 2 ] = silk_ADD_SAT16( frame_4kHz[ i - 2 ], frame_4kHz[ i - 3 ] );
171
172     /******************************************************************************
173     * FIRST STAGE, operating in 4 khz
174     ******************************************************************************/
175     silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));
176     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
177     for( k = 0; k < nb_subfr >> 1; k++ ) {
178         /* Check that we are within range of the array */
179         silk_assert( target_ptr >= frame_4kHz );
180         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
181
182         basis_ptr = target_ptr - min_lag_4kHz;
183
184         /* Check that we are within range of the array */
185         silk_assert( basis_ptr >= frame_4kHz );
186         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
187
188         celt_pitch_xcorr( target_ptr, target_ptr-max_lag_4kHz, xcorr, sf_length_8kHz, max_lag_4kHz - min_lag_4kHz + 1, arch );
189
190         /* Calculate first vector products before loop */
191         cross_corr = xcorr[ max_lag_4kHz - min_lag_4kHz ];
192         normalizer = silk_energy_FLP( target_ptr, sf_length_8kHz ) +
193                      silk_energy_FLP( basis_ptr,  sf_length_8kHz ) +
194                      sf_length_8kHz * 4000.0f;
195
196         C[ 0 ][ min_lag_4kHz ] += (silk_float)( 2 * cross_corr / normalizer );
197
198         /* From now on normalizer is computed recursively */
199         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
200             basis_ptr--;
201
202             /* Check that we are within range of the array */
203             silk_assert( basis_ptr >= frame_4kHz );
204             silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
205
206             cross_corr = xcorr[ max_lag_4kHz - d ];
207
208             /* Add contribution of new sample and remove contribution from oldest sample */
209             normalizer +=
210                 basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] -
211                 basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ];
212             C[ 0 ][ d ] += (silk_float)( 2 * cross_corr / normalizer );
213         }
214         /* Update target pointer */
215         target_ptr += sf_length_8kHz;
216     }
217
218     /* Apply short-lag bias */
219     for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
220         C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;
221     }
222
223     /* Sort */
224     length_d_srch = 4 + 2 * complexity;
225     silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
226     silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
227
228     /* Escape if correlation is very low already here */
229     Cmax = C[ 0 ][ min_lag_4kHz ];
230     if( Cmax < 0.2f ) {
231         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
232         *LTPCorr      = 0.0f;
233         *lagIndex     = 0;
234         *contourIndex = 0;
235         return 1;
236     }
237
238     threshold = search_thres1 * Cmax;
239     for( i = 0; i < length_d_srch; i++ ) {
240         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
241         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
242             d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
243         } else {
244             length_d_srch = i;
245             break;
246         }
247     }
248     silk_assert( length_d_srch > 0 );
249
250     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
251         d_comp[ i ] = 0;
252     }
253     for( i = 0; i < length_d_srch; i++ ) {
254         d_comp[ d_srch[ i ] ] = 1;
255     }
256
257     /* Convolution */
258     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
259         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
260     }
261
262     length_d_srch = 0;
263     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
264         if( d_comp[ i + 1 ] > 0 ) {
265             d_srch[ length_d_srch ] = i;
266             length_d_srch++;
267         }
268     }
269
270     /* Convolution */
271     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
272         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
273     }
274
275     length_d_comp = 0;
276     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
277         if( d_comp[ i ] > 0 ) {
278             d_comp[ length_d_comp ] = (opus_int16)( i - 2 );
279             length_d_comp++;
280         }
281     }
282
283     /**********************************************************************************
284     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
285     *************************************************************************************/
286     /*********************************************************************************
287     * Find energy of each subframe projected onto its history, for a range of delays
288     *********************************************************************************/
289     silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float));
290
291     if( Fs_kHz == 8 ) {
292         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ];
293     } else {
294         target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
295     }
296     for( k = 0; k < nb_subfr; k++ ) {
297         energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz ) + 1.0;
298         for( j = 0; j < length_d_comp; j++ ) {
299             d = d_comp[ j ];
300             basis_ptr = target_ptr - d;
301             cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );
302             if( cross_corr > 0.0f ) {
303                 energy = silk_energy_FLP( basis_ptr, sf_length_8kHz );
304                 C[ k ][ d ] = (silk_float)( 2 * cross_corr / ( energy + energy_tmp ) );
305             } else {
306                 C[ k ][ d ] = 0.0f;
307             }
308         }
309         target_ptr += sf_length_8kHz;
310     }
311
312     /* search over lag range and lags codebook */
313     /* scale factor for lag codebook, as a function of center lag */
314
315     CCmax   = 0.0f; /* This value doesn't matter */
316     CCmax_b = -1000.0f;
317
318     CBimax = 0; /* To avoid returning undefined lag values */
319     lag = -1;   /* To check if lag with strong enough correlation has been found */
320
321     if( prevLag > 0 ) {
322         if( Fs_kHz == 12 ) {
323             prevLag = silk_LSHIFT( prevLag, 1 ) / 3;
324         } else if( Fs_kHz == 16 ) {
325             prevLag = silk_RSHIFT( prevLag, 1 );
326         }
327         prevLag_log2 = silk_log2( (silk_float)prevLag );
328     } else {
329         prevLag_log2 = 0;
330     }
331
332     /* Set up stage 2 codebook based on number of subframes */
333     if( nb_subfr == PE_MAX_NB_SUBFR ) {
334         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
335         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
336         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
337             /* If input is 8 khz use a larger codebook here because it is last stage */
338             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
339         } else {
340             nb_cbk_search = PE_NB_CBKS_STAGE2;
341         }
342     } else {
343         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
344         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
345         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
346     }
347
348     for( k = 0; k < length_d_srch; k++ ) {
349         d = d_srch[ k ];
350         for( j = 0; j < nb_cbk_search; j++ ) {
351             CC[j] = 0.0f;
352             for( i = 0; i < nb_subfr; i++ ) {
353                 /* Try all codebooks */
354                 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
355             }
356         }
357         /* Find best codebook */
358         CCmax_new  = -1000.0f;
359         CBimax_new = 0;
360         for( i = 0; i < nb_cbk_search; i++ ) {
361             if( CC[ i ] > CCmax_new ) {
362                 CCmax_new = CC[ i ];
363                 CBimax_new = i;
364             }
365         }
366
367         /* Bias towards shorter lags */
368         lag_log2 = silk_log2( (silk_float)d );
369         CCmax_new_b = CCmax_new - PE_SHORTLAG_BIAS * nb_subfr * lag_log2;
370
371         /* Bias towards previous lag */
372         if( prevLag > 0 ) {
373             delta_lag_log2_sqr = lag_log2 - prevLag_log2;
374             delta_lag_log2_sqr *= delta_lag_log2_sqr;
375             CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / ( delta_lag_log2_sqr + 0.5f );
376         }
377
378         if( CCmax_new_b > CCmax_b &&                /* Find maximum biased correlation                  */
379             CCmax_new > nb_subfr * search_thres2    /* Correlation needs to be high enough to be voiced */
380         ) {
381             CCmax_b = CCmax_new_b;
382             CCmax   = CCmax_new;
383             lag     = d;
384             CBimax  = CBimax_new;
385         }
386     }
387
388     if( lag == -1 ) {
389         /* No suitable candidate found */
390         silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) );
391         *LTPCorr      = 0.0f;
392         *lagIndex     = 0;
393         *contourIndex = 0;
394         return 1;
395     }
396
397     /* Output normalized correlation */
398     *LTPCorr = (silk_float)( CCmax / nb_subfr );
399     silk_assert( *LTPCorr >= 0.0f );
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
418         CCmax = -1000.0f;
419
420         /* Calculate the correlations and energies needed in stage 3 */
421         silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity, arch );
422         silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity );
423
424         lag_counter = 0;
425         silk_assert( lag == silk_SAT16( lag ) );
426         contour_bias = PE_FLATCONTOUR_BIAS / lag;
427
428         /* Set up cbk parameters according to complexity setting and frame length */
429         if( nb_subfr == PE_MAX_NB_SUBFR ) {
430             nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
431             cbk_size      = PE_NB_CBKS_STAGE3_MAX;
432             Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
433         } else {
434             nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
435             cbk_size      = PE_NB_CBKS_STAGE3_10MS;
436             Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
437         }
438
439         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ];
440         energy_tmp = silk_energy_FLP( target_ptr, nb_subfr * sf_length ) + 1.0;
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 = energy_tmp;
445                 for( k = 0; k < nb_subfr; k++ ) {
446                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
447                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];
448                 }
449                 if( cross_corr > 0.0 ) {
450                     CCmax_new = (silk_float)( 2 * 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 && ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) {
458                     CCmax   = CCmax_new;
459                     lag_new = d;
460                     CBimax  = j;
461                 }
462             }
463             lag_counter++;
464         }
465
466         for( k = 0; k < nb_subfr; k++ ) {
467             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
468             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
469         }
470         *lagIndex = (opus_int16)( lag_new - min_lag );
471         *contourIndex = (opus_int8)CBimax;
472     } else {        /* Fs_kHz == 8 */
473         /* Save Lags */
474         for( k = 0; k < nb_subfr; k++ ) {
475             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
476             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 );
477         }
478         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
479         *contourIndex = (opus_int8)CBimax;
480     }
481     silk_assert( *lagIndex >= 0 );
482     /* return as voiced */
483     return 0;
484 }
485
486 /***********************************************************************
487  * Calculates the correlations used in stage 3 search. In order to cover
488  * the whole lag codebook for all the searched offset lags (lag +- 2),
489  * the following correlations are needed in each sub frame:
490  *
491  * sf1: lag range [-8,...,7] total 16 correlations
492  * sf2: lag range [-4,...,4] total 9 correlations
493  * sf3: lag range [-3,....4] total 8 correltions
494  * sf4: lag range [-6,....8] total 15 correlations
495  *
496  * In total 48 correlations. The direct implementation computed in worst
497  * case 4*12*5 = 240 correlations, but more likely around 120.
498  ***********************************************************************/
499 static void silk_P_Ana_calc_corr_st3(
500     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
501     const silk_float    frame[],            /* I vector to correlate                                            */
502     opus_int            start_lag,          /* I start lag                                                      */
503     opus_int            sf_length,          /* I sub frame length                                               */
504     opus_int            nb_subfr,           /* I number of subframes                                            */
505     opus_int            complexity,         /* I Complexity setting                                             */
506     int                 arch                /* I Run-time architecture                                          */
507 )
508 {
509     const silk_float *target_ptr;
510     opus_int   i, j, k, lag_counter, lag_low, lag_high;
511     opus_int   nb_cbk_search, delta, idx, cbk_size;
512     silk_float scratch_mem[ SCRATCH_SIZE ];
513     opus_val32 xcorr[ SCRATCH_SIZE ];
514     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
515
516     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
517     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
518
519     if( nb_subfr == PE_MAX_NB_SUBFR ) {
520         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
521         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
522         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
523         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
524     } else {
525         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
526         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
527         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
528         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
529         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
530     }
531
532     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
533     for( k = 0; k < nb_subfr; k++ ) {
534         lag_counter = 0;
535
536         /* Calculate the correlations for each subframe */
537         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
538         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
539         silk_assert(lag_high-lag_low+1 <= SCRATCH_SIZE);
540         celt_pitch_xcorr( target_ptr, target_ptr - start_lag - lag_high, xcorr, sf_length, lag_high - lag_low + 1, arch );
541         for( j = lag_low; j <= lag_high; j++ ) {
542             silk_assert( lag_counter < SCRATCH_SIZE );
543             scratch_mem[ lag_counter ] = xcorr[ lag_high - j ];
544             lag_counter++;
545         }
546
547         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
548         for( i = 0; i < nb_cbk_search; i++ ) {
549             /* Fill out the 3 dim array that stores the correlations for */
550             /* each code_book vector for each start lag */
551             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
552             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
553                 silk_assert( idx + j < SCRATCH_SIZE );
554                 silk_assert( idx + j < lag_counter );
555                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
556             }
557         }
558         target_ptr += sf_length;
559     }
560 }
561
562 /********************************************************************/
563 /* Calculate the energies for first two subframes. The energies are */
564 /* calculated recursively.                                          */
565 /********************************************************************/
566 static void silk_P_Ana_calc_energy_st3(
567     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
568     const silk_float    frame[],            /* I vector to correlate                                            */
569     opus_int            start_lag,          /* I start lag                                                      */
570     opus_int            sf_length,          /* I sub frame length                                               */
571     opus_int            nb_subfr,           /* I number of subframes                                            */
572     opus_int            complexity          /* I Complexity setting                                             */
573 )
574 {
575     const silk_float *target_ptr, *basis_ptr;
576     double    energy;
577     opus_int   k, i, j, lag_counter;
578     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
579     silk_float scratch_mem[ SCRATCH_SIZE ];
580     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
581
582     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
583     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
584
585     if( nb_subfr == PE_MAX_NB_SUBFR ) {
586         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
587         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
588         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
589         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
590     } else {
591         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
592         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
593         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
594         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
595         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
596     }
597
598     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
599     for( k = 0; k < nb_subfr; k++ ) {
600         lag_counter = 0;
601
602         /* Calculate the energy for first lag */
603         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
604         energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;
605         silk_assert( energy >= 0.0 );
606         scratch_mem[lag_counter] = (silk_float)energy;
607         lag_counter++;
608
609         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
610         for( i = 1; i < lag_diff; i++ ) {
611             /* remove part outside new window */
612             energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i];
613             silk_assert( energy >= 0.0 );
614
615             /* add part that comes into window */
616             energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ];
617             silk_assert( energy >= 0.0 );
618             silk_assert( lag_counter < SCRATCH_SIZE );
619             scratch_mem[lag_counter] = (silk_float)energy;
620             lag_counter++;
621         }
622
623         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
624         for( i = 0; i < nb_cbk_search; i++ ) {
625             /* Fill out the 3 dim array that stores the correlations for    */
626             /* each code_book vector for each start lag                     */
627             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
628             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
629                 silk_assert( idx + j < SCRATCH_SIZE );
630                 silk_assert( idx + j < lag_counter );
631                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
632                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );
633             }
634         }
635         target_ptr += sf_length;
636     }
637 }