/*
 * Copyright @ 2015 Atlassian Pty Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.jitsi.impl.neomedia.codec.audio.silk;

import static org.jitsi.impl.neomedia.codec.audio.silk.CommonPitchEstDefines.*;
import static org.jitsi.impl.neomedia.codec.audio.silk.Macros.*;
import static org.jitsi.impl.neomedia.codec.audio.silk.PitchEstDefinesFLP.*;

/**
 * Pitch analysis.
 *
 * @author Jing Dai
 * @author Dingxin Xu
 */
//TODO: float or dobule ???
public class PitchAnalysisCoreFLP
{
    static final int SCRATCH_SIZE =   22;

    static final float eps =  1.192092896e-07f;

    /* using log2() helps the fixed-point conversion */
    static float SKP_P_log2(double x)
    {
        return (float)(3.32192809488736 * Math.log10(x));
    }

    /**
     * CORE PITCH ANALYSIS FUNCTION.
     * @param signal signal of length PITCH_EST_FRAME_LENGTH_MS*Fs_kHz
     * @param pitch_out 4 pitch lag values
     * @param lagIndex lag Index
     * @param contourIndex pitch contour Index
     * @param LTPCorr normalized correlation; input: value from previous frame
     * @param prevLag last lag of previous frame; set to zero is unvoiced
     * @param search_thres1 first stage threshold for lag candidates 0 - 1
     * @param search_thres2 final threshold for lag candidates 0 - 1
     * @param Fs_kHz sample frequency (kHz)
     * @param complexity Complexity setting, 0-2, where 2 is highest
     * @return voicing estimate: 0 voiced, 1 unvoiced
     */
    static int SKP_Silk_pitch_analysis_core_FLP( /* O voicing estimate: 0 voiced, 1 unvoiced                 */
        float[] signal,            /* I signal of length PITCH_EST_FRAME_LENGTH_MS*Fs_kHz              */
        int[]         pitch_out,         /* O 4 pitch lag values                                             */
        int[]         lagIndex,          /* O lag Index                                                      */
        int[]         contourIndex,      /* O pitch contour Index                                            */
        float[]       LTPCorr,           /* I/O normalized correlation; input: value from previous frame     */
        int         prevLag,            /* I last lag of previous frame; set to zero is unvoiced            */
        final float search_thres1,      /* I first stage threshold for lag candidates 0 - 1                 */
        final float search_thres2,      /* I final threshold for lag candidates 0 - 1                       */
        final int   Fs_kHz,             /* I sample frequency (kHz)                                         */
        final int   complexity          /* I Complexity setting, 0-2, where 2 is highest                    */
    )
    {
        float[] signal_8kHz = new float[ PITCH_EST_FRAME_LENGTH_MS * 8 ];
        float[] signal_4kHz = new float[ PITCH_EST_FRAME_LENGTH_MS * 4 ];
        float[] scratch_mem = new float[ PITCH_EST_MAX_FRAME_LENGTH * 3 ];
        float[] filt_state = new float[ PITCH_EST_MAX_DECIMATE_STATE_LENGTH ];
        int   i, k, d, j;
        float threshold, contour_bias;
        float[][] C = new float[PITCH_EST_NB_SUBFR][(PITCH_EST_MAX_LAG >> 1) + 5]; /* use to be +2 but then valgrind reported errors for SWB */
        float[] CC = new float[PITCH_EST_NB_CBKS_STAGE2_EXT];
        float[] target_ptr, basis_ptr;
        int target_ptr_offset, basis_ptr_offset;
        double    cross_corr, normalizer, energy, energy_tmp;
        int[]   d_srch = new int[PITCH_EST_D_SRCH_LENGTH];
        short[] d_comp = new short[(PITCH_EST_MAX_LAG >> 1) + 5];
        int   length_d_srch, length_d_comp;
        float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;
        int   CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;
        int   cbk_offset, cbk_size;
        float lag_log2, prevLag_log2, delta_lag_log2_sqr;
        float[][][] energies_st3 = new float[ PITCH_EST_NB_SUBFR ][ PITCH_EST_NB_CBKS_STAGE3_MAX ][ PITCH_EST_NB_STAGE3_LAGS ];
        float[][][] cross_corr_st3 = new float[ PITCH_EST_NB_SUBFR ][ PITCH_EST_NB_CBKS_STAGE3_MAX ][ PITCH_EST_NB_STAGE3_LAGS ];

        int diff, lag_counter;
        int frame_length, frame_length_8kHz, frame_length_4kHz;
        int sf_length, sf_length_8kHz, sf_length_4kHz;
        int min_lag, min_lag_8kHz, min_lag_4kHz;
        int max_lag, max_lag_8kHz, max_lag_4kHz;

        int nb_cbks_stage2;

        /* Check for valid sampling frequency */
        assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 || Fs_kHz == 24 );

