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