void SKP_Silk_noise_shape_analysis_FIX(
    SKP_Silk_encoder_state_FIX      *psEnc,         /* I/O  Encoder state FIX                           */
    SKP_Silk_encoder_control_FIX    *psEncCtrl,     /* I/O  Encoder control FIX                         */
    const SKP_int16                 *pitch_res,     /* I    LPC residual from pitch analysis            */
    const SKP_int16                 *x              /* I    Input signal [ frame_length + la_shape ]    */
)
{
    SKP_Silk_shape_state_FIX *psShapeSt = &psEnc->sShape;
    SKP_int     k, i, nSamples, Qnrg, b_Q14, warping_Q16, scale = 0;
    SKP_int32   SNR_adj_dB_Q7, HarmBoost_Q16, HarmShapeGain_Q16, Tilt_Q16, tmp32;
    SKP_int32   nrg, pre_nrg_Q30, log_energy_Q7, log_energy_prev_Q7, energy_variation_Q7;
    SKP_int32   delta_Q16, BWExp1_Q16, BWExp2_Q16, gain_mult_Q16, gain_add_Q16, strength_Q16, b_Q8;
    SKP_int32   auto_corr[     MAX_SHAPE_LPC_ORDER + 1 ];
    SKP_int32   refl_coef_Q16[ MAX_SHAPE_LPC_ORDER ];
    SKP_int32   AR1_Q24[       MAX_SHAPE_LPC_ORDER ];
    SKP_int32   AR2_Q24[       MAX_SHAPE_LPC_ORDER ];
    SKP_int16   x_windowed[    SHAPE_LPC_WIN_MAX ];
    SKP_int32   sqrt_nrg[ MAX_NB_SUBFR ], Qnrg_vec[ MAX_NB_SUBFR ];
    const SKP_int16 *x_ptr, *pitch_res_ptr;

    /* Point to start of first LPC analysis block */
    x_ptr = x - psEnc->sCmn.la_shape;

    /****************/
    /* CONTROL SNR  */
    /****************/
    /* Reduce SNR_dB values if recent bitstream has exceeded TargetRate */
    psEncCtrl->current_SNR_dB_Q7 = psEnc->SNR_dB_Q7 - SKP_SMULBB( psEnc->BufferedInChannel_ms, SKP_FIX_CONST( 0.1, 7 ) );

    /* Reduce SNR_dB because of any inband FEC used */
    psEncCtrl->current_SNR_dB_Q7 -= psEnc->inBandFEC_SNR_comp_Q7;

    /****************/
    /* GAIN CONTROL */
    /****************/
    /* Input quality is the average of the quality in the lowest two VAD bands */
    psEncCtrl->input_quality_Q14 = ( SKP_int )SKP_RSHIFT( ( SKP_int32 )psEnc->sCmn.input_quality_bands_Q15[ 0 ] 
        + psEnc->sCmn.input_quality_bands_Q15[ 1 ], 2 );

    /* Coding quality level, between 0.0_Q0 and 1.0_Q0, but in Q14 */
    psEncCtrl->coding_quality_Q14 = SKP_RSHIFT( SKP_Silk_sigm_Q15( SKP_RSHIFT_ROUND( psEncCtrl->current_SNR_dB_Q7 - 
        SKP_FIX_CONST( 18.0, 7 ), 4 ) ), 1 );

    /* Reduce coding SNR during low speech activity */
    SNR_adj_dB_Q7 = psEncCtrl->current_SNR_dB_Q7;
    if( psEnc->sCmn.useCBR == 0 ) {
        b_Q8 = SKP_FIX_CONST( 1.0, 8 ) - psEnc->sCmn.speech_activity_Q8;
        b_Q8 = SKP_SMULWB( SKP_LSHIFT( b_Q8, 8 ), b_Q8 );
        SNR_adj_dB_Q7 = SKP_SMLAWB( SNR_adj_dB_Q7,
            SKP_SMULBB( SKP_FIX_CONST( -BG_SNR_DECR_dB, 7 ) >> ( 4 + 1 ), b_Q8 ),                                       // Q11
            SKP_SMULWB( SKP_FIX_CONST( 1.0, 14 ) + psEncCtrl->input_quality_Q14, psEncCtrl->coding_quality_Q14 ) );     // Q12
    }
/* non-warped frequency scale. (So that it can be implemented with a minimum-phase monic filter.)   */
SKP_INLINE SKP_int32 warped_gain( // gain in Q16
    const SKP_int32     *coefs_Q24, 
    SKP_int             lambda_Q16, 
    SKP_int             order 
) {
    SKP_int   i;
    SKP_int32 gain_Q24;

    lambda_Q16 = -lambda_Q16;
    gain_Q24 = coefs_Q24[ order - 1 ];
    for( i = order - 2; i >= 0; i-- ) {
        gain_Q24 = SKP_SMLAWB( coefs_Q24[ i ], gain_Q24, lambda_Q16 );
    }
    gain_Q24  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), gain_Q24, -lambda_Q16 );
    return SKP_INVERSE32_varQ( gain_Q24, 40 );
}
/* amplitude of monic warped coefficients by using bandwidth expansion on the true coefficients */
SKP_INLINE void limit_warped_coefs( 
    SKP_int32           *coefs_syn_Q24,
    SKP_int32           *coefs_ana_Q24,
    SKP_int             lambda_Q16,
    SKP_int32           limit_Q24,
    SKP_int             order
) {
    SKP_int   i, iter, ind = 0;
    SKP_int32 tmp, maxabs_Q24, chirp_Q16, gain_syn_Q16, gain_ana_Q16;
    SKP_int32 nom_Q16, den_Q24;

    /* Convert to monic coefficients */
    lambda_Q16 = -lambda_Q16;
    for( i = order - 1; i > 0; i-- ) {
        coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
        coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
    }
    lambda_Q16 = -lambda_Q16;
    nom_Q16  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 16 ), -lambda_Q16,        lambda_Q16 );
    den_Q24  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 );
    gain_syn_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 );
    den_Q24  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 );
    gain_ana_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 );
    for( i = 0; i < order; i++ ) {
        coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
        coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
    }

    for( iter = 0; iter < 10; iter++ ) {
        /* Find maximum absolute value */
        maxabs_Q24 = -1;
        for( i = 0; i < order; i++ ) {
            tmp = SKP_max( SKP_abs_int32( coefs_syn_Q24[ i ] ), SKP_abs_int32( coefs_ana_Q24[ i ] ) );
            if( tmp > maxabs_Q24 ) {
                maxabs_Q24 = tmp;
                ind = i;
            }
        }
        if( maxabs_Q24 <= limit_Q24 ) {
            /* Coefficients are within range - done */
            return;
        }

        /* Convert back to true warped coefficients */
        for( i = 1; i < order; i++ ) {
            coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
            coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
        }
        gain_syn_Q16 = SKP_INVERSE32_varQ( gain_syn_Q16, 32 );
        gain_ana_Q16 = SKP_INVERSE32_varQ( gain_ana_Q16, 32 );
        for( i = 0; i < order; i++ ) {
            coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
            coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
        }

        /* Apply bandwidth expansion */
        chirp_Q16 = SKP_FIX_CONST( 0.99, 16 ) - SKP_DIV32_varQ(
            SKP_SMULWB( maxabs_Q24 - limit_Q24, SKP_SMLABB( SKP_FIX_CONST( 0.8, 10 ), SKP_FIX_CONST( 0.1, 10 ), iter ) ), 
            SKP_MUL( maxabs_Q24, ind + 1 ), 22 );
        SKP_Silk_bwexpander_32( coefs_syn_Q24, order, chirp_Q16 );
        SKP_Silk_bwexpander_32( coefs_ana_Q24, order, chirp_Q16 );

        /* Convert to monic warped coefficients */
        lambda_Q16 = -lambda_Q16;
        for( i = order - 1; i > 0; i-- ) {
            coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 );
            coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 );
        }
        lambda_Q16 = -lambda_Q16;
        nom_Q16  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 16 ), -lambda_Q16,        lambda_Q16 );
        den_Q24  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 );
        gain_syn_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 );
        den_Q24  = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 );
        gain_ana_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 );
        for( i = 0; i < order; i++ ) {
            coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] );
            coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] );
        }
    }
	SKP_assert( 0 );
}
/* High-pass filter with cutoff frequency adaptation based on pitch lag statistics */
void SKP_Silk_HP_variable_cutoff_FIX(
    SKP_Silk_encoder_state_FIX      *psEnc,             /* I/O  Encoder state FIX                           */
    SKP_Silk_encoder_control_FIX    *psEncCtrl,         /* I/O  Encoder control FIX                         */
    SKP_int16                       *out,               /* O    high-pass filtered output signal            */
    const SKP_int16                 *in                 /* I    input signal                                */
)
{
    SKP_int   quality_Q15;
    SKP_int32 B_Q28[ 3 ], A_Q28[ 2 ];
    SKP_int32 Fc_Q19, r_Q28, r_Q22;
    SKP_int32 pitch_freq_Hz_Q16, pitch_freq_log_Q7, delta_freq_Q7;

    /*********************************************/
    /* Estimate Low End of Pitch Frequency Range */
    /*********************************************/
    if( psEnc->sCmn.prev_sigtype == SIG_TYPE_VOICED ) {
        /* difference, in log domain */
        pitch_freq_Hz_Q16 = SKP_DIV32_16( SKP_LSHIFT( SKP_MUL( psEnc->sCmn.fs_kHz, 1000 ), 16 ), psEnc->sCmn.prevLag );
        pitch_freq_log_Q7 = SKP_Silk_lin2log( pitch_freq_Hz_Q16 ) - ( 16 << 7 ); //0x70

        /* adjustment based on quality */
        quality_Q15 = psEncCtrl->input_quality_bands_Q15[ 0 ];
        pitch_freq_log_Q7 = SKP_SUB32( pitch_freq_log_Q7, SKP_SMULWB( SKP_SMULWB( SKP_LSHIFT( quality_Q15, 2 ), quality_Q15 ), 
            pitch_freq_log_Q7 - SKP_LOG2_VARIABLE_HP_MIN_FREQ_Q7 ) );
        pitch_freq_log_Q7 = SKP_ADD32( pitch_freq_log_Q7, SKP_RSHIFT( SKP_FIX_CONST( 0.6, 15 ) - quality_Q15, 9 ) );

        //delta_freq = pitch_freq_log - psEnc->variable_HP_smth1;
        delta_freq_Q7 = pitch_freq_log_Q7 - SKP_RSHIFT( psEnc->variable_HP_smth1_Q15, 8 );
        if( delta_freq_Q7 < 0 ) {
            /* less smoothing for decreasing pitch frequency, to track something close to the minimum */
            delta_freq_Q7 = SKP_MUL( delta_freq_Q7, 3 );
        }

        /* limit delta, to reduce impact of outliers */
        delta_freq_Q7 = SKP_LIMIT_32( delta_freq_Q7, -SKP_FIX_CONST( VARIABLE_HP_MAX_DELTA_FREQ, 7 ), SKP_FIX_CONST( VARIABLE_HP_MAX_DELTA_FREQ, 7 ) );

        /* update smoother */
        psEnc->variable_HP_smth1_Q15 = SKP_SMLAWB( psEnc->variable_HP_smth1_Q15, 
            SKP_MUL( SKP_LSHIFT( psEnc->speech_activity_Q8, 1 ), delta_freq_Q7 ), SKP_FIX_CONST( VARIABLE_HP_SMTH_COEF1, 16 ) );
    }
    /* second smoother */
    psEnc->variable_HP_smth2_Q15 = SKP_SMLAWB( psEnc->variable_HP_smth2_Q15, 
        psEnc->variable_HP_smth1_Q15 - psEnc->variable_HP_smth2_Q15, SKP_FIX_CONST( VARIABLE_HP_SMTH_COEF2, 16 ) );

    /* convert from log scale to Hertz */
    psEncCtrl->pitch_freq_low_Hz = SKP_Silk_log2lin( SKP_RSHIFT( psEnc->variable_HP_smth2_Q15, 8 ) );

    /* limit frequency range */
    psEncCtrl->pitch_freq_low_Hz = SKP_LIMIT_32( psEncCtrl->pitch_freq_low_Hz, 
        SKP_FIX_CONST( VARIABLE_HP_MIN_FREQ, 0 ), SKP_FIX_CONST( VARIABLE_HP_MAX_FREQ, 0 ) );

    /********************************/
    /* Compute Filter Coefficients  */
    /********************************/
    /* compute cut-off frequency, in radians */
    //Fc_num   = (SKP_float)( 0.45f * 2.0f * 3.14159265359 * psEncCtrl->pitch_freq_low_Hz );
    //Fc_denom = (SKP_float)( 1e3f * psEnc->sCmn.fs_kHz );
    SKP_assert( psEncCtrl->pitch_freq_low_Hz <= SKP_int32_MAX / SKP_RADIANS_CONSTANT_Q19 );
    Fc_Q19 = SKP_DIV32_16( SKP_SMULBB( SKP_RADIANS_CONSTANT_Q19, psEncCtrl->pitch_freq_low_Hz ), psEnc->sCmn.fs_kHz ); // range: 3704 - 27787, 11-15 bits
    SKP_assert( Fc_Q19 >=  3704 );
    SKP_assert( Fc_Q19 <= 27787 );

    r_Q28 = SKP_FIX_CONST( 1.0, 28 ) - SKP_MUL( SKP_FIX_CONST( 0.92, 9 ), Fc_Q19 );
    SKP_assert( r_Q28 >= 255347779 );
    SKP_assert( r_Q28 <= 266690872 );

    /* b = r * [ 1; -2; 1 ]; */
    /* a = [ 1; -2 * r * ( 1 - 0.5 * Fc^2 ); r^2 ]; */
    B_Q28[ 0 ] = r_Q28;
    B_Q28[ 1 ] = SKP_LSHIFT( -r_Q28, 1 );
    B_Q28[ 2 ] = r_Q28;
    
    // -r * ( 2 - Fc * Fc );
    r_Q22  = SKP_RSHIFT( r_Q28, 6 );
    A_Q28[ 0 ] = SKP_SMULWW( r_Q22, SKP_SMULWW( Fc_Q19, Fc_Q19 ) - SKP_FIX_CONST( 2.0,  22 ) );
    A_Q28[ 1 ] = SKP_SMULWW( r_Q22, r_Q22 );

    /********************************/
    /* High-Pass Filter             */
    /********************************/
    SKP_Silk_biquad_alt( in, B_Q28, A_Q28, psEnc->sCmn.In_HP_State, out, psEnc->sCmn.frame_length );
}
/* Find pitch lags */
void SKP_Silk_find_pitch_lags_FIX(
    SKP_Silk_encoder_state_FIX      *psEnc,         /* I/O  encoder state                               */
    SKP_Silk_encoder_control_FIX    *psEncCtrl,     /* I/O  encoder control                             */
    SKP_int16                       res[],          /* O    residual                                    */
    const SKP_int16                 x[]             /* I    Speech signal                               */
)
{
    SKP_Silk_predict_state_FIX *psPredSt = &psEnc->sPred;
    SKP_int   buf_len, i, scale;
    SKP_int32 thrhld_Q15, res_nrg;
    const SKP_int16 *x_buf, *x_buf_ptr;
    SKP_int16 Wsig[      FIND_PITCH_LPC_WIN_MAX ], *Wsig_ptr;
    SKP_int32 auto_corr[ MAX_FIND_PITCH_LPC_ORDER + 1 ];
    SKP_int16 rc_Q15[    MAX_FIND_PITCH_LPC_ORDER ];
    SKP_int32 A_Q24[     MAX_FIND_PITCH_LPC_ORDER ];
    SKP_int32 FiltState[ MAX_FIND_PITCH_LPC_ORDER ];
    SKP_int16 A_Q12[     MAX_FIND_PITCH_LPC_ORDER ];

    /******************************************/
    /* Setup buffer lengths etc based on Fs   */
    /******************************************/
    buf_len = SKP_ADD_LSHIFT( psEnc->sCmn.la_pitch, psEnc->sCmn.frame_length, 1 );

    /* Safty check */
    SKP_assert( buf_len >= psPredSt->pitch_LPC_win_length );

    x_buf = x - psEnc->sCmn.frame_length;

    /*************************************/
    /* Estimate LPC AR coefficients      */
    /*************************************/

    /* Calculate windowed signal */

    /* First LA_LTP samples */
    x_buf_ptr = x_buf + buf_len - psPredSt->pitch_LPC_win_length;
    Wsig_ptr  = Wsig;
    SKP_Silk_apply_sine_window_new( Wsig_ptr, x_buf_ptr, 1, psEnc->sCmn.la_pitch );

    /* Middle un - windowed samples */
    Wsig_ptr  += psEnc->sCmn.la_pitch;
    x_buf_ptr += psEnc->sCmn.la_pitch;
    SKP_memcpy( Wsig_ptr, x_buf_ptr, ( psPredSt->pitch_LPC_win_length - SKP_LSHIFT( psEnc->sCmn.la_pitch, 1 ) ) * sizeof( SKP_int16 ) );

    /* Last LA_LTP samples */
    Wsig_ptr  += psPredSt->pitch_LPC_win_length - SKP_LSHIFT( psEnc->sCmn.la_pitch, 1 );
    x_buf_ptr += psPredSt->pitch_LPC_win_length - SKP_LSHIFT( psEnc->sCmn.la_pitch, 1 );
    SKP_Silk_apply_sine_window_new( Wsig_ptr, x_buf_ptr, 2, psEnc->sCmn.la_pitch );

    /* Calculate autocorrelation sequence */
    SKP_Silk_autocorr( auto_corr, &scale, Wsig, psPredSt->pitch_LPC_win_length, psEnc->sCmn.pitchEstimationLPCOrder + 1 );

    /* Add white noise, as fraction of energy */
    auto_corr[ 0 ] = SKP_SMLAWB( auto_corr[ 0 ], auto_corr[ 0 ], SKP_FIX_CONST( FIND_PITCH_WHITE_NOISE_FRACTION, 16 ) );

    /* Calculate the reflection coefficients using schur */
    res_nrg = SKP_Silk_schur( rc_Q15, auto_corr, psEnc->sCmn.pitchEstimationLPCOrder );

    /* Prediction gain */
    psEncCtrl->predGain_Q16 = SKP_DIV32_varQ( auto_corr[ 0 ], SKP_max_int( res_nrg, 1 ), 16 );

    /* Convert reflection coefficients to prediction coefficients */
    SKP_Silk_k2a( A_Q24, rc_Q15, psEnc->sCmn.pitchEstimationLPCOrder );

    /* Convert From 32 bit Q24 to 16 bit Q12 coefs */
    for( i = 0; i < psEnc->sCmn.pitchEstimationLPCOrder; i++ ) {
        A_Q12[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT( A_Q24[ i ], 12 ) );
    }

    /* Do BWE */
    SKP_Silk_bwexpander( A_Q12, psEnc->sCmn.pitchEstimationLPCOrder, SKP_FIX_CONST( FIND_PITCH_BANDWITH_EXPANSION, 16 ) );

    /*****************************************/
    /* LPC analysis filtering                */
    /*****************************************/
    SKP_memset( FiltState, 0, psEnc->sCmn.pitchEstimationLPCOrder * sizeof( SKP_int32 ) ); /* Not really necessary, but Valgrind will complain otherwise */
    SKP_Silk_MA_Prediction( x_buf, A_Q12, FiltState, res, buf_len, psEnc->sCmn.pitchEstimationLPCOrder );
    SKP_memset( res, 0, psEnc->sCmn.pitchEstimationLPCOrder * sizeof( SKP_int16 ) );

    /* Threshold for pitch estimator */
    thrhld_Q15 = SKP_FIX_CONST( 0.45, 15 );
    thrhld_Q15 = SKP_SMLABB( thrhld_Q15, SKP_FIX_CONST( -0.004, 15 ), psEnc->sCmn.pitchEstimationLPCOrder );
    thrhld_Q15 = SKP_SMLABB( thrhld_Q15, SKP_FIX_CONST( -0.1,   7  ), psEnc->speech_activity_Q8 );
    thrhld_Q15 = SKP_SMLABB( thrhld_Q15, SKP_FIX_CONST(  0.15,  15 ), psEnc->sCmn.prev_sigtype );
    thrhld_Q15 = SKP_SMLAWB( thrhld_Q15, SKP_FIX_CONST( -0.1,   16 ), psEncCtrl->input_tilt_Q15 );
    thrhld_Q15 = SKP_SAT16(  thrhld_Q15 );

    /*****************************************/
    /* Call pitch estimator                  */
    /*****************************************/
    psEncCtrl->sCmn.sigtype = SKP_Silk_pitch_analysis_core( res, psEncCtrl->sCmn.pitchL, &psEncCtrl->sCmn.lagIndex,
        &psEncCtrl->sCmn.contourIndex, &psEnc->LTPCorr_Q15, psEnc->sCmn.prevLag, psEnc->sCmn.pitchEstimationThreshold_Q16,
        ( SKP_int16 )thrhld_Q15, psEnc->sCmn.fs_kHz, psEnc->sCmn.pitchEstimationComplexity, SKP_FALSE );
}
SKP_INLINE void SKP_Silk_LDL_factorize_FIX(
    SKP_int32           *A,         /* I   Pointer to Symetric Square Matrix */
    SKP_int             M,          /* I   Size of Matrix */
    SKP_int32           *L_Q16,     /* I/O Pointer to Square Upper triangular Matrix */
    inv_D_t             *inv_D      /* I/O Pointer to vector holding inverted diagonal elements of D */
)
{
    SKP_int   i, j, k, status, loop_count;
    const SKP_int32 *ptr1, *ptr2;
    SKP_int32 diag_min_value, tmp_32, err;
    SKP_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ];
    SKP_int32 one_div_diag_Q36, one_div_diag_Q40, one_div_diag_Q48;

    SKP_assert( M <= MAX_MATRIX_SIZE );

    status = 1;
    diag_min_value = SKP_max_32( SKP_SMMUL( SKP_ADD_SAT32( A[ 0 ], A[ SKP_SMULBB( M, M ) - 1 ] ), SKP_FIX_CONST( FIND_LTP_COND_FAC, 31 ) ), 1 << 9 );
    for( loop_count = 0; loop_count < M && status == 1; loop_count++ ) {
        status = 0;
        for( j = 0; j < M; j++ ) {
            ptr1 = matrix_adr( L_Q16, j, 0, M );
            tmp_32 = 0;
            for( i = 0; i < j; i++ ) {
                v_Q0[ i ] = SKP_SMULWW(         D_Q0[ i ], ptr1[ i ] ); /* Q0 */
                tmp_32    = SKP_SMLAWW( tmp_32, v_Q0[ i ], ptr1[ i ] ); /* Q0 */
            }
            tmp_32 = SKP_SUB32( matrix_ptr( A, j, j, M ), tmp_32 );

            if( tmp_32 < diag_min_value ) {
                tmp_32 = SKP_SUB32( SKP_SMULBB( loop_count + 1, diag_min_value ), tmp_32 );
                /* Matrix not positive semi-definite, or ill conditioned */
                for( i = 0; i < M; i++ ) {
                    matrix_ptr( A, i, i, M ) = SKP_ADD32( matrix_ptr( A, i, i, M ), tmp_32 );
                }
                status = 1;
                break;
            }
            D_Q0[ j ] = tmp_32;                         /* always < max(Correlation) */
        
            /* two-step division */
            one_div_diag_Q36 = SKP_INVERSE32_varQ( tmp_32, 36 );                    /* Q36 */
            one_div_diag_Q40 = SKP_LSHIFT( one_div_diag_Q36, 4 );                   /* Q40 */
            err = SKP_SUB32( 1 << 24, SKP_SMULWW( tmp_32, one_div_diag_Q40 ) );     /* Q24 */
            one_div_diag_Q48 = SKP_SMULWW( err, one_div_diag_Q40 );                 /* Q48 */

            /* Save 1/Ds */
            inv_D[ j ].Q36_part = one_div_diag_Q36;
            inv_D[ j ].Q48_part = one_div_diag_Q48;

            matrix_ptr( L_Q16, j, j, M ) = 65536; /* 1.0 in Q16 */
            ptr1 = matrix_adr( A, j, 0, M );
            ptr2 = matrix_adr( L_Q16, j + 1, 0, M );
            for( i = j + 1; i < M; i++ ) { 
                tmp_32 = 0;
                for( k = 0; k < j; k++ ) {
                    tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ k ], ptr2[ k ] ); /* Q0 */
                }
                tmp_32 = SKP_SUB32( ptr1[ i ], tmp_32 ); /* always < max(Correlation) */

                /* tmp_32 / D_Q0[j] : Divide to Q16 */
                matrix_ptr( L_Q16, i, j, M ) = SKP_ADD32( SKP_SMMUL( tmp_32, one_div_diag_Q48 ),
                    SKP_RSHIFT( SKP_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );

                /* go to next column */
                ptr2 += M; 
            }
        }
    }

    SKP_assert( status == 0 );
}
        /* Convert to monic warped prediction coefficients and limit absolute values */
        limit_warped_coefs( AR2_Q24, AR1_Q24, warping_Q16, SKP_FIX_CONST( 3.999, 24 ), psEnc->sCmn.shapingLPCOrder );

        /* Convert from Q24 to Q13 and store in int16 */
        for( i = 0; i < psEnc->sCmn.shapingLPCOrder; i++ ) {
            psEncCtrl->AR1_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (SKP_int16)SKP_SAT16( SKP_RSHIFT_ROUND( AR1_Q24[ i ], 11 ) );
            psEncCtrl->AR2_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (SKP_int16)SKP_SAT16( SKP_RSHIFT_ROUND( AR2_Q24[ i ], 11 ) );
        }
    }

    /*****************/
    /* Gain tweaking */
    /*****************/
    /* Increase gains during low speech activity and put lower limit on gains */
    gain_mult_Q16 = SKP_Silk_log2lin( -SKP_SMLAWB( -SKP_FIX_CONST( 16.0, 7 ), SNR_adj_dB_Q7,                            SKP_FIX_CONST( 0.16, 16 ) ) );
    gain_add_Q16  = SKP_Silk_log2lin(  SKP_SMLAWB(  SKP_FIX_CONST( 16.0, 7 ), SKP_FIX_CONST( NOISE_FLOOR_dB, 7 ),       SKP_FIX_CONST( 0.16, 16 ) ) );
    tmp32         = SKP_Silk_log2lin(  SKP_SMLAWB(  SKP_FIX_CONST( 16.0, 7 ), SKP_FIX_CONST( RELATIVE_MIN_GAIN_dB, 7 ), SKP_FIX_CONST( 0.16, 16 ) ) );
    tmp32 = SKP_SMULWW( psEnc->avgGain_Q16, tmp32 );
    gain_add_Q16 = SKP_ADD_SAT32( gain_add_Q16, tmp32 );
    SKP_assert( gain_mult_Q16 >= 0 );

    for( k = 0; k < NB_SUBFR; k++ ) {
        psEncCtrl->Gains_Q16[ k ] = SKP_SMULWW( psEncCtrl->Gains_Q16[ k ], gain_mult_Q16 );
        if( psEncCtrl->Gains_Q16[ k ] < 0 ) {
            psEncCtrl->Gains_Q16[ k ] = SKP_int32_MAX;
        }
    }

    for( k = 0; k < NB_SUBFR; k++ ) {
        psEncCtrl->Gains_Q16[ k ] = SKP_ADD_POS_SAT32( psEncCtrl->Gains_Q16[ k ], gain_add_Q16 );
void SKP_Silk_noise_shape_analysis_FIX(
    SKP_Silk_encoder_state_FIX      *psEnc,         /* I/O  Encoder state FIX                           */
    SKP_Silk_encoder_control_FIX    *psEncCtrl,     /* I/O  Encoder control FIX                         */
    const SKP_int16                 *pitch_res,     /* I    LPC residual from pitch analysis            */
    const SKP_int16                 *x              /* I    Input signal [ frame_length + la_shape ]    */
)
{
    SKP_Silk_shape_state_FIX *psShapeSt = &psEnc->sShape;
    SKP_int     k, i, nSamples, Qnrg, b_Q14, warping_Q16, scale = 0;
    SKP_int32   SNR_adj_dB_Q7, HarmBoost_Q16, HarmShapeGain_Q16, Tilt_Q16, tmp32;
    SKP_int32   nrg, pre_nrg_Q30, log_energy_Q7, log_energy_prev_Q7, energy_variation_Q7;
    SKP_int32   delta_Q16, BWExp1_Q16, BWExp2_Q16, gain_mult_Q16, gain_add_Q16, strength_Q16, b_Q8;
    SKP_int32   auto_corr[     MAX_SHAPE_LPC_ORDER + 1 ];
    SKP_int32   refl_coef_Q16[ MAX_SHAPE_LPC_ORDER ];
    SKP_int32   AR1_Q24[       MAX_SHAPE_LPC_ORDER ];
    SKP_int32   AR2_Q24[       MAX_SHAPE_LPC_ORDER ];
    SKP_int16   x_windowed[    SHAPE_LPC_WIN_MAX ];
    const SKP_int16 *x_ptr, *pitch_res_ptr;

    SKP_int32   sqrt_nrg[ NB_SUBFR ], Qnrg_vec[ NB_SUBFR ];

    /* Point to start of first LPC analysis block */
    x_ptr = x - psEnc->sCmn.la_shape;

    /****************/
    /* CONTROL SNR  */
    /****************/
    /* Reduce SNR_dB values if recent bitstream has exceeded TargetRate */
    psEncCtrl->current_SNR_dB_Q7 = psEnc->SNR_dB_Q7 - SKP_SMULWB( SKP_LSHIFT( ( SKP_int32 )psEnc->BufferedInChannel_ms, 7 ), 
        SKP_FIX_CONST( 0.05, 16 ) );

    /* Reduce SNR_dB if inband FEC used */
    if( psEnc->speech_activity_Q8 > SKP_FIX_CONST( LBRR_SPEECH_ACTIVITY_THRES, 8 ) ) {
        psEncCtrl->current_SNR_dB_Q7 -= SKP_RSHIFT( psEnc->inBandFEC_SNR_comp_Q8, 1 );
    }

    /****************/
    /* GAIN CONTROL */
    /****************/
    /* Input quality is the average of the quality in the lowest two VAD bands */
    psEncCtrl->input_quality_Q14 = ( SKP_int )SKP_RSHIFT( ( SKP_int32 )psEncCtrl->input_quality_bands_Q15[ 0 ] 
        + psEncCtrl->input_quality_bands_Q15[ 1 ], 2 );

    /* Coding quality level, between 0.0_Q0 and 1.0_Q0, but in Q14 */
    psEncCtrl->coding_quality_Q14 = SKP_RSHIFT( SKP_Silk_sigm_Q15( SKP_RSHIFT_ROUND( psEncCtrl->current_SNR_dB_Q7 - 
        SKP_FIX_CONST( 18.0, 7 ), 4 ) ), 1 );

    /* Reduce coding SNR during low speech activity */
    b_Q8 = SKP_FIX_CONST( 1.0, 8 ) - psEnc->speech_activity_Q8;
    b_Q8 = SKP_SMULWB( SKP_LSHIFT( b_Q8, 8 ), b_Q8 );
    SNR_adj_dB_Q7 = SKP_SMLAWB( psEncCtrl->current_SNR_dB_Q7,
        SKP_SMULBB( SKP_FIX_CONST( -BG_SNR_DECR_dB, 7 ) >> ( 4 + 1 ), b_Q8 ),                                       // Q11
        SKP_SMULWB( SKP_FIX_CONST( 1.0, 14 ) + psEncCtrl->input_quality_Q14, psEncCtrl->coding_quality_Q14 ) );     // Q12

    if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
        /* Reduce gains for periodic signals */
        SNR_adj_dB_Q7 = SKP_SMLAWB( SNR_adj_dB_Q7, SKP_FIX_CONST( HARM_SNR_INCR_dB, 8 ), psEnc->LTPCorr_Q15 );
    } else { 
        /* For unvoiced signals and low-quality input, adjust the quality slower than SNR_dB setting */
        SNR_adj_dB_Q7 = SKP_SMLAWB( SNR_adj_dB_Q7, 
            SKP_SMLAWB( SKP_FIX_CONST( 6.0, 9 ), -SKP_FIX_CONST( 0.4, 18 ), psEncCtrl->current_SNR_dB_Q7 ),
            SKP_FIX_CONST( 1.0, 14 ) - psEncCtrl->input_quality_Q14 );
    }

    /*************************/
    /* SPARSENESS PROCESSING */
    /*************************/
    /* Set quantizer offset */
    if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
        /* Initally set to 0; may be overruled in process_gains(..) */
        psEncCtrl->sCmn.QuantOffsetType = 0;
        psEncCtrl->sparseness_Q8 = 0;
    } else {
        /* Sparseness measure, based on relative fluctuations of energy per 2 milliseconds */
        nSamples = SKP_LSHIFT( psEnc->sCmn.fs_kHz, 1 );
        energy_variation_Q7 = 0;
        log_energy_prev_Q7  = 0;
        pitch_res_ptr = pitch_res;
        for( k = 0; k < FRAME_LENGTH_MS / 2; k++ ) {    
            SKP_Silk_sum_sqr_shift( &nrg, &scale, pitch_res_ptr, nSamples );
            nrg += SKP_RSHIFT( nSamples, scale );           // Q(-scale)
            
            log_energy_Q7 = SKP_Silk_lin2log( nrg );
            if( k > 0 ) {
                energy_variation_Q7 += SKP_abs( log_energy_Q7 - log_energy_prev_Q7 );
            }
            log_energy_prev_Q7 = log_energy_Q7;
            pitch_res_ptr += nSamples;
        }

        psEncCtrl->sparseness_Q8 = SKP_RSHIFT( SKP_Silk_sigm_Q15( SKP_SMULWB( energy_variation_Q7 - 
            SKP_FIX_CONST( 5.0, 7 ), SKP_FIX_CONST( 0.1, 16 ) ) ), 7 );

        /* Set quantization offset depending on sparseness measure */
        if( psEncCtrl->sparseness_Q8 > SKP_FIX_CONST( SPARSENESS_THRESHOLD_QNT_OFFSET, 8 ) ) {
            psEncCtrl->sCmn.QuantOffsetType = 0;
        } else {
            psEncCtrl->sCmn.QuantOffsetType = 1;
        }
        
        /* Increase coding SNR for sparse signals */
        SNR_adj_dB_Q7 = SKP_SMLAWB( SNR_adj_dB_Q7, SKP_FIX_CONST( SPARSE_SNR_INCR_dB, 15 ), psEncCtrl->sparseness_Q8 - SKP_FIX_CONST( 0.5, 8 ) );
    }

    /*******************************/
    /* Control bandwidth expansion */
    /*******************************/
    /* More BWE for signals with high prediction gain */
    strength_Q16 = SKP_SMULWB( psEncCtrl->predGain_Q16, SKP_FIX_CONST( FIND_PITCH_WHITE_NOISE_FRACTION, 16 ) );
    BWExp1_Q16 = BWExp2_Q16 = SKP_DIV32_varQ( SKP_FIX_CONST( BANDWIDTH_EXPANSION, 16 ), 
        SKP_SMLAWW( SKP_FIX_CONST( 1.0, 16 ), strength_Q16, strength_Q16 ), 16 );
    delta_Q16  = SKP_SMULWB( SKP_FIX_CONST( 1.0, 16 ) - SKP_SMULBB( 3, psEncCtrl->coding_quality_Q14 ), 
        SKP_FIX_CONST( LOW_RATE_BANDWIDTH_EXPANSION_DELTA, 16 ) );
    BWExp1_Q16 = SKP_SUB32( BWExp1_Q16, delta_Q16 );
    BWExp2_Q16 = SKP_ADD32( BWExp2_Q16, delta_Q16 );
    /* BWExp1 will be applied after BWExp2, so make it relative */
    BWExp1_Q16 = SKP_DIV32_16( SKP_LSHIFT( BWExp1_Q16, 14 ), SKP_RSHIFT( BWExp2_Q16, 2 ) );

    if( psEnc->sCmn.warping_Q16 > 0 ) {
        /* Slightly more warping in analysis will move quantization noise up in frequency, where it's better masked */
        warping_Q16 = SKP_SMLAWB( psEnc->sCmn.warping_Q16, psEncCtrl->coding_quality_Q14, SKP_FIX_CONST( 0.01, 18 ) );
    } else {
        warping_Q16 = 0;
    }

    /********************************************/
    /* Compute noise shaping AR coefs and gains */
    /********************************************/
    for( k = 0; k < NB_SUBFR; k++ ) {
        /* Apply window: sine slope followed by flat part followed by cosine slope */
        SKP_int shift, slope_part, flat_part;
        flat_part = psEnc->sCmn.fs_kHz * 5;
        slope_part = SKP_RSHIFT( psEnc->sCmn.shapeWinLength - flat_part, 1 );

        SKP_Silk_apply_sine_window_new( x_windowed, x_ptr, 1, slope_part );
        shift = slope_part;
        SKP_memcpy( x_windowed + shift, x_ptr + shift, flat_part * sizeof(SKP_int16) );
        shift += flat_part;
        SKP_Silk_apply_sine_window_new( x_windowed + shift, x_ptr + shift, 2, slope_part );
        
        /* Update pointer: next LPC analysis block */
        x_ptr += psEnc->sCmn.subfr_length;

        if( psEnc->sCmn.warping_Q16 > 0 ) {
            /* Calculate warped auto correlation */
            SKP_Silk_warped_autocorrelation_FIX( auto_corr, &scale, x_windowed, warping_Q16, psEnc->sCmn.shapeWinLength, psEnc->sCmn.shapingLPCOrder ); 
        } else {
            /* Calculate regular auto correlation */
            SKP_Silk_autocorr( auto_corr, &scale, x_windowed, psEnc->sCmn.shapeWinLength, psEnc->sCmn.shapingLPCOrder + 1 );
        }

        /* Add white noise, as a fraction of energy */
        auto_corr[0] = SKP_ADD32( auto_corr[0], SKP_max_32( SKP_SMULWB( SKP_RSHIFT( auto_corr[ 0 ], 4 ), 
            SKP_FIX_CONST( SHAPE_WHITE_NOISE_FRACTION, 20 ) ), 1 ) ); 

        /* Calculate the reflection coefficients using schur */
        nrg = SKP_Silk_schur64( refl_coef_Q16, auto_corr, psEnc->sCmn.shapingLPCOrder );
        SKP_assert( nrg >= 0 );

        /* Convert reflection coefficients to prediction coefficients */
        SKP_Silk_k2a_Q16( AR2_Q24, refl_coef_Q16, psEnc->sCmn.shapingLPCOrder );

        Qnrg = -scale;          // range: -12...30
        SKP_assert( Qnrg >= -12 );
        SKP_assert( Qnrg <=  30 );

        /* Make sure that Qnrg is an even number */
        if( Qnrg & 1 ) {
            Qnrg -= 1;
            nrg >>= 1;
        }

        tmp32 = SKP_Silk_SQRT_APPROX( nrg );
        Qnrg >>= 1;             // range: -6...15

        sqrt_nrg[ k ] = tmp32;
        Qnrg_vec[ k ] = Qnrg;

        psEncCtrl->Gains_Q16[ k ] = SKP_LSHIFT_SAT32( tmp32, 16 - Qnrg );

        if( psEnc->sCmn.warping_Q16 > 0 ) {
            /* Adjust gain for warping */
            gain_mult_Q16 = warped_gain( AR2_Q24, warping_Q16, psEnc->sCmn.shapingLPCOrder );
            SKP_assert( psEncCtrl->Gains_Q16[ k ] >= 0 );
            psEncCtrl->Gains_Q16[ k ] = SKP_SMULWW( psEncCtrl->Gains_Q16[ k ], gain_mult_Q16 );
            if( psEncCtrl->Gains_Q16[ k ] < 0 ) {
                psEncCtrl->Gains_Q16[ k ] = SKP_int32_MAX;
            }
        }

        /* Bandwidth expansion for synthesis filter shaping */
        SKP_Silk_bwexpander_32( AR2_Q24, psEnc->sCmn.shapingLPCOrder, BWExp2_Q16 );

        /* Compute noise shaping filter coefficients */
        SKP_memcpy( AR1_Q24, AR2_Q24, psEnc->sCmn.shapingLPCOrder * sizeof( SKP_int32 ) );

        /* Bandwidth expansion for analysis filter shaping */
        SKP_assert( BWExp1_Q16 <= SKP_FIX_CONST( 1.0, 16 ) );
        SKP_Silk_bwexpander_32( AR1_Q24, psEnc->sCmn.shapingLPCOrder, BWExp1_Q16 );

        /* Ratio of prediction gains, in energy domain */
        SKP_Silk_LPC_inverse_pred_gain_Q24( &pre_nrg_Q30, AR2_Q24, psEnc->sCmn.shapingLPCOrder );
        SKP_Silk_LPC_inverse_pred_gain_Q24( &nrg,         AR1_Q24, psEnc->sCmn.shapingLPCOrder );

        //psEncCtrl->GainsPre[ k ] = 1.0f - 0.7f * ( 1.0f - pre_nrg / nrg ) = 0.3f + 0.7f * pre_nrg / nrg;
        pre_nrg_Q30 = SKP_LSHIFT32( SKP_SMULWB( pre_nrg_Q30, SKP_FIX_CONST( 0.7, 15 ) ), 1 );
        psEncCtrl->GainsPre_Q14[ k ] = ( SKP_int ) SKP_FIX_CONST( 0.3, 14 ) + SKP_DIV32_varQ( pre_nrg_Q30, nrg, 14 );

        /* Convert to monic warped prediction coefficients and limit absolute values */
        limit_warped_coefs( AR2_Q24, AR1_Q24, warping_Q16, SKP_FIX_CONST( 3.999, 24 ), psEnc->sCmn.shapingLPCOrder );

        /* Convert from Q24 to Q13 and store in int16 */
        for( i = 0; i < psEnc->sCmn.shapingLPCOrder; i++ ) {
            psEncCtrl->AR1_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (SKP_int16)SKP_SAT16( SKP_RSHIFT_ROUND( AR1_Q24[ i ], 11 ) );
            psEncCtrl->AR2_Q13[ k * MAX_SHAPE_LPC_ORDER + i ] = (SKP_int16)SKP_SAT16( SKP_RSHIFT_ROUND( AR2_Q24[ i ], 11 ) );
        }
    }
