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