/* Compute reflection coefficients from input signal */
internal static void silk_burg_modified(
BoxedValueInt res_nrg, /* O Residual energy */
BoxedValueInt res_nrg_Q, /* O Residual energy Q value */
int[] A_Q16, /* O Prediction coefficients (length order) */
short[] x, /* I Input signal, length: nb_subfr * ( D + subfr_length ) */
int x_ptr,
int minInvGain_Q30, /* I Inverse of max prediction gain */
int subfr_length, /* I Input signal subframe length (incl. D preceding samples) */
int nb_subfr, /* I Number of subframes stacked in x */
int D /* I Order */
)
{
int k, n, s, lz, rshifts, reached_max_gain;
int C0, num, nrg, rc_Q31, invGain_Q30, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;
int x_offset;
int[] C_first_row = new int[SilkConstants.SILK_MAX_ORDER_LPC];
int[] C_last_row = new int[SilkConstants.SILK_MAX_ORDER_LPC];
int[] Af_QA = new int[SilkConstants.SILK_MAX_ORDER_LPC];
int[] CAf = new int[SilkConstants.SILK_MAX_ORDER_LPC + 1];
int[] CAb = new int[SilkConstants.SILK_MAX_ORDER_LPC + 1];
int[] xcorr = new int[SilkConstants.SILK_MAX_ORDER_LPC];
long C0_64;
Inlines.OpusAssert(subfr_length * nb_subfr <= MAX_FRAME_SIZE);
/* Compute autocorrelations, added over subframes */
C0_64 = Inlines.silk_inner_prod16_aligned_64(x, x_ptr, x, x_ptr, subfr_length * nb_subfr);
lz = Inlines.silk_CLZ64(C0_64);
rshifts = 32 + 1 + N_BITS_HEAD_ROOM - lz;
if (rshifts > MAX_RSHIFTS)
{
rshifts = MAX_RSHIFTS;
}
if (rshifts < MIN_RSHIFTS)
{
rshifts = MIN_RSHIFTS;
}
if (rshifts > 0)
{
C0 = (int)Inlines.silk_RSHIFT64(C0_64, rshifts);
}
else
{
C0 = Inlines.silk_LSHIFT32((int)C0_64, -rshifts);
}
CAb[0] = CAf[0] = C0 + Inlines.silk_SMMUL(((int)((TuningParameters.FIND_LPC_COND_FAC) * ((long)1 << (32)) + 0.5)) /*Inlines.SILK_CONST(TuningParameters.FIND_LPC_COND_FAC, 32)*/, C0) + 1; /* Q(-rshifts) */
Arrays.MemSetInt(C_first_row, 0, SilkConstants.SILK_MAX_ORDER_LPC);
if (rshifts > 0)
{
for (s = 0; s < nb_subfr; s++)
{
x_offset = x_ptr + s * subfr_length;
for (n = 1; n < D + 1; n++)
{
C_first_row[n - 1] += (int)Inlines.silk_RSHIFT64(
Inlines.silk_inner_prod16_aligned_64(x, x_offset, x, x_offset + n, subfr_length - n), rshifts);
}
}
}
else
{
for (s = 0; s < nb_subfr; s++)
{
int i;
int d;
x_offset = x_ptr + s * subfr_length;
CeltPitchXCorr.pitch_xcorr(x, x_offset, x, x_offset + 1, xcorr, subfr_length - D, D);
for (n = 1; n < D + 1; n++)
{
for (i = n + subfr_length - D, d = 0; i < subfr_length; i++)
{
d = Inlines.MAC16_16(d, x[x_offset + i], x[x_offset + i - n]);
}
xcorr[n - 1] += d;
}
for (n = 1; n < D + 1; n++)
{
C_first_row[n - 1] += Inlines.silk_LSHIFT32(xcorr[n - 1], -rshifts);
}
}
}
Array.Copy(C_first_row, C_last_row, SilkConstants.SILK_MAX_ORDER_LPC);
/* Initialize */
CAb[0] = CAf[0] = C0 + Inlines.silk_SMMUL(((int)((TuningParameters.FIND_LPC_COND_FAC) * ((long)1 << (32)) + 0.5)) /*Inlines.SILK_CONST(TuningParameters.FIND_LPC_COND_FAC, 32)*/, C0) + 1; /* Q(-rshifts) */
invGain_Q30 = (int)1 << 30;
reached_max_gain = 0;
for (n = 0; n < D; n++)
{
/* Update first row of correlation matrix (without first element) */
/* Update last row of correlation matrix (without last element, stored in reversed order) */
/* Update C * Af */
/* Update C * flipud(Af) (stored in reversed order) */
if (rshifts > -2)
{
for (s = 0; s < nb_subfr; s++)
{
x_offset = x_ptr + s * subfr_length;
x1 = -Inlines.silk_LSHIFT32((int)x[x_offset + n], 16 - rshifts); /* Q(16-rshifts) */
x2 = -Inlines.silk_LSHIFT32((int)x[x_offset + subfr_length - n - 1], 16 - rshifts); /* Q(16-rshifts) */
tmp1 = Inlines.silk_LSHIFT32((int)x[x_offset + n], QA - 16); /* Q(QA-16) */
tmp2 = Inlines.silk_LSHIFT32((int)x[x_offset + subfr_length - n - 1], QA - 16); /* Q(QA-16) */
for (k = 0; k < n; k++)
{
C_first_row[k] = Inlines.silk_SMLAWB(C_first_row[k], x1, x[x_offset + n - k - 1]); /* Q( -rshifts ) */
C_last_row[k] = Inlines.silk_SMLAWB(C_last_row[k], x2, x[x_offset + subfr_length - n + k]); /* Q( -rshifts ) */
Atmp_QA = Af_QA[k];
tmp1 = Inlines.silk_SMLAWB(tmp1, Atmp_QA, x[x_offset + n - k - 1]); /* Q(QA-16) */
tmp2 = Inlines.silk_SMLAWB(tmp2, Atmp_QA, x[x_offset + subfr_length - n + k]); /* Q(QA-16) */
}
tmp1 = Inlines.silk_LSHIFT32(-tmp1, 32 - QA - rshifts); /* Q(16-rshifts) */
tmp2 = Inlines.silk_LSHIFT32(-tmp2, 32 - QA - rshifts); /* Q(16-rshifts) */
for (k = 0; k <= n; k++)
{
CAf[k] = Inlines.silk_SMLAWB(CAf[k], tmp1, x[x_offset + n - k]); /* Q( -rshift ) */
CAb[k] = Inlines.silk_SMLAWB(CAb[k], tmp2, x[x_offset + subfr_length - n + k - 1]); /* Q( -rshift ) */
}
}
}
else
{
for (s = 0; s < nb_subfr; s++)
{
x_offset = x_ptr + s * subfr_length;
x1 = -Inlines.silk_LSHIFT32((int)x[x_offset + n], -rshifts); /* Q( -rshifts ) */
x2 = -Inlines.silk_LSHIFT32((int)x[x_offset + subfr_length - n - 1], -rshifts); /* Q( -rshifts ) */
tmp1 = Inlines.silk_LSHIFT32((int)x[x_offset + n], 17); /* Q17 */
tmp2 = Inlines.silk_LSHIFT32((int)x[x_offset + subfr_length - n - 1], 17); /* Q17 */
for (k = 0; k < n; k++)
{
C_first_row[k] = Inlines.silk_MLA(C_first_row[k], x1, x[x_offset + n - k - 1]); /* Q( -rshifts ) */
C_last_row[k] = Inlines.silk_MLA(C_last_row[k], x2, x[x_offset + subfr_length - n + k]); /* Q( -rshifts ) */
Atmp1 = Inlines.silk_RSHIFT_ROUND(Af_QA[k], QA - 17); /* Q17 */
tmp1 = Inlines.silk_MLA(tmp1, x[x_offset + n - k - 1], Atmp1); /* Q17 */
tmp2 = Inlines.silk_MLA(tmp2, x[x_offset + subfr_length - n + k], Atmp1); /* Q17 */
}
tmp1 = -tmp1; /* Q17 */
tmp2 = -tmp2; /* Q17 */
for (k = 0; k <= n; k++)
{
CAf[k] = Inlines.silk_SMLAWW(CAf[k], tmp1,
Inlines.silk_LSHIFT32((int)x[x_offset + n - k], -rshifts - 1)); /* Q( -rshift ) */
CAb[k] = Inlines.silk_SMLAWW(CAb[k], tmp2,
Inlines.silk_LSHIFT32((int)x[x_offset + subfr_length - n + k - 1], -rshifts - 1)); /* Q( -rshift ) */
}
}
}
/* Calculate nominator and denominator for the next order reflection (parcor) coefficient */
tmp1 = C_first_row[n]; /* Q( -rshifts ) */
tmp2 = C_last_row[n]; /* Q( -rshifts ) */
num = 0; /* Q( -rshifts ) */
nrg = Inlines.silk_ADD32(CAb[0], CAf[0]); /* Q( 1-rshifts ) */
for (k = 0; k < n; k++)
{
Atmp_QA = Af_QA[k];
lz = Inlines.silk_CLZ32(Inlines.silk_abs(Atmp_QA)) - 1;
lz = Inlines.silk_min(32 - QA, lz);
Atmp1 = Inlines.silk_LSHIFT32(Atmp_QA, lz); /* Q( QA + lz ) */
tmp1 = Inlines.silk_ADD_LSHIFT32(tmp1, Inlines.silk_SMMUL(C_last_row[n - k - 1], Atmp1), 32 - QA - lz); /* Q( -rshifts ) */
tmp2 = Inlines.silk_ADD_LSHIFT32(tmp2, Inlines.silk_SMMUL(C_first_row[n - k - 1], Atmp1), 32 - QA - lz); /* Q( -rshifts ) */
num = Inlines.silk_ADD_LSHIFT32(num, Inlines.silk_SMMUL(CAb[n - k], Atmp1), 32 - QA - lz); /* Q( -rshifts ) */
nrg = Inlines.silk_ADD_LSHIFT32(nrg, Inlines.silk_SMMUL(Inlines.silk_ADD32(CAb[k + 1], CAf[k + 1]),
Atmp1), 32 - QA - lz); /* Q( 1-rshifts ) */
}
CAf[n + 1] = tmp1; /* Q( -rshifts ) */
CAb[n + 1] = tmp2; /* Q( -rshifts ) */
num = Inlines.silk_ADD32(num, tmp2); /* Q( -rshifts ) */
num = Inlines.silk_LSHIFT32(-num, 1); /* Q( 1-rshifts ) */
/* Calculate the next order reflection (parcor) coefficient */
if (Inlines.silk_abs(num) < nrg)
{
rc_Q31 = Inlines.silk_DIV32_varQ(num, nrg, 31);
}
else
{
rc_Q31 = (num > 0) ? int.MaxValue : int.MinValue;
}
/* Update inverse prediction gain */
tmp1 = ((int)1 << 30) - Inlines.silk_SMMUL(rc_Q31, rc_Q31);
tmp1 = Inlines.silk_LSHIFT(Inlines.silk_SMMUL(invGain_Q30, tmp1), 2);
if (tmp1 <= minInvGain_Q30)
{
/* Max prediction gain exceeded; set reflection coefficient such that max prediction gain is exactly hit */
tmp2 = ((int)1 << 30) - Inlines.silk_DIV32_varQ(minInvGain_Q30, invGain_Q30, 30); /* Q30 */
rc_Q31 = Inlines.silk_SQRT_APPROX(tmp2); /* Q15 */
/* Newton-Raphson iteration */
rc_Q31 = Inlines.silk_RSHIFT32(rc_Q31 + Inlines.silk_DIV32(tmp2, rc_Q31), 1); /* Q15 */
rc_Q31 = Inlines.silk_LSHIFT32(rc_Q31, 16); /* Q31 */
if (num < 0)
{
/* Ensure adjusted reflection coefficients has the original sign */
rc_Q31 = -rc_Q31;
}
invGain_Q30 = minInvGain_Q30;
reached_max_gain = 1;
}
else
{
invGain_Q30 = tmp1;
}
/* Update the AR coefficients */
for (k = 0; k < (n + 1) >> 1; k++)
{
tmp1 = Af_QA[k]; /* QA */
tmp2 = Af_QA[n - k - 1]; /* QA */
Af_QA[k] = Inlines.silk_ADD_LSHIFT32(tmp1, Inlines.silk_SMMUL(tmp2, rc_Q31), 1); /* QA */
Af_QA[n - k - 1] = Inlines.silk_ADD_LSHIFT32(tmp2, Inlines.silk_SMMUL(tmp1, rc_Q31), 1); /* QA */
}
Af_QA[n] = Inlines.silk_RSHIFT32(rc_Q31, 31 - QA); /* QA */
if (reached_max_gain != 0)
{
/* Reached max prediction gain; set remaining coefficients to zero and exit loop */
for (k = n + 1; k < D; k++)
{
Af_QA[k] = 0;
}
break;
}
/* Update C * Af and C * Ab */
for (k = 0; k <= n + 1; k++)
{
tmp1 = CAf[k]; /* Q( -rshifts ) */
tmp2 = CAb[n - k + 1]; /* Q( -rshifts ) */
CAf[k] = Inlines.silk_ADD_LSHIFT32(tmp1, Inlines.silk_SMMUL(tmp2, rc_Q31), 1); /* Q( -rshifts ) */
CAb[n - k + 1] = Inlines.silk_ADD_LSHIFT32(tmp2, Inlines.silk_SMMUL(tmp1, rc_Q31), 1); /* Q( -rshifts ) */
}
}
if (reached_max_gain != 0)
{
for (k = 0; k < D; k++)
{
/* Scale coefficients */
A_Q16[k] = -Inlines.silk_RSHIFT_ROUND(Af_QA[k], QA - 16);
}
/* Subtract energy of preceding samples from C0 */
if (rshifts > 0)
{
for (s = 0; s < nb_subfr; s++)
{
x_offset = x_ptr + s * subfr_length;
C0 -= (int)Inlines.silk_RSHIFT64(Inlines.silk_inner_prod16_aligned_64(x, x_offset, x, x_offset, D), rshifts);
}
}
else
{
for (s = 0; s < nb_subfr; s++)
{
x_offset = x_ptr + s * subfr_length;
C0 -= Inlines.silk_LSHIFT32(Inlines.silk_inner_prod_self(x, x_offset, D), -rshifts);
}
}
/* Approximate residual energy */
res_nrg.Val = Inlines.silk_LSHIFT(Inlines.silk_SMMUL(invGain_Q30, C0), 2);
res_nrg_Q.Val = 0 - rshifts;
}
else
{
/* Return residual energy */
nrg = CAf[0]; /* Q( -rshifts ) */
tmp1 = (int)1 << 16; /* Q16 */
for (k = 0; k < D; k++)
{
Atmp1 = Inlines.silk_RSHIFT_ROUND(Af_QA[k], QA - 16); /* Q16 */
nrg = Inlines.silk_SMLAWW(nrg, CAf[k + 1], Atmp1); /* Q( -rshifts ) */
tmp1 = Inlines.silk_SMLAWW(tmp1, Atmp1, Atmp1); /* Q16 */
A_Q16[k] = -Atmp1;
}
res_nrg.Val = Inlines.silk_SMLAWW(nrg, Inlines.silk_SMMUL(((int)((TuningParameters.FIND_LPC_COND_FAC) * ((long)1 << (32)) + 0.5)) /*Inlines.SILK_CONST(TuningParameters.FIND_LPC_COND_FAC, 32)*/, C0), -tmp1);/* Q( -rshifts ) */
res_nrg_Q.Val = -rshifts;
}
}
/*************************************************************/
/* FIXED POINT CORE PITCH ANALYSIS FUNCTION */
/*************************************************************/
internal static int silk_pitch_analysis_core( /* O Voicing estimate: 0 voiced, 1 unvoiced */
short[] frame, /* I Signal of length PE_FRAME_LENGTH_MS*Fs_kHz */
int[] pitch_out, /* O 4 pitch lag values */
BoxedValueShort lagIndex, /* O Lag Index */
BoxedValueSbyte contourIndex, /* O Pitch contour Index */
BoxedValueInt LTPCorr_Q15, /* I/O Normalized correlation; input: value from previous frame */
int prevLag, /* I Last lag of previous frame; set to zero is unvoiced */
int search_thres1_Q16, /* I First stage threshold for lag candidates 0 - 1 */
int search_thres2_Q13, /* I Final threshold for lag candidates 0 - 1 */
int Fs_kHz, /* I Sample frequency (kHz) */
int complexity, /* I Complexity setting, 0-2, where 2 is highest */
int nb_subfr /* I number of 5 ms subframes */
)
{
short[] frame_8kHz;
short[] frame_4kHz;
int[] filt_state = new int[6];
short[] input_frame_ptr;
int i, k, d, j;
short[] C;
int[] xcorr32;
short[] basis;
int basis_ptr;
short[] target;
int target_ptr;
int cross_corr, normalizer, energy, shift, energy_basis, energy_target;
int Cmax, length_d_srch, length_d_comp;
int[] d_srch = new int[SilkConstants.PE_D_SRCH_LENGTH];
short[] d_comp;
int sum, threshold, lag_counter;
int CBimax, CBimax_new, CBimax_old, lag, start_lag, end_lag, lag_new;
int CCmax, CCmax_b, CCmax_new_b, CCmax_new;
int[] CC = new int[SilkConstants.PE_NB_CBKS_STAGE2_EXT];
silk_pe_stage3_vals[] energies_st3;
silk_pe_stage3_vals[] cross_corr_st3;
int frame_length, frame_length_8kHz, frame_length_4kHz;
int sf_length;
int min_lag;
int max_lag;
int contour_bias_Q15, diff;
int nb_cbk_search;
int delta_lag_log2_sqr_Q7, lag_log2_Q7, prevLag_log2_Q7, prev_lag_bias_Q13;
sbyte[][] Lag_CB_ptr;
/* Check for valid sampling frequency */
Inlines.OpusAssert(Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16);
/* Check for valid complexity setting */
Inlines.OpusAssert(complexity >= SilkConstants.SILK_PE_MIN_COMPLEX);
Inlines.OpusAssert(complexity <= SilkConstants.SILK_PE_MAX_COMPLEX);
Inlines.OpusAssert(search_thres1_Q16 >= 0 && search_thres1_Q16 <= (1 << 16));
Inlines.OpusAssert(search_thres2_Q13 >= 0 && search_thres2_Q13 <= (1 << 13));
/* Set up frame lengths max / min lag for the sampling frequency */
frame_length = (SilkConstants.PE_LTP_MEM_LENGTH_MS + nb_subfr * SilkConstants.PE_SUBFR_LENGTH_MS) * Fs_kHz;
frame_length_4kHz = (SilkConstants.PE_LTP_MEM_LENGTH_MS + nb_subfr * SilkConstants.PE_SUBFR_LENGTH_MS) * 4;
frame_length_8kHz = (SilkConstants.PE_LTP_MEM_LENGTH_MS + nb_subfr * SilkConstants.PE_SUBFR_LENGTH_MS) * 8;
sf_length = SilkConstants.PE_SUBFR_LENGTH_MS * Fs_kHz;
min_lag = SilkConstants.PE_MIN_LAG_MS * Fs_kHz;
max_lag = SilkConstants.PE_MAX_LAG_MS * Fs_kHz - 1;
/* Resample from input sampled at Fs_kHz to 8 kHz */
frame_8kHz = new short[frame_length_8kHz];
if (Fs_kHz == 16)
{
Arrays.MemSetInt(filt_state, 0, 2);
Resampler.silk_resampler_down2(filt_state, frame_8kHz, frame, frame_length);
}
else if (Fs_kHz == 12)
{
Arrays.MemSetInt(filt_state, 0, 6);
Resampler.silk_resampler_down2_3(filt_state, frame_8kHz, frame, frame_length);
}
else
{
Inlines.OpusAssert(Fs_kHz == 8);
Array.Copy(frame, frame_8kHz, frame_length_8kHz);
}
/* Decimate again to 4 kHz */
Arrays.MemSetInt(filt_state, 0, 2); /* Set state to zero */
frame_4kHz = new short[frame_length_4kHz];
Resampler.silk_resampler_down2(filt_state, frame_4kHz, frame_8kHz, frame_length_8kHz);
/* Low-pass filter */
for (i = frame_length_4kHz - 1; i > 0; i--)
{
frame_4kHz[i] = Inlines.silk_ADD_SAT16(frame_4kHz[i], frame_4kHz[i - 1]);
}
/*******************************************************************************
** Scale 4 kHz signal down to prevent correlations measures from overflowing
** find scaling as max scaling for each 8kHz(?) subframe
*******************************************************************************/
/* Inner product is calculated with different lengths, so scale for the worst case */
SumSqrShift.silk_sum_sqr_shift(out energy, out shift, frame_4kHz, frame_length_4kHz);
if (shift > 0)
{
shift = Inlines.silk_RSHIFT(shift, 1);
for (i = 0; i < frame_length_4kHz; i++)
{
frame_4kHz[i] = Inlines.silk_RSHIFT16(frame_4kHz[i], shift);
}
}
/******************************************************************************
* FIRST STAGE, operating in 4 khz
******************************************************************************/
C = new short[nb_subfr * CSTRIDE_8KHZ];
xcorr32 = new int[MAX_LAG_4KHZ - MIN_LAG_4KHZ + 1];
Arrays.MemSetShort(C, 0, (nb_subfr >> 1) * CSTRIDE_4KHZ);
target = frame_4kHz;
target_ptr = Inlines.silk_LSHIFT(SF_LENGTH_4KHZ, 2);
for (k = 0; k < nb_subfr >> 1; k++)
{
basis = target;
basis_ptr = target_ptr - MIN_LAG_4KHZ;
CeltPitchXCorr.pitch_xcorr(target, target_ptr, target, target_ptr - MAX_LAG_4KHZ, xcorr32, SF_LENGTH_8KHZ, MAX_LAG_4KHZ - MIN_LAG_4KHZ + 1);
/* Calculate first vector products before loop */
cross_corr = xcorr32[MAX_LAG_4KHZ - MIN_LAG_4KHZ];
normalizer = Inlines.silk_inner_prod_self(target, target_ptr, SF_LENGTH_8KHZ);
normalizer = Inlines.silk_ADD32(normalizer, Inlines.silk_inner_prod_self(basis, basis_ptr, SF_LENGTH_8KHZ));
normalizer = Inlines.silk_ADD32(normalizer, Inlines.silk_SMULBB(SF_LENGTH_8KHZ, 4000));
Inlines.MatrixSet(C, k, 0, CSTRIDE_4KHZ,
(short)Inlines.silk_DIV32_varQ(cross_corr, normalizer, 13 + 1)); /* Q13 */
/* From now on normalizer is computed recursively */
for (d = MIN_LAG_4KHZ + 1; d <= MAX_LAG_4KHZ; d++)
{
basis_ptr--;
cross_corr = xcorr32[MAX_LAG_4KHZ - d];
/* Add contribution of new sample and remove contribution from oldest sample */
normalizer = Inlines.silk_ADD32(normalizer,
Inlines.silk_SMULBB(basis[basis_ptr], basis[basis_ptr]) -
Inlines.silk_SMULBB(basis[basis_ptr + SF_LENGTH_8KHZ], basis[basis_ptr + SF_LENGTH_8KHZ]));
Inlines.MatrixSet(C, k, d - MIN_LAG_4KHZ, CSTRIDE_4KHZ,
(short)Inlines.silk_DIV32_varQ(cross_corr, normalizer, 13 + 1)); /* Q13 */
}
/* Update target pointer */
target_ptr += SF_LENGTH_8KHZ;
}
/* Combine two subframes into single correlation measure and apply short-lag bias */
if (nb_subfr == SilkConstants.PE_MAX_NB_SUBFR)
{
for (i = MAX_LAG_4KHZ; i >= MIN_LAG_4KHZ; i--)
{
sum = (int)Inlines.MatrixGet(C, 0, i - MIN_LAG_4KHZ, CSTRIDE_4KHZ)
+ (int)Inlines.MatrixGet(C, 1, i - MIN_LAG_4KHZ, CSTRIDE_4KHZ); /* Q14 */
sum = Inlines.silk_SMLAWB(sum, sum, Inlines.silk_LSHIFT(-i, 4)); /* Q14 */
C[i - MIN_LAG_4KHZ] = (short)sum; /* Q14 */
}
}
else
{
/* Only short-lag bias */
for (i = MAX_LAG_4KHZ; i >= MIN_LAG_4KHZ; i--)
{
sum = Inlines.silk_LSHIFT((int)C[i - MIN_LAG_4KHZ], 1); /* Q14 */
sum = Inlines.silk_SMLAWB(sum, sum, Inlines.silk_LSHIFT(-i, 4)); /* Q14 */
C[i - MIN_LAG_4KHZ] = (short)sum; /* Q14 */
}
}
/* Sort */
length_d_srch = Inlines.silk_ADD_LSHIFT32(4, complexity, 1);
Inlines.OpusAssert(3 * length_d_srch <= SilkConstants.PE_D_SRCH_LENGTH);
Sort.silk_insertion_sort_decreasing_int16(C, d_srch, CSTRIDE_4KHZ, length_d_srch);
/* Escape if correlation is very low already here */
Cmax = (int)C[0]; /* Q14 */
if (Cmax < ((int)((0.2f) * ((long)1 << (14)) + 0.5)) /*Inlines.SILK_CONST(0.2f, 14)*/)
{
Arrays.MemSetInt(pitch_out, 0, nb_subfr);
LTPCorr_Q15.Val = 0;
lagIndex.Val = 0;
contourIndex.Val = 0;
return(1);
}
threshold = Inlines.silk_SMULWB(search_thres1_Q16, Cmax);
for (i = 0; i < length_d_srch; i++)
{
/* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
if (C[i] > threshold)
{
d_srch[i] = Inlines.silk_LSHIFT(d_srch[i] + MIN_LAG_4KHZ, 1);
}
else
{
length_d_srch = i;
break;
}
}
Inlines.OpusAssert(length_d_srch > 0);
d_comp = new short[D_COMP_STRIDE];
for (i = D_COMP_MIN; i < D_COMP_MAX; i++)
{
d_comp[i - D_COMP_MIN] = 0;
}
for (i = 0; i < length_d_srch; i++)
{
d_comp[d_srch[i] - D_COMP_MIN] = 1;
}
/* Convolution */
for (i = D_COMP_MAX - 1; i >= MIN_LAG_8KHZ; i--)
{
d_comp[i - D_COMP_MIN] += (short)(d_comp[i - 1 - D_COMP_MIN] + d_comp[i - 2 - D_COMP_MIN]);
}
length_d_srch = 0;
for (i = MIN_LAG_8KHZ; i < MAX_LAG_8KHZ + 1; i++)
{
if (d_comp[i + 1 - D_COMP_MIN] > 0)
{
d_srch[length_d_srch] = i;
length_d_srch++;
}
}
/* Convolution */
for (i = D_COMP_MAX - 1; i >= MIN_LAG_8KHZ; i--)
{
d_comp[i - D_COMP_MIN] += (short)(d_comp[i - 1 - D_COMP_MIN] + d_comp[i - 2 - D_COMP_MIN] + d_comp[i - 3 - D_COMP_MIN]);
}
length_d_comp = 0;
for (i = MIN_LAG_8KHZ; i < D_COMP_MAX; i++)
{
if (d_comp[i - D_COMP_MIN] > 0)
{
d_comp[length_d_comp] = (short)(i - 2);
length_d_comp++;
}
}
/**********************************************************************************
** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
*************************************************************************************/
/******************************************************************************
** Scale signal down to avoid correlations measures from overflowing
*******************************************************************************/
/* find scaling as max scaling for each subframe */
SumSqrShift.silk_sum_sqr_shift(out energy, out shift, frame_8kHz, frame_length_8kHz);
if (shift > 0)
{
shift = Inlines.silk_RSHIFT(shift, 1);
for (i = 0; i < frame_length_8kHz; i++)
{
frame_8kHz[i] = Inlines.silk_RSHIFT16(frame_8kHz[i], shift);
}
}
/*********************************************************************************
* Find energy of each subframe projected onto its history, for a range of delays
*********************************************************************************/
Arrays.MemSetShort(C, 0, nb_subfr * CSTRIDE_8KHZ);
target = frame_8kHz;
target_ptr = SilkConstants.PE_LTP_MEM_LENGTH_MS * 8;
for (k = 0; k < nb_subfr; k++)
{
energy_target = Inlines.silk_ADD32(Inlines.silk_inner_prod(target, target_ptr, target, target_ptr, SF_LENGTH_8KHZ), 1);
for (j = 0; j < length_d_comp; j++)
{
d = d_comp[j];
basis = target;
basis_ptr = target_ptr - d;
cross_corr = Inlines.silk_inner_prod(target, target_ptr, basis, basis_ptr, SF_LENGTH_8KHZ);
if (cross_corr > 0)
{
energy_basis = Inlines.silk_inner_prod_self(basis, basis_ptr, SF_LENGTH_8KHZ);
Inlines.MatrixSet(C, k, d - (MIN_LAG_8KHZ - 2), CSTRIDE_8KHZ,
(short)Inlines.silk_DIV32_varQ(cross_corr,
Inlines.silk_ADD32(energy_target,
energy_basis),
13 + 1)); /* Q13 */
}
else
{
Inlines.MatrixSet <short>(C, k, d - (MIN_LAG_8KHZ - 2), CSTRIDE_8KHZ, 0);
}
}
target_ptr += SF_LENGTH_8KHZ;
}
/* search over lag range and lags codebook */
/* scale factor for lag codebook, as a function of center lag */
CCmax = int.MinValue;
CCmax_b = int.MinValue;
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 = Inlines.silk_DIV32_16(Inlines.silk_LSHIFT(prevLag, 1), 3);
}
else if (Fs_kHz == 16)
{
prevLag = Inlines.silk_RSHIFT(prevLag, 1);
}
prevLag_log2_Q7 = Inlines.silk_lin2log((int)prevLag);
}
else
{
prevLag_log2_Q7 = 0;
}
Inlines.OpusAssert(search_thres2_Q13 == Inlines.silk_SAT16(search_thres2_Q13));
/* Set up stage 2 codebook based on number of subframes */
if (nb_subfr == SilkConstants.PE_MAX_NB_SUBFR)
{
Lag_CB_ptr = Tables.silk_CB_lags_stage2;
if (Fs_kHz == 8 && complexity > SilkConstants.SILK_PE_MIN_COMPLEX)
{
/* If input is 8 khz use a larger codebook here because it is last stage */
nb_cbk_search = SilkConstants.PE_NB_CBKS_STAGE2_EXT;
}
else
{
nb_cbk_search = SilkConstants.PE_NB_CBKS_STAGE2;
}
}
else
{
Lag_CB_ptr = Tables.silk_CB_lags_stage2_10_ms;
nb_cbk_search = SilkConstants.PE_NB_CBKS_STAGE2_10MS;
}
for (k = 0; k < length_d_srch; k++)
{
d = d_srch[k];
for (j = 0; j < nb_cbk_search; j++)
{
CC[j] = 0;
for (i = 0; i < nb_subfr; i++)
{
int d_subfr;
/* Try all codebooks */
d_subfr = d + Lag_CB_ptr[i][j];
CC[j] = CC[j]
+ (int)Inlines.MatrixGet(C, i,
d_subfr - (MIN_LAG_8KHZ - 2),
CSTRIDE_8KHZ);
}
}
/* Find best codebook */
CCmax_new = int.MinValue;
CBimax_new = 0;
for (i = 0; i < nb_cbk_search; i++)
{
if (CC[i] > CCmax_new)
{
CCmax_new = CC[i];
CBimax_new = i;
}
}
/* Bias towards shorter lags */
lag_log2_Q7 = Inlines.silk_lin2log(d); /* Q7 */
Inlines.OpusAssert(lag_log2_Q7 == Inlines.silk_SAT16(lag_log2_Q7));
Inlines.OpusAssert(nb_subfr * ((int)((SilkConstants.PE_SHORTLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_SHORTLAG_BIAS, 13)*/ == Inlines.silk_SAT16(nb_subfr * ((int)((SilkConstants.PE_SHORTLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_SHORTLAG_BIAS, 13)*/));
CCmax_new_b = CCmax_new - Inlines.silk_RSHIFT(Inlines.silk_SMULBB(nb_subfr * ((int)((SilkConstants.PE_SHORTLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_SHORTLAG_BIAS, 13)*/, lag_log2_Q7), 7); /* Q13 */
/* Bias towards previous lag */
Inlines.OpusAssert(nb_subfr * ((int)((SilkConstants.PE_PREVLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_PREVLAG_BIAS, 13)*/ == Inlines.silk_SAT16(nb_subfr * ((int)((SilkConstants.PE_PREVLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_PREVLAG_BIAS, 13)*/));
if (prevLag > 0)
{
delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;
Inlines.OpusAssert(delta_lag_log2_sqr_Q7 == Inlines.silk_SAT16(delta_lag_log2_sqr_Q7));
delta_lag_log2_sqr_Q7 = Inlines.silk_RSHIFT(Inlines.silk_SMULBB(delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7), 7);
prev_lag_bias_Q13 = Inlines.silk_RSHIFT(Inlines.silk_SMULBB(nb_subfr * ((int)((SilkConstants.PE_PREVLAG_BIAS) * ((long)1 << (13)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_PREVLAG_BIAS, 13)*/, LTPCorr_Q15.Val), 15); /* Q13 */
prev_lag_bias_Q13 = Inlines.silk_DIV32(Inlines.silk_MUL(prev_lag_bias_Q13, delta_lag_log2_sqr_Q7), delta_lag_log2_sqr_Q7 + ((int)((0.5f) * ((long)1 << (7)) + 0.5)) /*Inlines.SILK_CONST(0.5f, 7)*/);
CCmax_new_b -= prev_lag_bias_Q13; /* Q13 */
}
if (CCmax_new_b > CCmax_b && /* Find maximum biased correlation */
CCmax_new > Inlines.silk_SMULBB(nb_subfr, search_thres2_Q13) && /* Correlation needs to be high enough to be voiced */
Tables.silk_CB_lags_stage2[0][CBimax_new] <= MIN_LAG_8KHZ /* Lag must be in range */
)
{
CCmax_b = CCmax_new_b;
CCmax = CCmax_new;
lag = d;
CBimax = CBimax_new;
}
}
if (lag == -1)
{
/* No suitable candidate found */
Arrays.MemSetInt(pitch_out, 0, nb_subfr);
LTPCorr_Q15.Val = 0;
lagIndex.Val = 0;
contourIndex.Val = 0;
return(1);
}
/* Output normalized correlation */
LTPCorr_Q15.Val = (int)Inlines.silk_LSHIFT(Inlines.silk_DIV32_16(CCmax, nb_subfr), 2);
Inlines.OpusAssert(LTPCorr_Q15.Val >= 0);
if (Fs_kHz > 8)
{
short[] scratch_mem;
/***************************************************************************/
/* Scale input signal down to avoid correlations measures from overflowing */
/***************************************************************************/
/* find scaling as max scaling for each subframe */
SumSqrShift.silk_sum_sqr_shift(out energy, out shift, frame, frame_length);
if (shift > 0)
{
scratch_mem = new short[frame_length];
/* Move signal to scratch mem because the input signal should be unchanged */
shift = Inlines.silk_RSHIFT(shift, 1);
for (i = 0; i < frame_length; i++)
{
scratch_mem[i] = Inlines.silk_RSHIFT16(frame[i], shift);
}
input_frame_ptr = scratch_mem;
}
else
{
input_frame_ptr = frame;
}
/* Search in original signal */
CBimax_old = CBimax;
/* Compensate for decimation */
Inlines.OpusAssert(lag == Inlines.silk_SAT16(lag));
if (Fs_kHz == 12)
{
lag = Inlines.silk_RSHIFT(Inlines.silk_SMULBB(lag, 3), 1);
}
else if (Fs_kHz == 16)
{
lag = Inlines.silk_LSHIFT(lag, 1);
}
else
{
lag = Inlines.silk_SMULBB(lag, 3);
}
lag = Inlines.silk_LIMIT_int(lag, min_lag, max_lag);
start_lag = Inlines.silk_max_int(lag - 2, min_lag);
end_lag = Inlines.silk_min_int(lag + 2, max_lag);
lag_new = lag; /* to avoid undefined lag */
CBimax = 0; /* to avoid undefined lag */
CCmax = int.MinValue;
/* pitch lags according to second stage */
for (k = 0; k < nb_subfr; k++)
{
pitch_out[k] = lag + 2 * Tables.silk_CB_lags_stage2[k][CBimax_old];
}
/* Set up codebook parameters according to complexity setting and frame length */
if (nb_subfr == SilkConstants.PE_MAX_NB_SUBFR)
{
nb_cbk_search = (int)Tables.silk_nb_cbk_searchs_stage3[complexity];
Lag_CB_ptr = Tables.silk_CB_lags_stage3;
}
else
{
nb_cbk_search = SilkConstants.PE_NB_CBKS_STAGE3_10MS;
Lag_CB_ptr = Tables.silk_CB_lags_stage3_10_ms;
}
/* Calculate the correlations and energies needed in stage 3 */
energies_st3 = new silk_pe_stage3_vals[nb_subfr * nb_cbk_search];
cross_corr_st3 = new silk_pe_stage3_vals[nb_subfr * nb_cbk_search];
for (int c = 0; c < nb_subfr * nb_cbk_search; c++)
{
energies_st3[c] = new silk_pe_stage3_vals(); // fixme: these can be replaced with a linearized array probably, or at least a struct
cross_corr_st3[c] = new silk_pe_stage3_vals();
}
silk_P_Ana_calc_corr_st3(cross_corr_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity);
silk_P_Ana_calc_energy_st3(energies_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity);
lag_counter = 0;
Inlines.OpusAssert(lag == Inlines.silk_SAT16(lag));
contour_bias_Q15 = Inlines.silk_DIV32_16(((int)((SilkConstants.PE_FLATCONTOUR_BIAS) * ((long)1 << (15)) + 0.5)) /*Inlines.SILK_CONST(SilkConstants.PE_FLATCONTOUR_BIAS, 15)*/, lag);
target = input_frame_ptr;
target_ptr = SilkConstants.PE_LTP_MEM_LENGTH_MS * Fs_kHz;
energy_target = Inlines.silk_ADD32(Inlines.silk_inner_prod_self(target, target_ptr, nb_subfr * sf_length), 1);
for (d = start_lag; d <= end_lag; d++)
{
for (j = 0; j < nb_cbk_search; j++)
{
cross_corr = 0;
energy = energy_target;
for (k = 0; k < nb_subfr; k++)
{
cross_corr = Inlines.silk_ADD32(cross_corr,
Inlines.MatrixGet(cross_corr_st3, k, j,
nb_cbk_search).Values[lag_counter]);
energy = Inlines.silk_ADD32(energy,
Inlines.MatrixGet(energies_st3, k, j,
nb_cbk_search).Values[lag_counter]);
Inlines.OpusAssert(energy >= 0);
}
if (cross_corr > 0)
{
CCmax_new = Inlines.silk_DIV32_varQ(cross_corr, energy, 13 + 1); /* Q13 */
/* Reduce depending on flatness of contour */
diff = short.MaxValue - Inlines.silk_MUL(contour_bias_Q15, j); /* Q15 */
Inlines.OpusAssert(diff == Inlines.silk_SAT16(diff));
CCmax_new = Inlines.silk_SMULWB(CCmax_new, diff); /* Q14 */
}
else
{
CCmax_new = 0;
}
if (CCmax_new > CCmax && (d + Tables.silk_CB_lags_stage3[0][j]) <= max_lag)
{
CCmax = CCmax_new;
lag_new = d;
CBimax = j;
}
}
lag_counter++;
}
for (k = 0; k < nb_subfr; k++)
{
pitch_out[k] = lag_new + Lag_CB_ptr[k][CBimax];
pitch_out[k] = Inlines.silk_LIMIT(pitch_out[k], min_lag, SilkConstants.PE_MAX_LAG_MS * Fs_kHz);
}
lagIndex.Val = (short)(lag_new - min_lag);
contourIndex.Val = (sbyte)CBimax;
}
else /* Fs_kHz == 8 */
/* Save Lags */
{
for (k = 0; k < nb_subfr; k++)
{
pitch_out[k] = lag + Lag_CB_ptr[k][CBimax];
pitch_out[k] = Inlines.silk_LIMIT(pitch_out[k], MIN_LAG_8KHZ, SilkConstants.PE_MAX_LAG_MS * 8);
}
lagIndex.Val = (short)(lag - MIN_LAG_8KHZ);
contourIndex.Val = (sbyte)CBimax;
}
Inlines.OpusAssert(lagIndex.Val >= 0);
/* return as voiced */
return(0);
}
internal static int _celt_autocorr(
short[] x, /* in: [0...n-1] samples x */
int[] ac, /* out: [0...lag-1] ac values */
int lag,
int n
)
{
int d;
int i, k;
int fastN = n - lag;
int shift;
short[] xptr;
short[] xx = new short[n];
Inlines.OpusAssert(n > 0);
xptr = x;
shift = 0;
{
int ac0;
ac0 = 1 + (n << 7);
if ((n & 1) != 0)
{
ac0 += Inlines.SHR32(Inlines.MULT16_16(xptr[0], xptr[0]), 9);
}
for (i = (n & 1); i < n; i += 2)
{
ac0 += Inlines.SHR32(Inlines.MULT16_16(xptr[i], xptr[i]), 9);
ac0 += Inlines.SHR32(Inlines.MULT16_16(xptr[i + 1], xptr[i + 1]), 9);
}
shift = Inlines.celt_ilog2(ac0) - 30 + 10;
shift = (shift) / 2;
if (shift > 0)
{
for (i = 0; i < n; i++)
{
xx[i] = (short)(Inlines.PSHR32(xptr[i], shift));
}
xptr = xx;
}
else
{
shift = 0;
}
}
CeltPitchXCorr.pitch_xcorr(xptr, xptr, ac, fastN, lag + 1);
for (k = 0; k <= lag; k++)
{
for (i = k + fastN, d = 0; i < n; i++)
{
d = Inlines.MAC16_16(d, xptr[i], xptr[i - k]);
}
ac[k] += d;
}
shift = 2 * shift;
if (shift <= 0)
{
ac[0] += Inlines.SHL32((int)1, -shift);
}
if (ac[0] < 268435456)
{
int shift2 = 29 - Inlines.EC_ILOG((uint)ac[0]);
for (i = 0; i <= lag; i++)
{
ac[i] = Inlines.SHL32(ac[i], shift2);
}
shift -= shift2;
}
else if (ac[0] >= 536870912)
{
int shift2 = 1;
if (ac[0] >= 1073741824)
{
shift2++;
}
for (i = 0; i <= lag; i++)
{
ac[i] = Inlines.SHR32(ac[i], shift2);
}
shift += shift2;
}
return(shift);
}
/***********************************************************************
* 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.
***********************************************************************/
private static void silk_P_Ana_calc_corr_st3(
silk_pe_stage3_vals[] cross_corr_st3, /* O 3 DIM correlation array */
short[] frame, /* I vector to correlate */
int start_lag, /* I lag offset to search around */
int sf_length, /* I length of a 5 ms subframe */
int nb_subfr, /* I number of subframes */
int complexity /* I Complexity setting */
)
{
int target_ptr;
int i, j, k, lag_counter, lag_low, lag_high;
int nb_cbk_search, delta, idx;
int[] scratch_mem;
int[] xcorr32;
sbyte[][] Lag_range_ptr;
sbyte[][] Lag_CB_ptr;
Inlines.OpusAssert(complexity >= SilkConstants.SILK_PE_MIN_COMPLEX);
Inlines.OpusAssert(complexity <= SilkConstants.SILK_PE_MAX_COMPLEX);
if (nb_subfr == SilkConstants.PE_MAX_NB_SUBFR)
{
Lag_range_ptr = Tables.silk_Lag_range_stage3[complexity];
Lag_CB_ptr = Tables.silk_CB_lags_stage3;
nb_cbk_search = Tables.silk_nb_cbk_searchs_stage3[complexity];
}
else
{
Inlines.OpusAssert(nb_subfr == SilkConstants.PE_MAX_NB_SUBFR >> 1);
Lag_range_ptr = Tables.silk_Lag_range_stage3_10_ms;
Lag_CB_ptr = Tables.silk_CB_lags_stage3_10_ms;
nb_cbk_search = SilkConstants.PE_NB_CBKS_STAGE3_10MS;
}
scratch_mem = new int[SCRATCH_SIZE];
xcorr32 = new int[SCRATCH_SIZE];
target_ptr = Inlines.silk_LSHIFT(sf_length, 2); /* Pointer to middle of frame */
for (k = 0; k < nb_subfr; k++)
{
lag_counter = 0;
/* Calculate the correlations for each subframe */
lag_low = Lag_range_ptr[k][0];
lag_high = Lag_range_ptr[k][1];
Inlines.OpusAssert(lag_high - lag_low + 1 <= SCRATCH_SIZE);
CeltPitchXCorr.pitch_xcorr(frame, target_ptr, frame, target_ptr - start_lag - lag_high, xcorr32, sf_length, lag_high - lag_low + 1);
for (j = lag_low; j <= lag_high; j++)
{
Inlines.OpusAssert(lag_counter < SCRATCH_SIZE);
scratch_mem[lag_counter] = xcorr32[lag_high - j];
lag_counter++;
}
delta = Lag_range_ptr[k][0];
for (i = 0; i < nb_cbk_search; i++)
{
/* Fill out the 3 dim array that stores the correlations for */
/* each code_book vector for each start lag */
idx = Lag_CB_ptr[k][i] - delta;
for (j = 0; j < SilkConstants.PE_NB_STAGE3_LAGS; j++)
{
Inlines.OpusAssert(idx + j < SCRATCH_SIZE);
Inlines.OpusAssert(idx + j < lag_counter);
Inlines.MatrixGet(cross_corr_st3, k, i, nb_cbk_search).Values[j] =
scratch_mem[idx + j];
}
}
target_ptr += sf_length;
}
}