void SKP_Silk_prefilter_FIX(
    SKP_Silk_encoder_state_FIX          *psEnc,         /* I/O  Encoder state FIX                           */
    const SKP_Silk_encoder_control_FIX  *psEncCtrl,     /* I    Encoder control FIX                         */
    SKP_int16                           xw[],           /* O    Weighted signal                             */
    const SKP_int16                     x[]             /* I    Speech signal                               */
)
{
    SKP_Silk_prefilter_state_FIX *P = &psEnc->sPrefilt;
    SKP_int   j, k, lag;
    SKP_int32 tmp_32;
    const SKP_int16 *AR1_shp_Q13;
    const SKP_int16 *px;
    SKP_int16 *pxw;
    SKP_int   HarmShapeGain_Q12, Tilt_Q14;
    SKP_int32 HarmShapeFIRPacked_Q12, LF_shp_Q14;
    SKP_int32 x_filt_Q12[ MAX_FRAME_LENGTH / NB_SUBFR ];
    SKP_int16 st_res[ ( MAX_FRAME_LENGTH / NB_SUBFR ) + MAX_SHAPE_LPC_ORDER ];
    SKP_int16 B_Q12[ 2 ];

    /* Setup pointers */
    px  = x;
    pxw = xw;
    lag = P->lagPrev;
    for( k = 0; k < NB_SUBFR; k++ ) {
        /* Update Variables that change per sub frame */
        if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
            lag = psEncCtrl->sCmn.pitchL[ k ];
        }

        /* Noise shape parameters */
        HarmShapeGain_Q12 = SKP_SMULWB( psEncCtrl->HarmShapeGain_Q14[ k ], 16384 - psEncCtrl->HarmBoost_Q14[ k ] );
        SKP_assert( HarmShapeGain_Q12 >= 0 );
        HarmShapeFIRPacked_Q12  =                          SKP_RSHIFT( HarmShapeGain_Q12, 2 );
        HarmShapeFIRPacked_Q12 |= SKP_LSHIFT( ( SKP_int32 )SKP_RSHIFT( HarmShapeGain_Q12, 1 ), 16 );
        Tilt_Q14    = psEncCtrl->Tilt_Q14[   k ];
        LF_shp_Q14  = psEncCtrl->LF_shp_Q14[ k ];
        AR1_shp_Q13 = &psEncCtrl->AR1_Q13[   k * MAX_SHAPE_LPC_ORDER ];

        /* Short term FIR filtering*/
        SKP_Silk_warped_LPC_analysis_filter_FIX( P->sAR_shp, st_res, AR1_shp_Q13, px, 
            psEnc->sCmn.warping_Q16, psEnc->sCmn.subfr_length, psEnc->sCmn.shapingLPCOrder );

        /* reduce (mainly) low frequencies during harmonic emphasis */
        B_Q12[ 0 ] = SKP_RSHIFT_ROUND( psEncCtrl->GainsPre_Q14[ k ], 2 );
        tmp_32 = SKP_SMLABB( SKP_FIX_CONST( INPUT_TILT, 26 ), psEncCtrl->HarmBoost_Q14[ k ], HarmShapeGain_Q12 );   /* Q26 */
        tmp_32 = SKP_SMLABB( tmp_32, psEncCtrl->coding_quality_Q14, SKP_FIX_CONST( HIGH_RATE_INPUT_TILT, 12 ) );    /* Q26 */
        tmp_32 = SKP_SMULWB( tmp_32, -psEncCtrl->GainsPre_Q14[ k ] );                                               /* Q24 */
        tmp_32 = SKP_RSHIFT_ROUND( tmp_32, 12 );                                                                    /* Q12 */
        B_Q12[ 1 ]= SKP_SAT16( tmp_32 );

        x_filt_Q12[ 0 ] = SKP_SMLABB( SKP_SMULBB( st_res[ 0 ], B_Q12[ 0 ] ), P->sHarmHP, B_Q12[ 1 ] );
        for( j = 1; j < psEnc->sCmn.subfr_length; j++ ) {
            x_filt_Q12[ j ] = SKP_SMLABB( SKP_SMULBB( st_res[ j ], B_Q12[ 0 ] ), st_res[ j - 1 ], B_Q12[ 1 ] );
        }
        P->sHarmHP = st_res[ psEnc->sCmn.subfr_length - 1 ];

        SKP_Silk_prefilt_FIX( P, x_filt_Q12, pxw, HarmShapeFIRPacked_Q12, Tilt_Q14, 
            LF_shp_Q14, lag, psEnc->sCmn.subfr_length );

        px  += psEnc->sCmn.subfr_length;
        pxw += psEnc->sCmn.subfr_length;
    }

    P->lagPrev = psEncCtrl->sCmn.pitchL[ NB_SUBFR - 1 ];
}
/* Processing of gains */
void SKP_Silk_process_gains_FIX(
    SKP_Silk_encoder_state_FIX      *psEnc,         /* I/O  Encoder state_FIX                           */
    SKP_Silk_encoder_control_FIX    *psEncCtrl      /* I/O  Encoder control_FIX                         */
)
{
    SKP_Silk_shape_state_FIX    *psShapeSt = &psEnc->sShape;
    SKP_int     k;
    SKP_int32   s_Q16, InvMaxSqrVal_Q16, gain, gain_squared, ResNrg, ResNrgPart;

    /* Gain reduction when LTP coding gain is high */
    if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
        /*s = -0.5f * SKP_sigmoid( 0.25f * ( psEncCtrl->LTPredCodGain - 12.0f ) ); */
        s_Q16 = -SKP_Silk_sigm_Q15( SKP_RSHIFT_ROUND( psEncCtrl->LTPredCodGain_Q7 - (12 << 7), 4 ) );
        for( k = 0; k < NB_SUBFR; k++ ) {
            psEncCtrl->Gains_Q16[ k ] = SKP_SMLAWB( psEncCtrl->Gains_Q16[ k ], psEncCtrl->Gains_Q16[ k ], s_Q16 );
        }
    }

    /* Limit the quantized signal */
    /*  69 = 21.0f + 16/0.33    */
    InvMaxSqrVal_Q16 = SKP_DIV32_16( SKP_Silk_log2lin( 
        SKP_SMULWB( (69 << 7) - psEncCtrl->current_SNR_dB_Q7, SKP_FIX_CONST( 0.33, 16 )) ), psEnc->sCmn.subfr_length );

    for( k = 0; k < NB_SUBFR; k++ ) {
        /* Soft limit on ratio residual energy and squared gains */
        ResNrg     = psEncCtrl->ResNrg[ k ];
        ResNrgPart = SKP_SMULWW( ResNrg, InvMaxSqrVal_Q16 );
        if( psEncCtrl->ResNrgQ[ k ] > 0 ) {
            if( psEncCtrl->ResNrgQ[ k ] < 32 ) {
                ResNrgPart = SKP_RSHIFT_ROUND( ResNrgPart, psEncCtrl->ResNrgQ[ k ] );
            } else {
                ResNrgPart = 0;
            }
        } else if( psEncCtrl->ResNrgQ[k] != 0 ) {
            if( ResNrgPart > SKP_RSHIFT( SKP_int32_MAX, -psEncCtrl->ResNrgQ[ k ] ) ) {
                ResNrgPart = SKP_int32_MAX;
            } else {
                ResNrgPart = SKP_LSHIFT( ResNrgPart, -psEncCtrl->ResNrgQ[ k ] );
            }
        }
        gain = psEncCtrl->Gains_Q16[ k ];
        gain_squared = SKP_ADD_SAT32( ResNrgPart, SKP_SMMUL( gain, gain ) );
        if( gain_squared < SKP_int16_MAX ) {
            /* recalculate with higher precision */
            gain_squared = SKP_SMLAWW( SKP_LSHIFT( ResNrgPart, 16 ), gain, gain );
            SKP_assert( gain_squared > 0 );
            gain = SKP_Silk_SQRT_APPROX( gain_squared );                  /* Q8   */
            psEncCtrl->Gains_Q16[ k ] = SKP_LSHIFT_SAT32( gain, 8 );        /* Q16  */
        } else {
            gain = SKP_Silk_SQRT_APPROX( gain_squared );                  /* Q0   */
            psEncCtrl->Gains_Q16[ k ] = SKP_LSHIFT_SAT32( gain, 16 );       /* Q16  */
        }
    }

    /* Noise shaping quantization */
    SKP_Silk_gains_quant( psEncCtrl->sCmn.GainsIndices, psEncCtrl->Gains_Q16, 
        &psShapeSt->LastGainIndex, psEnc->sCmn.nFramesInPayloadBuf );
    /* Set quantizer offset for voiced signals. Larger offset when LTP coding gain is low or tilt is high (ie low-pass) */
    if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
        if( psEncCtrl->LTPredCodGain_Q7 + SKP_RSHIFT( psEncCtrl->input_tilt_Q15, 8 ) > ( 1 << 7 ) ) {
            psEncCtrl->sCmn.QuantOffsetType = 0;
        } else {
            psEncCtrl->sCmn.QuantOffsetType = 1;
        }
    }

    /* Quantizer boundary adjustment */
    if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
        psEncCtrl->Lambda_Q10 = SKP_FIX_CONST( 1.3, 10 )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.5, 18 ), psEnc->speech_activity_Q8       )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.3, 12 ), psEncCtrl->input_quality_Q14    )
                  + SKP_SMULBB( SKP_FIX_CONST( 0.2, 10 ), psEncCtrl->sCmn.QuantOffsetType )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.1, 12 ), psEncCtrl->coding_quality_Q14   );
    } else {
        psEncCtrl->Lambda_Q10 = SKP_FIX_CONST( 1.3, 10 )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.5, 18 ), psEnc->speech_activity_Q8       )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.4, 12 ), psEncCtrl->input_quality_Q14    )
                  + SKP_SMULBB( SKP_FIX_CONST( 0.4, 10 ), psEncCtrl->sCmn.QuantOffsetType )
                  - SKP_SMULWB( SKP_FIX_CONST( 0.1, 12 ), psEncCtrl->coding_quality_Q14   );
    }
    SKP_assert( psEncCtrl->Lambda_Q10 >= 0 );
    SKP_assert( psEncCtrl->Lambda_Q10 < SKP_FIX_CONST( 2, 10 ) );
    
}
Ejemplo n.º 11
0
/* Finds LPC vector from correlations, and converts to NLSF */
void SKP_Silk_find_LPC_FIX(
    SKP_int             NLSF_Q15[],             /* O    NLSFs                                                                       */
    SKP_int             *interpIndex,           /* O    NLSF interpolation index, only used for NLSF interpolation                  */
    const SKP_int       prev_NLSFq_Q15[],       /* I    previous NLSFs, only used for NLSF interpolation                            */
    const SKP_int       useInterpolatedNLSFs,   /* I    Flag                                                                        */
    const SKP_int       LPC_order,              /* I    LPC order                                                                   */
    const SKP_int16     x[],                    /* I    Input signal                                                                */
    const SKP_int       subfr_length            /* I    Input signal subframe length including preceeding samples                   */
)
{
    SKP_int     k;
    SKP_int32   a_Q16[ MAX_LPC_ORDER ];
    SKP_int     isInterpLower, shift;
    SKP_int16   S[ MAX_LPC_ORDER ];
    SKP_int32   res_nrg0, res_nrg1;
    SKP_int     rshift0, rshift1; 

    /* Used only for LSF interpolation */
    SKP_int32   a_tmp_Q16[ MAX_LPC_ORDER ], res_nrg_interp, res_nrg, res_tmp_nrg;
    SKP_int     res_nrg_interp_Q, res_nrg_Q, res_tmp_nrg_Q;
    SKP_int16   a_tmp_Q12[ MAX_LPC_ORDER ];
    SKP_int     NLSF0_Q15[ MAX_LPC_ORDER ];
    SKP_int16   LPC_res[ ( MAX_FRAME_LENGTH + NB_SUBFR * MAX_LPC_ORDER ) / 2 ];

    /* Default: no interpolation */
    *interpIndex = 4;

    /* Burg AR analysis for the full frame */
    SKP_Silk_burg_modified( &res_nrg, &res_nrg_Q, a_Q16, x, subfr_length, NB_SUBFR, SKP_FIX_CONST( FIND_LPC_COND_FAC, 32 ), LPC_order );

    SKP_Silk_bwexpander_32( a_Q16, LPC_order, SKP_FIX_CONST( FIND_LPC_CHIRP, 16 ) );

    if( useInterpolatedNLSFs == 1 ) {

        /* Optimal solution for last 10 ms */
        SKP_Silk_burg_modified( &res_tmp_nrg, &res_tmp_nrg_Q, a_tmp_Q16, x + ( NB_SUBFR >> 1 ) * subfr_length, 
            subfr_length, ( NB_SUBFR >> 1 ), SKP_FIX_CONST( FIND_LPC_COND_FAC, 32 ), LPC_order );

        SKP_Silk_bwexpander_32( a_tmp_Q16, LPC_order, SKP_FIX_CONST( FIND_LPC_CHIRP, 16 ) );

        /* subtract residual energy here, as that's easier than adding it to the    */
        /* residual energy of the first 10 ms in each iteration of the search below */
        shift = res_tmp_nrg_Q - res_nrg_Q;
        if( shift >= 0 ) {
            if( shift < 32 ) { 
                res_nrg = res_nrg - SKP_RSHIFT( res_tmp_nrg, shift );
            }
        } else {
            SKP_assert( shift > -32 ); 
            res_nrg   = SKP_RSHIFT( res_nrg, -shift ) - res_tmp_nrg;
            res_nrg_Q = res_tmp_nrg_Q; 
        }
        
        /* Convert to NLSFs */
        SKP_Silk_A2NLSF( NLSF_Q15, a_tmp_Q16, LPC_order );

        /* Search over interpolation indices to find the one with lowest residual energy */
        for( k = 3; k >= 0; k-- ) {
            /* Interpolate NLSFs for first half */
            SKP_Silk_interpolate( NLSF0_Q15, prev_NLSFq_Q15, NLSF_Q15, k, LPC_order );

            /* Convert to LPC for residual energy evaluation */
            SKP_Silk_NLSF2A_stable( a_tmp_Q12, NLSF0_Q15, LPC_order );

            /* Calculate residual energy with NLSF interpolation */
            SKP_memset( S, 0, LPC_order * sizeof( SKP_int16 ) );
            SKP_Silk_LPC_analysis_filter( x, a_tmp_Q12, S, LPC_res, 2 * subfr_length, LPC_order );

            SKP_Silk_sum_sqr_shift( &res_nrg0, &rshift0, LPC_res + LPC_order,                subfr_length - LPC_order );
            SKP_Silk_sum_sqr_shift( &res_nrg1, &rshift1, LPC_res + LPC_order + subfr_length, subfr_length - LPC_order );

            /* Add subframe energies from first half frame */
            shift = rshift0 - rshift1;
            if( shift >= 0 ) {
                res_nrg1         = SKP_RSHIFT( res_nrg1, shift );
                res_nrg_interp_Q = -rshift0;
            } else {
                res_nrg0         = SKP_RSHIFT( res_nrg0, -shift );
                res_nrg_interp_Q = -rshift1;
            }
            res_nrg_interp = SKP_ADD32( res_nrg0, res_nrg1 );

            /* Compare with first half energy without NLSF interpolation, or best interpolated value so far */
            shift = res_nrg_interp_Q - res_nrg_Q;
            if( shift >= 0 ) {
                if( SKP_RSHIFT( res_nrg_interp, shift ) < res_nrg ) {
                    isInterpLower = SKP_TRUE;
                } else {
                    isInterpLower = SKP_FALSE;
                }
            } else {
                if( -shift < 32 ) { 
                    if( res_nrg_interp < SKP_RSHIFT( res_nrg, -shift ) ) {
                        isInterpLower = SKP_TRUE;
                    } else {
                        isInterpLower = SKP_FALSE;
                    }
                } else {
                    isInterpLower = SKP_FALSE;
                }
            }

            /* Determine whether current interpolated NLSFs are best so far */
            if( isInterpLower == SKP_TRUE ) {
                /* Interpolation has lower residual energy */
                res_nrg   = res_nrg_interp;
                res_nrg_Q = res_nrg_interp_Q;
                *interpIndex = k;
            }
        }
    }
void SKP_Silk_prefilter_FIX(
    SKP_Silk_encoder_state_FIX          *psEnc,         /* I/O  Encoder state FIX                           */
    const SKP_Silk_encoder_control_FIX  *psEncCtrl,     /* I    Encoder control FIX                         */
    SKP_int16                           xw[],           /* O    Weighted signal                             */
    const SKP_int16                     x[]             /* I    Speech signal                               */
)
{
    SKP_Silk_prefilter_state_FIX *P = &psEnc->sPrefilt;
    SKP_int   j, k, lag;
    SKP_int32 tmp_32;
    const SKP_int16 *AR1_shp_Q13;
    const SKP_int16 *px;
    SKP_int16 *pxw;
    SKP_int   HarmShapeGain_Q12, Tilt_Q14;
    SKP_int32 HarmShapeFIRPacked_Q12, LF_shp_Q14;
    SKP_int32 x_filt_Q12[ MAX_FRAME_LENGTH / NB_SUBFR ];
    SKP_int16 st_res[ ( MAX_FRAME_LENGTH / NB_SUBFR ) + MAX_SHAPE_LPC_ORDER ];
#if !defined(_SYSTEM_IS_BIG_ENDIAN)
    SKP_int32 B_Q12;
#else
    SKP_int16 B_Q12[ 2 ];
#endif

    /* Setup pointers */
    px  = x;
    pxw = xw;
    lag = P->lagPrev;
    for( k = 0; k < NB_SUBFR; k++ ) {
        /* Update Variables that change per sub frame */
        if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {
            lag = psEncCtrl->sCmn.pitchL[ k ];
        }

        /* Noise shape parameters */
        HarmShapeGain_Q12 = SKP_SMULWB( psEncCtrl->HarmShapeGain_Q14[ k ], 16384 - psEncCtrl->HarmBoost_Q14[ k ] );
        SKP_assert( HarmShapeGain_Q12 >= 0 );
        HarmShapeFIRPacked_Q12  =                          SKP_RSHIFT( HarmShapeGain_Q12, 2 );
        HarmShapeFIRPacked_Q12 |= SKP_LSHIFT( ( SKP_int32 )SKP_RSHIFT( HarmShapeGain_Q12, 1 ), 16 );
        Tilt_Q14    = psEncCtrl->Tilt_Q14[   k ];
        LF_shp_Q14  = psEncCtrl->LF_shp_Q14[ k ];
        AR1_shp_Q13 = &psEncCtrl->AR1_Q13[   k * MAX_SHAPE_LPC_ORDER ];

        /* Short term FIR filtering*/
        SKP_Silk_warped_LPC_analysis_filter_FIX( P->sAR_shp, st_res, AR1_shp_Q13, px, 
            psEnc->sCmn.warping_Q16, psEnc->sCmn.subfr_length, psEnc->sCmn.shapingLPCOrder );

        /* reduce (mainly) low frequencies during harmonic emphasis */
#if !defined(_SYSTEM_IS_BIG_ENDIAN)
        /* NOTE: the code below loads two int16 values in an int32, and multiplies each using the   */
        /* SMLABB and SMLABT instructions. On a big-endian CPU the two int16 variables would be     */
        /* loaded in reverse order and the code will give the wrong result. In that case swapping   */
        /* the SMLABB and SMLABT instructions should solve the problem.                             */
        B_Q12 = SKP_RSHIFT_ROUND( psEncCtrl->GainsPre_Q14[ k ], 2 );
        tmp_32 = SKP_SMLABB( SKP_FIX_CONST( INPUT_TILT, 26 ), psEncCtrl->HarmBoost_Q14[ k ], HarmShapeGain_Q12 );   /* Q26 */
        tmp_32 = SKP_SMLABB( tmp_32, psEncCtrl->coding_quality_Q14, SKP_FIX_CONST( HIGH_RATE_INPUT_TILT, 12 ) );    /* Q26 */
        tmp_32 = SKP_SMULWB( tmp_32, -psEncCtrl->GainsPre_Q14[ k ] );                                               /* Q24 */
        tmp_32 = SKP_RSHIFT_ROUND( tmp_32, 12 );                                                                    /* Q12 */
        B_Q12 |= SKP_LSHIFT( SKP_SAT16( tmp_32 ), 16 );

        x_filt_Q12[ 0 ] = SKP_SMLABT( SKP_SMULBB( st_res[ 0 ], B_Q12 ), P->sHarmHP, B_Q12 );
        for( j = 1; j < psEnc->sCmn.subfr_length; j++ ) {
            x_filt_Q12[ j ] = SKP_SMLABT( SKP_SMULBB( st_res[ j ], B_Q12 ), st_res[ j - 1 ], B_Q12 );
        }
#else
        B_Q12[ 0 ] = SKP_RSHIFT_ROUND( psEncCtrl->GainsPre_Q14[ k ], 2 );
        tmp_32 = SKP_SMLABB( SKP_FIX_CONST( INPUT_TILT, 26 ), psEncCtrl->HarmBoost_Q14[ k ], HarmShapeGain_Q12 );   /* Q26 */
        tmp_32 = SKP_SMLABB( tmp_32, psEncCtrl->coding_quality_Q14, SKP_FIX_CONST( HIGH_RATE_INPUT_TILT, 12 ) );    /* Q26 */
        tmp_32 = SKP_SMULWB( tmp_32, -psEncCtrl->GainsPre_Q14[ k ] );                                               /* Q24 */
        tmp_32 = SKP_RSHIFT_ROUND( tmp_32, 12 );                                                                    /* Q12 */
        B_Q12[ 1 ]= SKP_SAT16( tmp_32 );

        x_filt_Q12[ 0 ] = SKP_SMLABB( SKP_SMULBB( st_res[ 0 ], B_Q12[ 0 ] ), P->sHarmHP, B_Q12[ 1 ] );
        for( j = 1; j < psEnc->sCmn.subfr_length; j++ ) {
            x_filt_Q12[ j ] = SKP_SMLABB( SKP_SMULBB( st_res[ j ], B_Q12[ 0 ] ), st_res[ j - 1 ], B_Q12[ 1 ] );
        }
#endif
        P->sHarmHP = st_res[ psEnc->sCmn.subfr_length - 1 ];

        SKP_Silk_prefilt_FIX( P, x_filt_Q12, pxw, HarmShapeFIRPacked_Q12, Tilt_Q14, 
            LF_shp_Q14, lag, psEnc->sCmn.subfr_length );

        px  += psEnc->sCmn.subfr_length;
        pxw += psEnc->sCmn.subfr_length;
    }

    P->lagPrev = psEncCtrl->sCmn.pitchL[ NB_SUBFR - 1 ];
}
void SKP_Silk_NLSF_MSVQ_encode_FIX(
          SKP_int                   *NLSFIndices,           /* O    Codebook path vector [ CB_STAGES ]      */
          SKP_int                   *pNLSF_Q15,             /* I/O  Quantized NLSF vector [ LPC_ORDER ]     */
    const SKP_Silk_NLSF_CB_struct   *psNLSF_CB,             /* I    Codebook object                         */
    const SKP_int                   *pNLSF_q_Q15_prev,      /* I    Prev. quantized NLSF vector [LPC_ORDER] */
    const SKP_int                   *pW_Q6,                 /* I    NLSF weight vector [ LPC_ORDER ]        */
    const SKP_int                   NLSF_mu_Q15,            /* I    Rate weight for the RD optimization     */
    const SKP_int                   NLSF_mu_fluc_red_Q16,   /* I    Fluctuation reduction error weight      */
    const SKP_int                   NLSF_MSVQ_Survivors,    /* I    Max survivors from each stage           */
    const SKP_int                   LPC_order,              /* I    LPC order                               */
    const SKP_int                   deactivate_fluc_red     /* I    Deactivate fluctuation reduction        */
)
{
    SKP_int     i, s, k, cur_survivors = 0, prev_survivors, min_survivors, input_index, cb_index, bestIndex;
    SKP_int32   rateDistThreshold_Q18;
#if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 1 )
    SKP_int32   se_Q15, wsse_Q20, bestRateDist_Q20;
#endif

#if( LOW_COMPLEXITY_ONLY == 1 )
    SKP_int32   pRateDist_Q18[  NLSF_MSVQ_TREE_SEARCH_MAX_VECTORS_EVALUATED_LC_MODE ];
    SKP_int32   pRate_Q5[       MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];
    SKP_int32   pRate_new_Q5[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];
    SKP_int     pTempIndices[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ];
    SKP_int     pPath[          MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * NLSF_MSVQ_MAX_CB_STAGES ];
    SKP_int     pPath_new[      MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * NLSF_MSVQ_MAX_CB_STAGES ];
    SKP_int     pRes_Q15[       MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * MAX_LPC_ORDER ];
    SKP_int     pRes_new_Q15[   MAX_NLSF_MSVQ_SURVIVORS_LC_MODE * MAX_LPC_ORDER ];
#else
    SKP_int32   pRateDist_Q18[  NLSF_MSVQ_TREE_SEARCH_MAX_VECTORS_EVALUATED ];
    SKP_int32   pRate_Q5[       MAX_NLSF_MSVQ_SURVIVORS ];
    SKP_int32   pRate_new_Q5[   MAX_NLSF_MSVQ_SURVIVORS ];
    SKP_int     pTempIndices[   MAX_NLSF_MSVQ_SURVIVORS ];
    SKP_int     pPath[          MAX_NLSF_MSVQ_SURVIVORS * NLSF_MSVQ_MAX_CB_STAGES ];
    SKP_int     pPath_new[      MAX_NLSF_MSVQ_SURVIVORS * NLSF_MSVQ_MAX_CB_STAGES ];
    SKP_int     pRes_Q15[       MAX_NLSF_MSVQ_SURVIVORS * MAX_LPC_ORDER ];
    SKP_int     pRes_new_Q15[   MAX_NLSF_MSVQ_SURVIVORS * MAX_LPC_ORDER ];
#endif

    const SKP_int   *pConstInt;
          SKP_int   *pInt;
    const SKP_int16 *pCB_element;
    const SKP_Silk_NLSF_CBS *pCurrentCBStage;

#ifdef USE_UNQUANTIZED_LSFS
    SKP_int NLSF_orig[ MAX_LPC_ORDER ];
    SKP_memcpy( NLSF_orig, pNLSF_Q15, LPC_order * sizeof( SKP_int ) );
#endif

    SKP_assert( NLSF_MSVQ_Survivors <= MAX_NLSF_MSVQ_SURVIVORS );
    SKP_assert( ( LOW_COMPLEXITY_ONLY == 0 ) || ( NLSF_MSVQ_Survivors <= MAX_NLSF_MSVQ_SURVIVORS_LC_MODE ) );


    /****************************************************/
    /* Tree search for the multi-stage vector quantizer */
    /****************************************************/

    /* Clear accumulated rates */
    SKP_memset( pRate_Q5, 0, NLSF_MSVQ_Survivors * sizeof( SKP_int32 ) );
    
    /* Copy NLSFs into residual signal vector */
    for( i = 0; i < LPC_order; i++ ) {
        pRes_Q15[ i ] = pNLSF_Q15[ i ];
    }

    /* Set first stage values */
    prev_survivors = 1;

    /* Minimum number of survivors */
    min_survivors = NLSF_MSVQ_Survivors / 2;

    /* Loop over all stages */
    for( s = 0; s < psNLSF_CB->nStages; s++ ) {

        /* Set a pointer to the current stage codebook */
        pCurrentCBStage = &psNLSF_CB->CBStages[ s ];

        /* Calculate the number of survivors in the current stage */
        cur_survivors = SKP_min_32( NLSF_MSVQ_Survivors, SKP_SMULBB( prev_survivors, pCurrentCBStage->nVectors ) );

#if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 0 )
        /* Find a single best survivor in the last stage, if we */
        /* do not need candidates for fluctuation reduction     */
        if( s == psNLSF_CB->nStages - 1 ) {
            cur_survivors = 1;
        }
#endif

        /* Nearest neighbor clustering for multiple input data vectors */
        SKP_Silk_NLSF_VQ_rate_distortion_FIX( pRateDist_Q18, pCurrentCBStage, pRes_Q15, pW_Q6, 
            pRate_Q5, NLSF_mu_Q15, prev_survivors, LPC_order );

        /* Sort the rate-distortion errors */
        SKP_Silk_insertion_sort_increasing( pRateDist_Q18, pTempIndices, 
            prev_survivors * pCurrentCBStage->nVectors, cur_survivors );

        /* Discard survivors with rate-distortion values too far above the best one */
        if( pRateDist_Q18[ 0 ] < SKP_int32_MAX / MAX_NLSF_MSVQ_SURVIVORS ) {
            rateDistThreshold_Q18 = SKP_SMLAWB( pRateDist_Q18[ 0 ], 
                SKP_MUL( NLSF_MSVQ_Survivors, pRateDist_Q18[ 0 ] ), SKP_FIX_CONST( NLSF_MSVQ_SURV_MAX_REL_RD, 16 ) );
            while( pRateDist_Q18[ cur_survivors - 1 ] > rateDistThreshold_Q18 && cur_survivors > min_survivors ) {
                cur_survivors--;
            }
        }
        /* Update accumulated codebook contributions for the 'cur_survivors' best codebook indices */
        for( k = 0; k < cur_survivors; k++ ) { 
            if( s > 0 ) {
                /* Find the indices of the input and the codebook vector */
                if( pCurrentCBStage->nVectors == 8 ) {
                    input_index = SKP_RSHIFT( pTempIndices[ k ], 3 );
                    cb_index    = pTempIndices[ k ] & 7;
                } else {
                    input_index = SKP_DIV32_16( pTempIndices[ k ], pCurrentCBStage->nVectors );  
                    cb_index    = pTempIndices[ k ] - SKP_SMULBB( input_index, pCurrentCBStage->nVectors );
                }
            } else {
                /* Find the indices of the input and the codebook vector */
                input_index = 0;
                cb_index    = pTempIndices[ k ];
            }

            /* Subtract new contribution from the previous residual vector for each of 'cur_survivors' */
            pConstInt   = &pRes_Q15[ SKP_SMULBB( input_index, LPC_order ) ];
            pCB_element = &pCurrentCBStage->CB_NLSF_Q15[ SKP_SMULBB( cb_index, LPC_order ) ];
            pInt        = &pRes_new_Q15[ SKP_SMULBB( k, LPC_order ) ];
            for( i = 0; i < LPC_order; i++ ) {
                pInt[ i ] = pConstInt[ i ] - ( SKP_int )pCB_element[ i ];
            }

            /* Update accumulated rate for stage 1 to the current */
            pRate_new_Q5[ k ] = pRate_Q5[ input_index ] + pCurrentCBStage->Rates_Q5[ cb_index ];

            /* Copy paths from previous matrix, starting with the best path */
            pConstInt = &pPath[ SKP_SMULBB( input_index, psNLSF_CB->nStages ) ];
            pInt      = &pPath_new[ SKP_SMULBB( k, psNLSF_CB->nStages ) ];
            for( i = 0; i < s; i++ ) {
                pInt[ i ] = pConstInt[ i ];
            }
            /* Write the current stage indices for the 'cur_survivors' to the best path matrix */
            pInt[ s ] = cb_index;
        }

        if( s < psNLSF_CB->nStages - 1 ) {
            /* Copy NLSF residual matrix for next stage */
            SKP_memcpy( pRes_Q15, pRes_new_Q15, SKP_SMULBB( cur_survivors, LPC_order ) * sizeof( SKP_int ) );

            /* Copy rate vector for next stage */
            SKP_memcpy( pRate_Q5, pRate_new_Q5, cur_survivors * sizeof( SKP_int32 ) );

            /* Copy best path matrix for next stage */
            SKP_memcpy( pPath, pPath_new, SKP_SMULBB( cur_survivors, psNLSF_CB->nStages ) * sizeof( SKP_int ) );
        }

        prev_survivors = cur_survivors;
    }

    /* (Preliminary) index of the best survivor, later to be decoded */
    bestIndex = 0;

#if( NLSF_MSVQ_FLUCTUATION_REDUCTION == 1 )
    /******************************/
    /* NLSF fluctuation reduction */
    /******************************/
    if( deactivate_fluc_red != 1 ) {
    
        /* Search among all survivors, now taking also weighted fluctuation errors into account */
        bestRateDist_Q20 = SKP_int32_MAX;
        for( s = 0; s < cur_survivors; s++ ) {
            /* Decode survivor to compare with previous quantized NLSF vector */
            SKP_Silk_NLSF_MSVQ_decode( pNLSF_Q15, psNLSF_CB, &pPath_new[ SKP_SMULBB( s, psNLSF_CB->nStages ) ], LPC_order );

            /* Compare decoded NLSF vector with the previously quantized vector */ 
            wsse_Q20 = 0;
            for( i = 0; i < LPC_order; i += 2 ) {
                /* Compute weighted squared quantization error for index i */
                se_Q15 = pNLSF_Q15[ i ] - pNLSF_q_Q15_prev[ i ]; // range: [ -32767 : 32767 ]
                wsse_Q20 = SKP_SMLAWB( wsse_Q20, SKP_SMULBB( se_Q15, se_Q15 ), pW_Q6[ i ] );

                /* Compute weighted squared quantization error for index i + 1 */
                se_Q15 = pNLSF_Q15[ i + 1 ] - pNLSF_q_Q15_prev[ i + 1 ]; // range: [ -32767 : 32767 ]
                wsse_Q20 = SKP_SMLAWB( wsse_Q20, SKP_SMULBB( se_Q15, se_Q15 ), pW_Q6[ i + 1 ] );
            }
            SKP_assert( wsse_Q20 >= 0 );

            /* Add the fluctuation reduction penalty to the rate distortion error */
            wsse_Q20 = SKP_ADD_POS_SAT32( pRateDist_Q18[ s ], SKP_SMULWB( wsse_Q20, NLSF_mu_fluc_red_Q16 ) );

            /* Keep index of best survivor */
            if( wsse_Q20 < bestRateDist_Q20 ) {
                bestRateDist_Q20 = wsse_Q20;
                bestIndex = s;
            }
        }
    }
#endif

    /* Copy best path to output argument */
    SKP_memcpy( NLSFIndices, &pPath_new[ SKP_SMULBB( bestIndex, psNLSF_CB->nStages ) ], psNLSF_CB->nStages * sizeof( SKP_int ) );

    /* Decode and stabilize the best survivor */
    SKP_Silk_NLSF_MSVQ_decode( pNLSF_Q15, psNLSF_CB, NLSFIndices, LPC_order );

#ifdef USE_UNQUANTIZED_LSFS
    SKP_memcpy( pNLSF_Q15, NLSF_orig, LPC_order * sizeof( SKP_int ) );
#endif

}