/*
 * Jitsi, the OpenSource Java VoIP and Instant Messaging client.
 * 
 * Distributable under LGPL license. See terms of license at gnu.org.
 */
package org.atalk.impl.neomedia.codec.audio.silk;

import static org.atalk.impl.neomedia.codec.audio.silk.CommonPitchEstDefines.*;
import static org.atalk.impl.neomedia.codec.audio.silk.Macros.*;
import static org.atalk.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;
		}
	}
}
