Big SILK update
[opus.git] / src_FIX / SKP_Silk_NLSF_MSVQ_encode_FIX.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 #include "SKP_Silk_main_FIX.h"\r
29 \r
30 /***********************/\r
31 /* NLSF vector encoder */\r
32 /***********************/\r
33 void SKP_Silk_NLSF_MSVQ_encode_FIX(\r
34           SKP_int                   *NLSFIndices,           /* O    Codebook path vector [ CB_STAGES ]      */\r
35           SKP_int                   *pNLSF_Q15,             /* I/O  Quantized NLSF vector [ LPC_ORDER ]     */\r
36     const SKP_Silk_NLSF_CB_struct   *psNLSF_CB,             /* I    Codebook object                         */\r
37     const SKP_int                   *pNLSF_q_Q15_prev,      /* I    Prev. quantized NLSF vector [LPC_ORDER] */\r
38     const SKP_int                   *pW_Q6,                 /* I    NLSF weight vector [ LPC_ORDER ]        */\r
39     const SKP_int                   NLSF_mu_Q15,            /* I    Rate weight for the RD optimization     */\r
40     const SKP_int                   NLSF_mu_fluc_red_Q16,   /* I    Fluctuation reduction error weight      */\r
41     const SKP_int                   NLSF_MSVQ_Survivors,    /* I    Max survivors from each stage           */\r
42     const SKP_int                   LPC_order,              /* I    LPC order                               */\r
43     const SKP_int                   deactivate_fluc_red     /* I    Deactivate fluctuation reduction        */\r
44 )\r
45 {\r
46     SKP_int     i, s, k, cur_survivors = 0, prev_survivors, min_survivors, input_index, cb_index, bestIndex;\r
47     SKP_int32   rateDistThreshold_Q18;\r
48 #if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 1 )\r
49     SKP_int32   se_Q15, wsse_Q20, bestRateDist_Q20;\r
50 #endif\r
51 \r
52 #if( LOW_COMPLEXITY_ONLY == 1 )\r
53     SKP_int32   pRateDist_Q18[  NLSF_MSVQ_TREE_SEARCH_MAX_VECTORS_EVALUATED_LC_MODE ];\r
54     SKP_int32   pRate_Q5[       MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];\r
55     SKP_int32   pRate_new_Q5[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];\r
56     SKP_int     pTempIndices[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];\r
57     SKP_int     pPath[          MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * NLSF_MSVQ_MAX_CB_STAGES ];\r
58     SKP_int     pPath_new[      MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * NLSF_MSVQ_MAX_CB_STAGES ];\r
59     SKP_int     pRes_Q15[       MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * MAX_LPC_ORDER ];\r
60     SKP_int     pRes_new_Q15[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * MAX_LPC_ORDER ];\r
61 #else\r
62     SKP_int32   pRateDist_Q18[  NLSF_MSVQ_TREE_SEARCH_MAX_VECTORS_EVALUATED ];\r
63     SKP_int32   pRate_Q5[       MAX_NLSF_MSVQ_SURVIVORS ];\r
64     SKP_int32   pRate_new_Q5[   MAX_NLSF_MSVQ_SURVIVORS ];\r
65     SKP_int     pTempIndices[   MAX_NLSF_MSVQ_SURVIVORS ];\r
66     SKP_int     pPath[          MAX_NLSF_MSVQ_SURVIVORS * NLSF_MSVQ_MAX_CB_STAGES ];\r
67     SKP_int     pPath_new[      MAX_NLSF_MSVQ_SURVIVORS * NLSF_MSVQ_MAX_CB_STAGES ];\r
68     SKP_int     pRes_Q15[       MAX_NLSF_MSVQ_SURVIVORS * MAX_LPC_ORDER ];\r
69     SKP_int     pRes_new_Q15[   MAX_NLSF_MSVQ_SURVIVORS * MAX_LPC_ORDER ];\r
70 #endif\r
71 \r
72     const SKP_int   *pConstInt;\r
73           SKP_int   *pInt;\r
74     const SKP_int8  *pCB_element;\r
75     const SKP_Silk_NLSF_CBS *pCurrentCBStage;\r
76 \r
77     SKP_assert( NLSF_MSVQ_Survivors <= MAX_NLSF_MSVQ_SURVIVORS );\r
78     SKP_assert( ( LOW_COMPLEXITY_ONLY == 0 ) || ( NLSF_MSVQ_Survivors <= MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ) );\r
79 \r
80 #ifdef SAVE_ALL_INTERNAL_DATA\r
81     /* Use sigtype.dat to seperate into signal types */\r
82     DEBUG_STORE_DATA( NLSF.dat,    pNLSF_Q15,    LPC_order * sizeof( SKP_int   ) );\r
83     DEBUG_STORE_DATA( WNLSF.dat,   pW_Q6,        LPC_order * sizeof( SKP_int   ) );\r
84     DEBUG_STORE_DATA( NLSF_mu.dat, &NLSF_mu_Q15,             sizeof( SKP_int32 ) );\r
85 #endif\r
86 \r
87     /****************************************************/\r
88     /* Tree search for the multi-stage vector quantizer */\r
89     /****************************************************/\r
90 \r
91     /* Clear accumulated rates */\r
92     SKP_memset( pRate_Q5, 0, NLSF_MSVQ_Survivors * sizeof( SKP_int32 ) );\r
93 \r
94     /* Subtract 1/2 from NLSF input vector to create initial residual */\r
95     for( i = 0; i < LPC_order; i++ ) {\r
96         pRes_Q15[ i ] = pNLSF_Q15[ i ] - SKP_FIX_CONST( 0.5f, 15 );\r
97     }\r
98 \r
99     /* Set first stage values */\r
100     prev_survivors = 1;\r
101 \r
102     /* Minimum number of survivors */\r
103     min_survivors = NLSF_MSVQ_Survivors / 2;\r
104 \r
105     /* Loop over all stages */\r
106     for( s = 0; s < psNLSF_CB->nStages; s++ ) {\r
107 \r
108         /* Set a pointer to the current stage codebook */\r
109         pCurrentCBStage = &psNLSF_CB->CBStages[ s ];\r
110 \r
111         /* Calculate the number of survivors in the current stage */\r
112         cur_survivors = SKP_min_32( NLSF_MSVQ_Survivors, SKP_SMULBB( prev_survivors, pCurrentCBStage->nVectors ) );\r
113 \r
114 #if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 0 )\r
115         /* Find a single best survivor in the last stage, if we */\r
116         /* do not need candidates for fluctuation reduction     */\r
117         if( s == psNLSF_CB->nStages - 1 ) {\r
118             cur_survivors = 1;\r
119         }\r
120 #endif\r
121 \r
122         /* Nearest neighbor clustering for multiple input data vectors */\r
123         SKP_Silk_NLSF_VQ_rate_distortion_FIX( pRateDist_Q18, pCurrentCBStage, pRes_Q15, pW_Q6, \r
124             pRate_Q5, NLSF_mu_Q15, prev_survivors, LPC_order );\r
125 \r
126         /* Sort the rate-distortion errors */\r
127         SKP_Silk_insertion_sort_increasing( pRateDist_Q18, pTempIndices, \r
128             prev_survivors * pCurrentCBStage->nVectors, cur_survivors );\r
129 \r
130         /* Discard survivors with rate-distortion values too far above the best one */\r
131         if( pRateDist_Q18[ 0 ] < SKP_int32_MAX / MAX_NLSF_MSVQ_SURVIVORS ) {\r
132             rateDistThreshold_Q18 = SKP_SMLAWB( pRateDist_Q18[ 0 ], \r
133                 SKP_MUL( NLSF_MSVQ_Survivors, pRateDist_Q18[ 0 ] ), SKP_FIX_CONST( NLSF_MSVQ_SURV_MAX_REL_RD, 16 ) );\r
134             while( pRateDist_Q18[ cur_survivors - 1 ] > rateDistThreshold_Q18 && cur_survivors > min_survivors ) {\r
135                 cur_survivors--;\r
136             }\r
137         }\r
138         /* Update accumulated codebook contributions for the 'cur_survivors' best codebook indices */\r
139         for( k = 0; k < cur_survivors; k++ ) { \r
140             if( s > 0 ) {\r
141                 /* Find the indices of the input and the codebook vector */\r
142                 if( pCurrentCBStage->nVectors == 8 ) {\r
143                     input_index = SKP_RSHIFT( pTempIndices[ k ], 3 );\r
144                     cb_index    = pTempIndices[ k ] & 7;\r
145                 } else {\r
146                     input_index = SKP_DIV32_16( pTempIndices[ k ], pCurrentCBStage->nVectors );  \r
147                     cb_index    = pTempIndices[ k ] - SKP_SMULBB( input_index, pCurrentCBStage->nVectors );\r
148                 }\r
149             } else {\r
150                 /* Find the indices of the input and the codebook vector */\r
151                 input_index = 0;\r
152                 cb_index    = pTempIndices[ k ];\r
153             }\r
154 \r
155             /* Subtract new contribution from the previous residual vector for each of 'cur_survivors' */\r
156             pConstInt   = &pRes_Q15[ SKP_SMULBB( input_index, LPC_order ) ];\r
157             pCB_element = &pCurrentCBStage->CB_NLSF_Q8[ SKP_SMULBB( cb_index, LPC_order ) ];\r
158             pInt        = &pRes_new_Q15[ SKP_SMULBB( k, LPC_order ) ];\r
159             for( i = 0; i < LPC_order; i++ ) {\r
160                 pInt[ i ] = pConstInt[ i ] - SKP_LSHIFT16( ( SKP_int )pCB_element[ i ], 7 );\r
161             }\r
162 \r
163             /* Update accumulated rate for stage 1 to the current */\r
164             pRate_new_Q5[ k ] = pRate_Q5[ input_index ] + SKP_LSHIFT32( ( SKP_int32 )pCurrentCBStage->Rates_Q4[ cb_index ], 1 );\r
165 \r
166             /* Copy paths from previous matrix, starting with the best path */\r
167             pConstInt = &pPath[ SKP_SMULBB( input_index, psNLSF_CB->nStages ) ];\r
168             pInt      = &pPath_new[ SKP_SMULBB( k, psNLSF_CB->nStages ) ];\r
169             for( i = 0; i < s; i++ ) {\r
170                 pInt[ i ] = pConstInt[ i ];\r
171             }\r
172             /* Write the current stage indices for the 'cur_survivors' to the best path matrix */\r
173             pInt[ s ] = cb_index;\r
174         }\r
175 \r
176         if( s < psNLSF_CB->nStages - 1 ) {\r
177             /* Copy NLSF residual matrix for next stage */\r
178             SKP_memcpy( pRes_Q15, pRes_new_Q15, SKP_SMULBB( cur_survivors, LPC_order ) * sizeof( SKP_int ) );\r
179 \r
180             /* Copy rate vector for next stage */\r
181             SKP_memcpy( pRate_Q5, pRate_new_Q5, cur_survivors * sizeof( SKP_int32 ) );\r
182 \r
183             /* Copy best path matrix for next stage */\r
184             SKP_memcpy( pPath, pPath_new, SKP_SMULBB( cur_survivors, psNLSF_CB->nStages ) * sizeof( SKP_int ) );\r
185         }\r
186 \r
187         prev_survivors = cur_survivors;\r
188     }\r
189 \r
190     /* (Preliminary) index of the best survivor, later to be decoded */\r
191     bestIndex = 0;\r
192 \r
193 #if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 1 )\r
194     /******************************/\r
195     /* NLSF fluctuation reduction */\r
196     /******************************/\r
197     if( deactivate_fluc_red != 1 ) {\r
198     \r
199         /* Search among all survivors, now taking also weighted fluctuation errors into account */\r
200         bestRateDist_Q20 = SKP_int32_MAX;\r
201         for( s = 0; s < cur_survivors; s++ ) {\r
202             /* Decode survivor to compare with previous quantized NLSF vector */\r
203             SKP_Silk_NLSF_MSVQ_decode( pNLSF_Q15, psNLSF_CB, &pPath_new[ SKP_SMULBB( s, psNLSF_CB->nStages ) ], LPC_order );\r
204 \r
205             /* Compare decoded NLSF vector with the previously quantized vector */ \r
206             wsse_Q20 = 0;\r
207             for( i = 0; i < LPC_order; i += 2 ) {\r
208                 /* Compute weighted squared quantization error for index i */\r
209                 se_Q15 = pNLSF_Q15[ i ] - pNLSF_q_Q15_prev[ i ]; // range: [ -32767 : 32767 ]\r
210                 wsse_Q20 = SKP_SMLAWB( wsse_Q20, SKP_SMULBB( se_Q15, se_Q15 ), pW_Q6[ i ] );\r
211 \r
212                 /* Compute weighted squared quantization error for index i + 1 */\r
213                 se_Q15 = pNLSF_Q15[ i + 1 ] - pNLSF_q_Q15_prev[ i + 1 ]; // range: [ -32767 : 32767 ]\r
214                 wsse_Q20 = SKP_SMLAWB( wsse_Q20, SKP_SMULBB( se_Q15, se_Q15 ), pW_Q6[ i + 1 ] );\r
215             }\r
216             SKP_assert( wsse_Q20 >= 0 );\r
217 \r
218             /* Add the fluctuation reduction penalty to the rate distortion error */\r
219             wsse_Q20 = SKP_ADD_POS_SAT32( pRateDist_Q18[ s ], SKP_SMULWB( wsse_Q20, NLSF_mu_fluc_red_Q16 ) );\r
220 \r
221             /* Keep index of best survivor */\r
222             if( wsse_Q20 < bestRateDist_Q20 ) {\r
223                 bestRateDist_Q20 = wsse_Q20;\r
224                 bestIndex = s;\r
225             }\r
226         }\r
227     }\r
228 #endif\r
229 \r
230     /* Copy best path to output argument */\r
231     SKP_memcpy( NLSFIndices, &pPath_new[ SKP_SMULBB( bestIndex, psNLSF_CB->nStages ) ], psNLSF_CB->nStages * sizeof( SKP_int ) );\r
232 \r
233     /* Decode and stabilize the best survivor */\r
234     SKP_Silk_NLSF_MSVQ_decode( pNLSF_Q15, psNLSF_CB, NLSFIndices, LPC_order );\r
235 \r
236 #ifdef SAVE_ALL_INTERNAL_DATA\r
237     {\r
238         SKP_float rateBPF_LSF;\r
239         SKP_float NLSF_coef;\r
240 \r
241         rateBPF_LSF = (SKP_float)pRate_new_Q5[ bestIndex ] / 32.0f; // Q5 -> Q0\r
242         DEBUG_STORE_DATA( rateBPF_LSF.dat, &rateBPF_LSF, sizeof( SKP_float ));\r
243         for( i = 0; i < LPC_order; i++ ) {\r
244             NLSF_coef = ( (SKP_float)pNLSF_Q15[ i ] ) * ( 1.0f / 32768.0f );\r
245             DEBUG_STORE_DATA( NLSFq.dat, &NLSF_coef, sizeof( SKP_float ) );\r
246         }\r
247     }\r
248 #endif\r
249 }\r