        /* Check for valid complexity setting */
        assert( complexity >= SigProcFIX.SKP_Silk_PITCH_EST_MIN_COMPLEX );
        assert( complexity <= SigProcFIX.SKP_Silk_PITCH_EST_MAX_COMPLEX );

        assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );
        assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );

        /* Setup frame lengths max / min lag for the sampling frequency */
        frame_length      = PITCH_EST_FRAME_LENGTH_MS * Fs_kHz;
        frame_length_4kHz = PITCH_EST_FRAME_LENGTH_MS * 4;
        frame_length_8kHz = PITCH_EST_FRAME_LENGTH_MS * 8;
        sf_length         = frame_length >>      3;
        sf_length_4kHz    = frame_length_4kHz >> 3;
        sf_length_8kHz    = frame_length_8kHz >> 3;
        min_lag           = PITCH_EST_MIN_LAG_MS * Fs_kHz;
        min_lag_4kHz      = PITCH_EST_MIN_LAG_MS * 4;
        min_lag_8kHz      = PITCH_EST_MIN_LAG_MS * 8;
        max_lag           = PITCH_EST_MAX_LAG_MS * Fs_kHz;
        max_lag_4kHz      = PITCH_EST_MAX_LAG_MS * 4;
        max_lag_8kHz      = PITCH_EST_MAX_LAG_MS * 8;

        for(int i_djinn=0; i_djinn< PITCH_EST_NB_SUBFR; i_djinn++)
        for(int j_djinn=0; j_djinn< (PITCH_EST_MAX_LAG >> 1) + 5; j_djinn++)
            C[i_djinn][j_djinn] = 0;

        /* Resample from input sampled at Fs_kHz to 8 kHz */
        if( Fs_kHz == 12 )
        {
            short[] signal_12 = new short[ 12 * PITCH_EST_FRAME_LENGTH_MS ];
            short[] signal_8 = new short[   8 * PITCH_EST_FRAME_LENGTH_MS ];
            int[] R23 = new int[ 6 ];

            /* Resample to 12 -> 8 khz */
            for(int i_djinn=0; i_djinn<6; i_djinn++)
                R23[i_djinn] = 0;
            SigProcFLP.SKP_float2short_array( signal_12,0, signal,0, PITCH_EST_FRAME_LENGTH_MS * 12);
            ResamplerDown23.SKP_Silk_resampler_down2_3( R23,0, signal_8,0, signal_12,0, PITCH_EST_FRAME_LENGTH_MS * 12 );
            SigProcFLP.SKP_short2float_array( signal_8kHz,0, signal_8,0, frame_length_8kHz );
        }
        else if( Fs_kHz == 16 )
        {
            if( complexity == SigProcFIX.SKP_Silk_PITCH_EST_MAX_COMPLEX )
            {
                assert( 4 <= PITCH_EST_MAX_DECIMATE_STATE_LENGTH );
                for(int i_djinn=0; i_djinn<4; i_djinn++)
                    filt_state[i_djinn] = 0;

                Decimate2CoarseFLP.SKP_Silk_decimate2_coarse_FLP( signal,0, filt_state,0, signal_8kHz,0,
                    scratch_mem,0, frame_length_8kHz );
            }
            else
            {
                assert( 2 <= PITCH_EST_MAX_DECIMATE_STATE_LENGTH );
                for(int i_djinn=0; i_djinn<2; i_djinn++)
                    filt_state[i_djinn] = 0;

                Decimate2CoarsestFLP.SKP_Silk_decimate2_coarsest_FLP( signal,0, filt_state,0, signal_8kHz,0,
                    scratch_mem,0, frame_length_8kHz );
            }
        }
        else if( Fs_kHz == 24 )
        {
            short[] signal_24 = new short[ PITCH_EST_MAX_FRAME_LENGTH ];
            short[] signal_8 = new short[ 8 * PITCH_EST_FRAME_LENGTH_MS ];
            int[] filt_state_fix = new int[ 8 ];

            /* Resample to 24 -> 8 khz */
            SigProcFLP.SKP_float2short_array( signal_24,0, signal,0, 24 * PITCH_EST_FRAME_LENGTH_MS );
            for(int i_djinn=0; i_djinn<8; i_djinn++)
                filt_state_fix[i_djinn] = 0;
            ResamplerDown3.SKP_Silk_resampler_down3( filt_state_fix,0, signal_8,0, signal_24,0, 24 * PITCH_EST_FRAME_LENGTH_MS );
            SigProcFLP.SKP_short2float_array( signal_8kHz,0, signal_8,0, frame_length_8kHz );
        }
        else
        {
            assert( Fs_kHz == 8 );
            for(int i_djinn=0; i_djinn<frame_length_8kHz; i_djinn++)
                signal_8kHz[i_djinn] = signal[i_djinn];
        }

        /* Decimate again to 4 kHz. Set mem to zero */
        if( complexity == SigProcFIX.SKP_Silk_PITCH_EST_MAX_COMPLEX )
        {
            assert( 4 <= PITCH_EST_MAX_DECIMATE_STATE_LENGTH );
            for(int i_djinn=0; i_djinn<4; i_djinn++)
                filt_state[i_djinn] = 0;
            Decimate2CoarseFLP.SKP_Silk_decimate2_coarse_FLP( signal_8kHz,0, filt_state,0,
                signal_4kHz,0, scratch_mem,0, frame_length_4kHz );
        }
        else
        {
            assert( 2 <= PITCH_EST_MAX_DECIMATE_STATE_LENGTH );
            for(int i_djinn=0; i_djinn<4; i_djinn++)
                filt_state[i_djinn] = 0;
            Decimate2CoarsestFLP.SKP_Silk_decimate2_coarsest_FLP( signal_8kHz,0, filt_state,0,
                signal_4kHz,0, scratch_mem,0, frame_length_4kHz );
        }

        /* Low-pass filter */
        for( i = frame_length_4kHz - 1; i > 0; i-- ) {
            signal_4kHz[ i ] += signal_4kHz[ i - 1 ];
        }

        /******************************************************************************
        * FIRST STAGE, operating in 4 khz
        ******************************************************************************/
        target_ptr = signal_4kHz;
        target_ptr_offset = frame_length_4kHz >> 1;
        for( k = 0; k < 2; k++ )
        {
            /* Check that we are within range of the array */
            assert( target_ptr_offset >= 0 );
            assert( target_ptr_offset + sf_length_8kHz <= frame_length_4kHz );

            basis_ptr = target_ptr;
            basis_ptr_offset = target_ptr_offset - min_lag_4kHz;

            /* Check that we are within range of the array */
            assert( basis_ptr_offset >= 0 );
            assert( basis_ptr_offset + sf_length_8kHz <= frame_length_4kHz );

            /* Calculate first vector products before loop */
            cross_corr = InnerProductFLP.SKP_Silk_inner_product_FLP( target_ptr,target_ptr_offset, basis_ptr,basis_ptr_offset, sf_length_8kHz );
            normalizer = EnergyFLP.SKP_Silk_energy_FLP( basis_ptr,basis_ptr_offset, sf_length_8kHz ) + 1000.0f;

            C[ 0 ][ min_lag_4kHz ] += (float)(cross_corr / Math.sqrt(normalizer));

            /* From now on normalizer is computed recursively */
            for(d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++)
            {
                basis_ptr_offset--;

                /* Check that we are within range of the array */
                assert( basis_ptr_offset >= 0 );
                assert( basis_ptr_offset + sf_length_8kHz <= frame_length_4kHz );

                cross_corr = InnerProductFLP.SKP_Silk_inner_product_FLP(target_ptr,target_ptr_offset, basis_ptr,basis_ptr_offset, sf_length_8kHz);

                /* Add contribution of new sample and remove contribution from oldest sample */
//                   normalizer +=
//                        basis_ptr[ 0 ] * basis_ptr[ 0 ] -
//                        basis_ptr[ sf_length_8kHz ] * basis_ptr[ sf_length_8kHz ];
                normalizer +=
                    basis_ptr[ basis_ptr_offset + 0 ] * basis_ptr[ basis_ptr_offset + 0 ] -
                    basis_ptr[ basis_ptr_offset + sf_length_8kHz ] * basis_ptr[ basis_ptr_offset + sf_length_8kHz ];
                C[ 0 ][ d ] += (float)(cross_corr / Math.sqrt( normalizer ));
            }
            /* Update target pointer */
            target_ptr_offset += sf_length_8kHz;
        }

        /* Apply short-lag bias */
        for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- )
        {
            C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;
        }

        /* Sort */
        length_d_srch = 5 + complexity;
        assert( length_d_srch <= PITCH_EST_D_SRCH_LENGTH );
        SortFLP.SKP_Silk_insertion_sort_decreasing_FLP( C[ 0 ],min_lag_4kHz, d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );

        /* Escape if correlation is very low already here */
        Cmax = C[ 0 ][ min_lag_4kHz ];
        target_ptr = signal_4kHz;
        target_ptr_offset = frame_length_4kHz >> 1;
        energy = 1000.0f;
        for( i = 0; i < frame_length_4kHz >> 1; i++ )
        {
            energy += target_ptr[target_ptr_offset+i] * target_ptr[target_ptr_offset+i];
        }
        threshold = Cmax * Cmax;
        if( energy / 16.0f > threshold )
        {
            for(int i_djinn=0; i_djinn<PITCH_EST_NB_SUBFR; i_djinn++)
                pitch_out[i_djinn] = 0;
            LTPCorr[0]      = 0.0f;
            lagIndex[0]     = 0;
            contourIndex[0] = 0;
            return 1;
        }

        threshold = search_thres1 * Cmax;
        for( i = 0; i < length_d_srch; i++ ) {
            /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
            if( C[ 0 ][ min_lag_4kHz + i ] > threshold )
            {
                d_srch[ i ] = ( d_srch[ i ] + min_lag_4kHz ) << 1;
            }
            else
            {
                length_d_srch = i;
                break;
            }
        }
        assert( length_d_srch > 0 );

        for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
            d_comp[ i ] = 0;
        }
        for( i = 0; i < length_d_srch; i++ ) {
            d_comp[ d_srch[ i ] ] = 1;
        }

        /* Convolution */
        for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
            d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
        }

        length_d_srch = 0;
        for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
            if( d_comp[ i + 1 ] > 0 ) {
                d_srch[ length_d_srch ] = i;
                length_d_srch++;
            }
        }

        /* Convolution */
        for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
            d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
        }

        length_d_comp = 0;
        for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ )
        {
            if( d_comp[ i ] > 0 ) {
                d_comp[ length_d_comp ] = (short)(i - 2);
                length_d_comp++;
            }
        }

        /**********************************************************************************
        ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
        *************************************************************************************/
        /*********************************************************************************
        * Find energy of each subframe projected onto its history, for a range of delays
        *********************************************************************************/
        for(int i_djinn=0; i_djinn< PITCH_EST_NB_SUBFR; i_djinn++)
            for(int j_djinn=0; j_djinn< ((PITCH_EST_MAX_LAG >> 1) + 5); j_djinn++)
                C[i_djinn][j_djinn] = 0;

        target_ptr = signal_8kHz; /* point to middle of frame */
        target_ptr_offset = frame_length_4kHz;
        for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
        {
            /* Check that we are within range of the array */
            assert( target_ptr_offset >= 0 );
            assert( target_ptr_offset + sf_length_8kHz <= frame_length_8kHz );

            energy_tmp = EnergyFLP.SKP_Silk_energy_FLP( target_ptr,target_ptr_offset, sf_length_8kHz );
            for( j = 0; j < length_d_comp; j++ )
            {
                d = d_comp[ j ];
                basis_ptr = target_ptr;
                basis_ptr_offset = target_ptr_offset - d;

                /* Check that we are within range of the array */
                assert( basis_ptr_offset >= 0 );
                assert( basis_ptr_offset + sf_length_8kHz <= frame_length_8kHz );

                cross_corr = InnerProductFLP.SKP_Silk_inner_product_FLP( basis_ptr,basis_ptr_offset, target_ptr,target_ptr_offset, sf_length_8kHz );
                energy     = EnergyFLP.SKP_Silk_energy_FLP( basis_ptr,basis_ptr_offset, sf_length_8kHz );
                if (cross_corr > 0.0f)
                {
                    C[ k ][ d ] = (float)(cross_corr * cross_corr / (energy * energy_tmp + eps));
                }
                else
                {
                    C[ k ][ d ] = 0.0f;
                }
            }
            target_ptr_offset += sf_length_8kHz;
        }

        /* search over lag range and lags codebook */
        /* scale factor for lag codebook, as a function of center lag */

        CCmax   = 0.0f; /* This value doesn't matter */
        CCmax_b = -1000.0f;

        CBimax = 0; /* To avoid returning undefined lag values */
        lag = -1;   /* To check if lag with strong enough correlation has been found */

        if( prevLag > 0 ) {
            if( Fs_kHz == 12 )
            {
                prevLag = ( prevLag<<1 ) / 3;
            }
            else if( Fs_kHz == 16 )
            {
                prevLag = prevLag>>1;
            }
            else if( Fs_kHz == 24 )
            {
                prevLag = prevLag / 3;
            }
            prevLag_log2 = SKP_P_log2(prevLag);
        }
        else
        {
            prevLag_log2 = 0;
        }

        /* If input is 8 khz use a larger codebook here because it is last stage */
        if( Fs_kHz == 8 && complexity > SigProcFIX.SKP_Silk_PITCH_EST_MIN_COMPLEX )
        {
            nb_cbks_stage2 = PITCH_EST_NB_CBKS_STAGE2_EXT;
        }
        else
        {
            nb_cbks_stage2 = PITCH_EST_NB_CBKS_STAGE2;
        }

        for( k = 0; k < length_d_srch; k++ )
        {
            d = d_srch[ k ];
            for( j = 0; j < nb_cbks_stage2; j++ )
            {
                CC[j] = 0.0f;
                for( i = 0; i < PITCH_EST_NB_SUBFR; i++ ) {
                    /* Try all codebooks */
                    CC[ j ] += C[ i ][ d + PitchEstTables.SKP_Silk_CB_lags_stage2[ i ][ j ] ];
                }
            }
            /* Find best codebook */
            CCmax_new  = -1000.0f;
            CBimax_new = 0;
            for( i = 0; i < nb_cbks_stage2; i++ )
            {
                if( CC[ i ] > CCmax_new )
                {
                    CCmax_new = CC[ i ];
                    CBimax_new = i;
                }
            }
            CCmax_new = Math.max(CCmax_new, 0.0f); /* To avoid taking square root of negative number later */
            CCmax_new_b = CCmax_new;

            /* Bias towards shorter lags */
            lag_log2 = SKP_P_log2(d);
            CCmax_new_b -= PITCH_EST_FLP_SHORTLAG_BIAS * PITCH_EST_NB_SUBFR * lag_log2;

            /* Bias towards previous lag */
            if ( prevLag > 0 )
            {
                delta_lag_log2_sqr = lag_log2 - prevLag_log2;
                delta_lag_log2_sqr *= delta_lag_log2_sqr;
                CCmax_new_b -= PITCH_EST_FLP_PREVLAG_BIAS * PITCH_EST_NB_SUBFR * LTPCorr[0] * delta_lag_log2_sqr / (delta_lag_log2_sqr + 0.5f);
            }

            if ( CCmax_new_b > CCmax_b && CCmax_new > PITCH_EST_NB_SUBFR * search_thres2 * search_thres2 )
            {
                CCmax_b = CCmax_new_b;
                CCmax   = CCmax_new;
                lag     = d;
                CBimax  = CBimax_new;
            }
        }

        if( lag == -1 )
        {
            /* No suitable candidate found */
            for(int i_djinn=0; i_djinn<PITCH_EST_NB_SUBFR; i_djinn++)
                pitch_out[i_djinn] = 0;
            LTPCorr[0]      = 0.0f;
            lagIndex[0]     = 0;
            contourIndex[0] = 0;
            return 1;
        }

        if( Fs_kHz > 8 ) {
            /* Search in original signal */

            /* Compensate for decimation */
            assert( lag == SigProcFIX.SKP_SAT16( lag ) );
            if( Fs_kHz == 12 )
            {
                lag = SigProcFIX.SKP_RSHIFT_ROUND( SKP_SMULBB( lag, 3 ), 1 );
            }
            else if( Fs_kHz == 16 )
            {
                lag = lag<<1;
            }
            else
            {
                lag = SKP_SMULBB( lag, 3 );
            }

            lag = SigProcFIX.SKP_LIMIT_int( lag, min_lag, max_lag );
            start_lag = Math.max( lag - 2, min_lag );
            end_lag   = Math.min( lag + 2, max_lag );
            lag_new   = lag;                                    /* to avoid undefined lag */
            CBimax    = 0;                                      /* to avoid undefined lag */
            assert( CCmax >= 0.0f );
            LTPCorr[0] = (float)Math.sqrt( CCmax / PITCH_EST_NB_SUBFR );   // Output normalized correlation

            CCmax = -1000.0f;

            /* Calculate the correlations and energies needed in stage 3 */
            SKP_P_Ana_calc_corr_st3( cross_corr_st3, signal,0, start_lag, sf_length, complexity );
            SKP_P_Ana_calc_energy_st3( energies_st3, signal,0, start_lag, sf_length, complexity );

            lag_counter = 0;
            assert( lag == SigProcFIX.SKP_SAT16( lag ) );
            contour_bias = PITCH_EST_FLP_FLATCONTOUR_BIAS / lag;

            /* Setup cbk parameters according to complexity setting */
            cbk_size   = PitchEstTables.SKP_Silk_cbk_sizes_stage3[   complexity ];
            cbk_offset = PitchEstTables.SKP_Silk_cbk_offsets_stage3[ complexity ];

            for( d = start_lag; d <= end_lag; d++ )
            {
                for( j = cbk_offset; j < ( cbk_offset + cbk_size ); j++ )
                {
                    cross_corr = 0.0;
                    energy = eps;
                    for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
                    {
                        energy     +=   energies_st3[ k ][ j ][ lag_counter ];
                        cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
                    }
                    if( cross_corr > 0.0 )
                    {
                        CCmax_new = (float)(cross_corr * cross_corr / energy);
                        /* Reduce depending on flatness of contour */
                        diff = j - ( PITCH_EST_NB_CBKS_STAGE3_MAX >> 1 );
                        CCmax_new *= ( 1.0f - contour_bias * diff * diff );
                    }
                    else
                    {
                        CCmax_new = 0.0f;
                    }

                    if( CCmax_new > CCmax )
                    {
                        CCmax   = CCmax_new;
                        lag_new = d;
                        CBimax  = j;
                    }
                }
                lag_counter++;
            }

            for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
            {
                pitch_out[k] = lag_new + PitchEstTables.SKP_Silk_CB_lags_stage3[ k ][ CBimax ];
            }
            lagIndex[0] = lag_new - min_lag;
            contourIndex[0] = CBimax;
        }
        else
        {
            /* Save Lags and correlation */
            assert( CCmax >= 0.0f );
            LTPCorr[0] = (float)Math.sqrt(CCmax / PITCH_EST_NB_SUBFR); /* Output normalized correlation */
            for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
            {
                pitch_out[ k ] = lag + PitchEstTables.SKP_Silk_CB_lags_stage2[ k ][ CBimax ];
            }
            lagIndex[0] = lag - min_lag;
            contourIndex[0] = CBimax;
        }
        assert( lagIndex[0] >= 0 );
        /* return as voiced */
        return 0;
    }

    /**
     * Internally used functions.
     *
     * @param cross_corr_st3 3 DIM correlation array.
     * @param signal vector to correlate.
     * @param signal_offset offset of valid data.
     * @param start_lag start lag.
     * @param sf_length sub frame length.
     * @param complexity Complexity setting.
     */
    static void SKP_P_Ana_calc_corr_st3
    (
        float[][][] cross_corr_st3,
        float signal[],           /* I vector to correlate                                            */
        int signal_offset,
        int start_lag,                  /* I start lag                                                      */
        int sf_length,                  /* I sub frame length                                               */
        int complexity                  /* I Complexity setting                                             */
    )
        /***********************************************************************
         Calculates the correlations used in stage 3 search. In order to cover
         the whole lag codebook for all the searched offset lags (lag +- 2),
         the following correlations are needed in each sub frame:

         sf1: lag range [-8,...,7] total 16 correlations
         sf2: lag range [-4,...,4] total 9 correlations
         sf3: lag range [-3,....4] total 8 correltions
         sf4: lag range [-6,....8] total 15 correlations

         In total 48 correlations. The direct implementation computed in worst case
         4*12*5 = 240 correlations, but more likely around 120.
         **********************************************************************/
    {
        float[] target_ptr, basis_ptr;
        int target_ptr_offset, basis_ptr_offset;
        int     i, j, k, lag_counter;
        int     cbk_offset, cbk_size, delta, idx;
        float[]   scratch_mem = new float[ SCRATCH_SIZE ];

        assert( complexity >= SigProcFIX.SKP_Silk_PITCH_EST_MIN_COMPLEX );
        assert( complexity <= SigProcFIX.SKP_Silk_PITCH_EST_MAX_COMPLEX );

        cbk_offset = PitchEstTables.SKP_Silk_cbk_offsets_stage3[ complexity ];
        cbk_size   = PitchEstTables.SKP_Silk_cbk_sizes_stage3[   complexity ];

        target_ptr = signal;/* Pointer to middle of frame */
        target_ptr_offset = signal_offset+( sf_length << 2 );
        for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
        {
            lag_counter = 0;

            /* Calculate the correlations for each subframe */
            for( j = PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 0 ]; j <= PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 1 ]; j++ )
            {
                basis_ptr = target_ptr;
                basis_ptr_offset = target_ptr_offset - ( start_lag + j );
                assert( lag_counter < SCRATCH_SIZE );
                scratch_mem[ lag_counter ] = (float)InnerProductFLP.SKP_Silk_inner_product_FLP( target_ptr,target_ptr_offset, basis_ptr,basis_ptr_offset, sf_length );
                lag_counter++;
            }

            delta = PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 0 ];
            for( i = cbk_offset; i < ( cbk_offset + cbk_size ); i++ )
            {
                /* Fill out the 3 dim array that stores the correlations for */
                /* each code_book vector for each start lag */
                idx = PitchEstTables.SKP_Silk_CB_lags_stage3[ k ][ i ] - delta;
                for( j = 0; j < PITCH_EST_NB_STAGE3_LAGS; j++ )
                {
                    assert( idx + j < SCRATCH_SIZE );
                    assert( idx + j < lag_counter );
                    cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
                }
            }
            target_ptr_offset += sf_length;
        }
    }

    /**
     * @param energies_st3 3 DIM correlation array.
     * @param signal vector to correlate.
     * @param signal_offset offset of valid data.
     * @param start_lag start lag.
     * @param sf_length sub frame length.
     * @param complexity Complexity setting.
     */
    static void SKP_P_Ana_calc_energy_st3
    (
        float[][][] energies_st3,
        float signal[],           /* I vector to correlate                                            */
        int signal_offset,
        int start_lag,                  /* I start lag                                                      */
        int sf_length,                  /* I sub frame length                                               */
        int complexity                  /* I Complexity setting                                             */
    )
    /****************************************************************
    Calculate the energies for first two subframes. The energies are
    calculated recursively.
    ****************************************************************/
    {
        float[] target_ptr, basis_ptr;
        int target_ptr_offset, basis_ptr_offset;
        double      energy;
        int     k, i, j, lag_counter;
        int     cbk_offset, cbk_size, delta, idx;
        float[]   scratch_mem = new float[ SCRATCH_SIZE ];

        assert( complexity >= SigProcFIX.SKP_Silk_PITCH_EST_MIN_COMPLEX );
        assert( complexity <= SigProcFIX.SKP_Silk_PITCH_EST_MAX_COMPLEX );

        cbk_offset = PitchEstTables.SKP_Silk_cbk_offsets_stage3[ complexity ];
        cbk_size   = PitchEstTables.SKP_Silk_cbk_sizes_stage3[   complexity ];

        target_ptr = signal;
        target_ptr_offset = signal_offset+( sf_length << 2 );
        for( k = 0; k < PITCH_EST_NB_SUBFR; k++ )
        {
            lag_counter = 0;

            /* Calculate the energy for first lag */
            basis_ptr = target_ptr;
            basis_ptr_offset = target_ptr_offset - ( start_lag + PitchEstTables.SKP_Silk_Lag_range_stage3[complexity ][ k ][ 0 ]);
            energy = EnergyFLP.SKP_Silk_energy_FLP( basis_ptr,basis_ptr_offset, sf_length ) + 1e-3;
            assert( energy >= 0.0 );
            scratch_mem[lag_counter] = (float)energy;
            lag_counter++;

            for( i = 1; i < ( PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 1 ] - PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 0 ] + 1 ); i++ )
            {
                /* remove part outside new window */
                energy -= basis_ptr[basis_ptr_offset + sf_length - i] * basis_ptr[basis_ptr_offset + sf_length - i];
                assert( energy >= 0.0 );

                /* add part that comes into window */
                energy += basis_ptr[ basis_ptr_offset -i ] * basis_ptr[ basis_ptr_offset-i ];
                assert( energy >= 0.0 );
                assert( lag_counter < SCRATCH_SIZE );
                scratch_mem[lag_counter] = (float)energy;
                lag_counter++;
            }

            delta = PitchEstTables.SKP_Silk_Lag_range_stage3[ complexity ][ k ][ 0 ];
            for( i = cbk_offset; i < ( cbk_offset + cbk_size ); i++ )
            {
                /* Fill out the 3 dim array that stores the correlations for    */
                /* each code_book vector for each start lag                     */
                idx = PitchEstTables.SKP_Silk_CB_lags_stage3[ k ][ i ] - delta;
                for(j = 0; j < PITCH_EST_NB_STAGE3_LAGS; j++)
                {
                    assert( idx + j < SCRATCH_SIZE );
                    assert( idx + j < lag_counter );
                    energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
                    assert( energies_st3[ k ][ i ][ j ] >= 0.0f );
                }
            }
            target_ptr_offset += sf_length;
        }
    }
}
