SILK update
[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;\r
106     SKP_int   frame_length, frame_length_8kHz, frame_length_4kHz;\r
107     SKP_int   sf_length, sf_length_8kHz, sf_length_4kHz;\r
108     SKP_int   min_lag, min_lag_8kHz, min_lag_4kHz;\r
109     SKP_int   max_lag, max_lag_8kHz, max_lag_4kHz;\r
110     SKP_int   nb_cbk_search;\r
111     const SKP_int8 *Lag_CB_ptr;\r
112 \r
113     /* Check for valid sampling frequency */\r
114     SKP_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 || Fs_kHz == 24 );\r
115 \r
116     /* Check for valid complexity setting */\r
117     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
118     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
119 \r
120     SKP_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );\r
121     SKP_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );\r
122 \r
123     /* Setup frame lengths max / min lag for the sampling frequency */\r
124     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;\r
125     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;\r
126     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;\r
127     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;\r
128     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;\r
129     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;\r
130     min_lag           = PE_MIN_LAG_MS * Fs_kHz;\r
131     min_lag_4kHz      = PE_MIN_LAG_MS * 4;\r
132     min_lag_8kHz      = PE_MIN_LAG_MS * 8;\r
133     max_lag           = PE_MAX_LAG_MS * Fs_kHz;\r
134     max_lag_4kHz      = PE_MAX_LAG_MS * 4;\r
135     max_lag_8kHz      = PE_MAX_LAG_MS * 8;\r
136 \r
137     SKP_memset(C, 0, sizeof(SKP_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));\r
138     \r
139     /* Resample from input sampled at Fs_kHz to 8 kHz */\r
140     if( Fs_kHz == 12 ) {\r
141         SKP_int16 signal_12[ 12 * PE_MAX_FRAME_LENGTH_MS ];\r
142         SKP_int16 signal_8[   8 * PE_MAX_FRAME_LENGTH_MS ];\r
143         SKP_int32 R23[ 6 ];\r
144 \r
145         /* Resample to 12 -> 8 khz */\r
146         SKP_memset( R23, 0, 6 * sizeof( SKP_int32 ) );\r
147         SKP_float2short_array( signal_12, signal, frame_length );\r
148         SKP_Silk_resampler_down2_3( R23, signal_8, signal_12, frame_length );\r
149         SKP_short2float_array( signal_8kHz, signal_8, frame_length_8kHz );\r
150     } else if( Fs_kHz == 16 ) {\r
151         if( complexity == SigProc_PE_MAX_COMPLEX ) {\r
152             SKP_assert( 4 <= PE_MAX_DECIMATE_STATE_LENGTH );\r
153             SKP_memset( filt_state, 0, 4 * sizeof(SKP_float) );\r
154 \r
155             SKP_Silk_decimate2_coarse_FLP( signal, filt_state, signal_8kHz, \r
156                 scratch_mem, frame_length_8kHz );\r
157         } else {\r
158             SKP_assert( 2 <= PE_MAX_DECIMATE_STATE_LENGTH );\r
159             SKP_memset( filt_state, 0, 2 * sizeof(SKP_float) );\r
160             \r
161             SKP_Silk_decimate2_coarsest_FLP( signal, filt_state, signal_8kHz, \r
162                 scratch_mem, frame_length_8kHz );\r
163         }\r
164     } else if( Fs_kHz == 24 ) {\r
165         SKP_int16 signal_24[ PE_MAX_FRAME_LENGTH ];\r
166         SKP_int16 signal_8[ 8 * PE_MAX_FRAME_LENGTH_MS ];\r
167         SKP_int32 filt_state_fix[ 8 ];\r
168 \r
169         /* Resample to 24 -> 8 khz */\r
170         SKP_float2short_array( signal_24, signal, frame_length );\r
171         SKP_memset( filt_state_fix, 0, 8 * sizeof(SKP_int32) );\r
172         SKP_Silk_resampler_down3( filt_state_fix, signal_8, signal_24, frame_length );\r
173         SKP_short2float_array( signal_8kHz, signal_8, frame_length_8kHz );\r
174     } else {\r
175         SKP_assert( Fs_kHz == 8 );\r
176         SKP_memcpy( signal_8kHz, signal, frame_length_8kHz * sizeof(SKP_float) );\r
177     }\r
178 \r
179     /* Decimate again to 4 kHz. Set mem to zero */\r
180     if( complexity == SigProc_PE_MAX_COMPLEX ) {\r
181         SKP_assert( 4 <= PE_MAX_DECIMATE_STATE_LENGTH );\r
182         SKP_memset( filt_state, 0, 4 * sizeof(SKP_float) );\r
183         SKP_Silk_decimate2_coarse_FLP( signal_8kHz, filt_state, \r
184             signal_4kHz, scratch_mem, frame_length_4kHz );\r
185     } else {\r
186         SKP_assert( 2 <= PE_MAX_DECIMATE_STATE_LENGTH );\r
187         SKP_memset( filt_state, 0, 2 * sizeof(SKP_float) ); \r
188         SKP_Silk_decimate2_coarsest_FLP( signal_8kHz, filt_state, \r
189             signal_4kHz, scratch_mem, frame_length_4kHz );\r
190     }\r
191 \r
192     /* Low-pass filter */\r
193     for( i = frame_length_4kHz - 1; i > 0; i-- ) {\r
194         signal_4kHz[ i ] += signal_4kHz[ i - 1 ];\r
195     }\r
196 \r
197     /******************************************************************************\r
198     * FIRST STAGE, operating in 4 khz\r
199     ******************************************************************************/\r
200     target_ptr = &signal_4kHz[ SKP_LSHIFT( sf_length_4kHz, 2 ) ];\r
201     for( k = 0; k < nb_subfr >> 1; k++ ) {\r
202         /* Check that we are within range of the array */\r
203         SKP_assert( target_ptr >= signal_4kHz );\r
204         SKP_assert( target_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
205 \r
206         basis_ptr = target_ptr - min_lag_4kHz;\r
207 \r
208         /* Check that we are within range of the array */\r
209         SKP_assert( basis_ptr >= signal_4kHz );\r
210         SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
211 \r
212         /* Calculate first vector products before loop */\r
213         cross_corr = SKP_Silk_inner_product_FLP( target_ptr, basis_ptr, sf_length_8kHz );\r
214         normalizer = SKP_Silk_energy_FLP( basis_ptr, sf_length_8kHz ) + sf_length_8kHz * 4000.0f;\r
215 \r
216         C[ 0 ][ min_lag_4kHz ] += (SKP_float)(cross_corr / sqrt(normalizer));\r
217 \r
218         /* From now on normalizer is computed recursively */\r
219         for(d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++) {\r
220             basis_ptr--;\r
221 \r
222             /* Check that we are within range of the array */\r
223             SKP_assert( basis_ptr >= signal_4kHz );\r
224             SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
225 \r
226             cross_corr = SKP_Silk_inner_product_FLP(target_ptr, basis_ptr, sf_length_8kHz);\r
227 \r
228             /* Add contribution of new sample and remove contribution from oldest sample */\r
229             normalizer +=\r
230                 basis_ptr[ 0 ] * basis_ptr[ 0 ] - \r
231                 basis_ptr[ sf_length_8kHz ] * basis_ptr[ sf_length_8kHz ];\r
232             C[ 0 ][ d ] += (SKP_float)(cross_corr / sqrt( normalizer ));\r
233         }\r
234         /* Update target pointer */\r
235         target_ptr += sf_length_8kHz;\r
236     }\r
237 \r
238     /* Apply short-lag bias */\r
239     for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {\r
240         C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;\r
241     }\r
242 \r
243     /* Sort */\r
244     length_d_srch = 4 + 2 * complexity;\r
245     SKP_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );\r
246     SKP_Silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );\r
247 \r
248     /* Escape if correlation is very low already here */\r
249     Cmax = C[ 0 ][ min_lag_4kHz ];\r
250     target_ptr = &signal_4kHz[ SKP_SMULBB( sf_length_4kHz, nb_subfr ) ];\r
251     energy = 1000.0f;\r
252     for( i = 0; i < SKP_LSHIFT( sf_length_4kHz, 2 ); i++ ) {\r
253         energy += target_ptr[i] * target_ptr[i];\r
254     }\r
255     threshold = Cmax * Cmax; \r
256     if( energy / 16.0f > threshold ) {\r
257         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
258         *LTPCorr      = 0.0f;\r
259         *lagIndex     = 0;\r
260         *contourIndex = 0;\r
261         return 1;\r
262     }\r
263 \r
264     threshold = search_thres1 * Cmax;\r
265     for( i = 0; i < length_d_srch; i++ ) {\r
266         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */\r
267         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {\r
268             d_srch[ i ] = SKP_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );\r
269         } else {\r
270             length_d_srch = i;\r
271             break;\r
272         }\r
273     }\r
274     SKP_assert( length_d_srch > 0 );\r
275 \r
276     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {\r
277         d_comp[ i ] = 0;\r
278     }\r
279     for( i = 0; i < length_d_srch; i++ ) {\r
280         d_comp[ d_srch[ i ] ] = 1;\r
281     }\r
282 \r
283     /* Convolution */\r
284     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
285         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];\r
286     }\r
287 \r
288     length_d_srch = 0;\r
289     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {    \r
290         if( d_comp[ i + 1 ] > 0 ) {\r
291             d_srch[ length_d_srch ] = i;\r
292             length_d_srch++;\r
293         }\r
294     }\r
295 \r
296     /* Convolution */\r
297     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
298         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];\r
299     }\r
300 \r
301     length_d_comp = 0;\r
302     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {    \r
303         if( d_comp[ i ] > 0 ) {\r
304             d_comp[ length_d_comp ] = i - 2;\r
305             length_d_comp++;\r
306         }\r
307     }\r
308 \r
309     /**********************************************************************************\r
310     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation\r
311     *************************************************************************************/\r
312     /********************************************************************************* \r
313     * Find energy of each subframe projected onto its history, for a range of delays\r
314     *********************************************************************************/\r
315     SKP_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(SKP_float)); // Is this needed?\r
316     \r
317     target_ptr = &signal_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];\r
318     for( k = 0; k < nb_subfr; k++ ) {\r
319 \r
320         /* Check that we are within range of the array */\r
321         SKP_assert( target_ptr >= signal_8kHz );\r
322         SKP_assert( target_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
323 \r
324         energy_tmp = SKP_Silk_energy_FLP( target_ptr, sf_length_8kHz );\r
325         for( j = 0; j < length_d_comp; j++ ) {\r
326             d = d_comp[ j ];\r
327             basis_ptr = target_ptr - d;\r
328 \r
329             /* Check that we are within range of the array */\r
330             SKP_assert( basis_ptr >= signal_8kHz );\r
331             SKP_assert( basis_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
332         \r
333             cross_corr = SKP_Silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );\r
334             energy     = SKP_Silk_energy_FLP( basis_ptr, sf_length_8kHz );\r
335             if (cross_corr > 0.0f) {\r
336                 C[ k ][ d ] = (SKP_float)(cross_corr * cross_corr / (energy * energy_tmp + eps));\r
337             } else {\r
338                 C[ k ][ d ] = 0.0f;\r
339             }\r
340         }\r
341         target_ptr += sf_length_8kHz;\r
342     }\r
343 \r
344     /* search over lag range and lags codebook */\r
345     /* scale factor for lag codebook, as a function of center lag */\r
346 \r
347     CCmax   = 0.0f; /* This value doesn't matter */\r
348     CCmax_b = -1000.0f;\r
349 \r
350     CBimax = 0; /* To avoid returning undefined lag values */\r
351     lag = -1;   /* To check if lag with strong enough correlation has been found */\r
352 \r
353     if( prevLag > 0 ) {\r
354         if( Fs_kHz == 12 ) {\r
355             prevLag = SKP_LSHIFT( prevLag, 1 ) / 3;\r
356         } else if( Fs_kHz == 16 ) {\r
357             prevLag = SKP_RSHIFT( prevLag, 1 );\r
358         } else if( Fs_kHz == 24 ) {\r
359             prevLag = prevLag / 3;\r
360         }\r
361         prevLag_log2 = SKP_P_log2((SKP_float)prevLag);\r
362     } else {\r
363         prevLag_log2 = 0;\r
364     }\r
365 \r
366     /* Setup stage 2 codebook based on number of subframes */\r
367     if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
368         cbk_size   = PE_NB_CBKS_STAGE2_EXT;\r
369         Lag_CB_ptr = &SKP_Silk_CB_lags_stage2[ 0 ][ 0 ];\r
370         if( Fs_kHz == 8 && complexity > SigProc_PE_MIN_COMPLEX ) {\r
371             /* If input is 8 khz use a larger codebook here because it is last stage */\r
372             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;\r
373         } else {\r
374             nb_cbk_search = PE_NB_CBKS_STAGE2;\r
375         }\r
376     } else {\r
377         cbk_size       = PE_NB_CBKS_STAGE2_10MS;\r
378         Lag_CB_ptr     = &SKP_Silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];\r
379         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;\r
380     }\r
381 \r
382     for( k = 0; k < length_d_srch; k++ ) {\r
383         d = d_srch[ k ];\r
384         for( j = 0; j < nb_cbk_search; j++ ) {\r
385             CC[j] = 0.0f;\r
386             for( i = 0; i < nb_subfr; i++ ) {\r
387                 /* Try all codebooks */\r
388                 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];\r
389             }\r
390         }\r
391         /* Find best codebook */\r
392         CCmax_new  = -1000.0f;\r
393         CBimax_new = 0;\r
394         for( i = 0; i < nb_cbk_search; i++ ) {\r
395             if( CC[ i ] > CCmax_new ) {\r
396                 CCmax_new = CC[ i ];\r
397                 CBimax_new = i;\r
398             }\r
399         }\r
400         CCmax_new = SKP_max_float(CCmax_new, 0.0f); /* To avoid taking square root of negative number later */\r
401         CCmax_new_b = CCmax_new;\r
402 \r
403         /* Bias towards shorter lags */\r
404         lag_log2 = SKP_P_log2((SKP_float)d);\r
405         CCmax_new_b -= PE_FLP_SHORTLAG_BIAS * nb_subfr * lag_log2;\r
406 \r
407         /* Bias towards previous lag */\r
408         if ( prevLag > 0 ) {\r
409             delta_lag_log2_sqr = lag_log2 - prevLag_log2;\r
410             delta_lag_log2_sqr *= delta_lag_log2_sqr;\r
411             CCmax_new_b -= PE_FLP_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / (delta_lag_log2_sqr + 0.5f);\r
412         }\r
413 \r
414         if ( CCmax_new_b > CCmax_b                                      && /* Find maximum biased correlation                  */\r
415              CCmax_new > nb_subfr * search_thres2 * search_thres2       && /* Correlation needs to be high enough to be voiced */\r
416              SKP_Silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz    /* Lag must be in range                             */\r
417             ) {\r
418             CCmax_b = CCmax_new_b;\r
419             CCmax   = CCmax_new;\r
420             lag     = d;\r
421             CBimax  = CBimax_new;\r
422         }\r
423     }\r
424 \r
425     if( lag == -1 ) {\r
426         /* No suitable candidate found */\r
427         SKP_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(SKP_int) );\r
428         *LTPCorr      = 0.0f;\r
429         *lagIndex     = 0;\r
430         *contourIndex = 0;\r
431         return 1;\r
432     }\r
433 \r
434     if( Fs_kHz > 8 ) {\r
435         /* Search in original signal */\r
436 \r
437         /* Compensate for decimation */\r
438         SKP_assert( lag == SKP_SAT16( lag ) );\r
439         if( Fs_kHz == 12 ) {\r
440             lag = SKP_RSHIFT_ROUND( SKP_SMULBB( lag, 3 ), 1 );\r
441         } else if( Fs_kHz == 16 ) {\r
442             lag = SKP_LSHIFT( lag, 1 );\r
443         } else {\r
444             lag = SKP_SMULBB( lag, 3 );\r
445         }\r
446 \r
447         lag = SKP_LIMIT_int( lag, min_lag, max_lag );\r
448         start_lag = SKP_max_int( lag - 2, min_lag );\r
449         end_lag   = SKP_min_int( lag + 2, max_lag );\r
450         lag_new   = lag;                                    /* to avoid undefined lag */\r
451         CBimax    = 0;                                      /* to avoid undefined lag */\r
452         SKP_assert( CCmax >= 0.0f ); \r
453         *LTPCorr = (SKP_float)sqrt( CCmax / nb_subfr );     /* Output normalized correlation */\r
454 \r
455         CCmax = -1000.0f;\r
456 \r
457         /* Calculate the correlations and energies needed in stage 3 */\r
458         SKP_P_Ana_calc_corr_st3( cross_corr_st3, signal, start_lag, sf_length, nb_subfr, complexity );\r
459         SKP_P_Ana_calc_energy_st3( energies_st3, signal, start_lag, sf_length, nb_subfr, complexity );\r
460 \r
461         lag_counter = 0;\r
462         SKP_assert( lag == SKP_SAT16( lag ) );\r
463         contour_bias = PE_FLP_FLATCONTOUR_BIAS / lag;\r
464 \r
465         /* Setup cbk parameters acording to complexity setting and frame length */\r
466         if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
467             nb_cbk_search   = (SKP_int)SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
468             cbk_size        = PE_NB_CBKS_STAGE3_MAX;\r
469             cbk_offset      = (SKP_int)SKP_Silk_cbk_offsets_stage3[ complexity ];\r
470             Lag_CB_ptr      = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
471         } else {\r
472             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;\r
473             cbk_size    = PE_NB_CBKS_STAGE3_10MS;\r
474             cbk_offset = 0;\r
475             Lag_CB_ptr = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
476         }\r
477 \r
478         for( d = start_lag; d <= end_lag; d++ ) {\r
479             for( j = cbk_offset; j < ( cbk_offset + nb_cbk_search ); j++ ) {\r
480                 cross_corr = 0.0;\r
481                 energy = eps;\r
482                 for( k = 0; k < nb_subfr; k++ ) {\r
483                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];\r
484                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];\r
485                 }\r
486                 if( cross_corr > 0.0 ) {\r
487                     CCmax_new = (SKP_float)(cross_corr * cross_corr / energy);\r
488                     /* Reduce depending on flatness of contour */\r
489                     diff = j - SKP_RSHIFT( cbk_size, 1 );\r
490                     CCmax_new *= ( 1.0f - contour_bias * diff * diff );\r
491                 } else {\r
492                     CCmax_new = 0.0f;               \r
493                 }\r
494 \r
495                 if( CCmax_new > CCmax &&\r
496                    ( d + (SKP_int)SKP_Silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag  \r
497                    ) {\r
498                     CCmax   = CCmax_new;\r
499                     lag_new = d;\r
500                     CBimax  = j;\r
501                 }\r
502             }\r
503             lag_counter++;\r
504         }\r
505 \r
506         for( k = 0; k < nb_subfr; k++ ) {\r
507             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );\r
508         }\r
509         *lagIndex = lag_new - min_lag;\r
510         *contourIndex = CBimax;\r
511     } else {\r
512         /* Save Lags and correlation */\r
513         SKP_assert( CCmax >= 0.0f );\r
514         *LTPCorr = (SKP_float)sqrt( CCmax / nb_subfr ); /* Output normalized correlation */\r
515         for( k = 0; k < nb_subfr; k++ ) {\r
516             pitch_out[ k ] = lag + SKP_Silk_CB_lags_stage2[ k ][ CBimax ];\r
517         }\r
518         *lagIndex = lag - min_lag;\r
519         *contourIndex = CBimax;\r
520     }\r
521     SKP_assert( *lagIndex >= 0 );\r
522     /* return as voiced */\r
523     return 0;\r
524 }\r
525 \r
526 static void SKP_P_Ana_calc_corr_st3(\r
527     SKP_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
528     const SKP_float signal[],           /* I vector to correlate                                            */\r
529     SKP_int         start_lag,          /* I start lag                                                      */\r
530     SKP_int         sf_length,          /* I sub frame length                                               */\r
531     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
532     SKP_int         complexity          /* I Complexity setting                                             */\r
533 )\r
534     /***********************************************************************\r
535      Calculates the correlations used in stage 3 search. In order to cover \r
536      the whole lag codebook for all the searched offset lags (lag +- 2), \r
537      the following correlations are needed in each sub frame:\r
538 \r
539      sf1: lag range [-8,...,7] total 16 correlations\r
540      sf2: lag range [-4,...,4] total 9 correlations\r
541      sf3: lag range [-3,....4] total 8 correltions\r
542      sf4: lag range [-6,....8] total 15 correlations\r
543 \r
544      In total 48 correlations. The direct implementation computed in worst case \r
545      4*12*5 = 240 correlations, but more likely around 120. \r
546      **********************************************************************/\r
547 {\r
548     const SKP_float *target_ptr, *basis_ptr;\r
549     SKP_int   i, j, k, lag_counter, lag_low, lag_high;\r
550     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size;\r
551     SKP_float scratch_mem[ SCRATCH_SIZE ];\r
552     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
553 \r
554     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
555     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
556 \r
557     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
558         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
559         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
560         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
561         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
562         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
563     } else {\r
564         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
565         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
566         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
567         cbk_offset    = 0;\r
568         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
569         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
570     }\r
571 \r
572     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */\r
573     for( k = 0; k < nb_subfr; k++ ) {\r
574         lag_counter = 0;\r
575 \r
576         /* Calculate the correlations for each subframe */\r
577         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
578         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );\r
579         for( j = lag_low; j <= lag_high; j++ ) {\r
580             basis_ptr = target_ptr - ( start_lag + j );\r
581             SKP_assert( lag_counter < SCRATCH_SIZE );\r
582             scratch_mem[ lag_counter ] = (SKP_float)SKP_Silk_inner_product_FLP( target_ptr, basis_ptr, sf_length );\r
583             lag_counter++;\r
584         }\r
585 \r
586         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
587         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
588             /* Fill out the 3 dim array that stores the correlations for */\r
589             /* each code_book vector for each start lag */\r
590             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
591             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
592                 SKP_assert( idx + j < SCRATCH_SIZE );\r
593                 SKP_assert( idx + j < lag_counter );\r
594                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
595             }\r
596         }\r
597         target_ptr += sf_length;\r
598     }\r
599 }\r
600 \r
601 static void SKP_P_Ana_calc_energy_st3(\r
602     SKP_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
603     const SKP_float signal[],           /* I vector to correlate                                            */\r
604     SKP_int         start_lag,          /* I start lag                                                      */\r
605     SKP_int         sf_length,          /* I sub frame length                                               */\r
606     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
607     SKP_int         complexity          /* I Complexity setting                                             */\r
608 )\r
609 /****************************************************************\r
610 Calculate the energies for first two subframes. The energies are\r
611 calculated recursively. \r
612 ****************************************************************/\r
613 {\r
614     const SKP_float *target_ptr, *basis_ptr;\r
615     double    energy;\r
616     SKP_int   k, i, j, lag_counter;\r
617     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size, lag_diff;\r
618     SKP_float scratch_mem[ SCRATCH_SIZE ];\r
619     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
620 \r
621     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
622     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
623 \r
624     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
625         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
626         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
627         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
628         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
629         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
630     } else {\r
631         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
632         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
633         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
634         cbk_offset    = 0;\r
635         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
636         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
637     }\r
638 \r
639     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ];\r
640     for( k = 0; k < nb_subfr; k++ ) {\r
641         lag_counter = 0;\r
642 \r
643         /* Calculate the energy for first lag */\r
644         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );\r
645         energy = SKP_Silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;\r
646         SKP_assert( energy >= 0.0 );\r
647         scratch_mem[lag_counter] = (SKP_float)energy;\r
648         lag_counter++;\r
649 \r
650         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );\r
651         for( i = 1; i < lag_diff; i++ ) {\r
652             /* remove part outside new window */\r
653             energy -= basis_ptr[sf_length - i] * basis_ptr[sf_length - i];\r
654             SKP_assert( energy >= 0.0 );\r
655 \r
656             /* add part that comes into window */\r
657             energy += basis_ptr[ -i ] * basis_ptr[ -i ];\r
658             SKP_assert( energy >= 0.0 );\r
659             SKP_assert( lag_counter < SCRATCH_SIZE );\r
660             scratch_mem[lag_counter] = (SKP_float)energy;\r
661             lag_counter++;\r
662         }\r
663 \r
664         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
665         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
666             /* Fill out the 3 dim array that stores the correlations for    */\r
667             /* each code_book vector for each start lag                     */\r
668             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
669             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
670                 SKP_assert( idx + j < SCRATCH_SIZE );\r
671                 SKP_assert( idx + j < lag_counter );\r
672                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
673                 SKP_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );\r
674             }\r
675         }\r
676         target_ptr += sf_length;\r
677     }\r
678 }\r