void w_dec_10i40_35bits(int16_t index[], /* (i) : index of 10 pulses (sign+position) */
int16_t cod[] /* (o) : algebraic (fixed) codebook w_excitation */
)
{
static const int16_t dgray[8] = { 0, 1, 3, 2, 5, 6, 4, 7 };
int16_t i, j, pos1, pos2, sign, tmp;
for (i = 0; i < L_CODE; i++) {
cod[i] = 0;
}
/* decode the positions and signs of pulses and build the codeword */
for (j = 0; j < NB_TRACK; j++) {
/* compute index i */
tmp = index[j];
i = tmp & 7;
i = dgray[i];
i = w_extract_l(w_L_w_shr(w_L_w_mult(i, 5), 1));
pos1 = w_add(i, j); /* position of pulse "j" */
i = w_shr(tmp, 3) & 1;
if (i == 0) {
sign = 4096; /* +1.0 */
} else {
sign = -4096; /* -1.0 */
}
cod[pos1] = sign;
/* compute index i */
i = index[w_add(j, 5)] & 7;
i = dgray[i];
i = w_extract_l(w_L_w_shr(w_L_w_mult(i, 5), 1));
pos2 = w_add(i, j); /* position of pulse "j+5" */
if (w_sub(pos2, pos1) < 0) {
sign = w_negate(sign);
}
cod[pos2] = w_add(cod[pos2], sign);
}
return;
}
void w_Q_plsf_5(int16_t * lsp1, /* input : 1st LSP vector */
int16_t * lsp2, /* input : 2nd LSP vector */
int16_t * lsp1_q, /* output: quantized 1st LSP vector */
int16_t * lsp2_q, /* output: quantized 2nd LSP vector */
int16_t * indice, /* output: quantization indices of 5 matrices */
int16_t w_txdtx_ctrl /* input : tx dtx control word */
)
{
int16_t i;
int16_t lsf1[M], lsf2[M], wf1[M], wf2[M], lsf_p[M], lsf_r1[M], lsf_r2[M];
int16_t lsf1_q[M], lsf2_q[M];
int16_t lsf_aver[M];
static int16_t w_lsf_p_CN[M];
/* convert LSFs to normalize frequency domain 0..16384 */
w_Lsp_lsf(lsp1, lsf1, M);
w_Lsp_lsf(lsp2, lsf2, M);
/* Update LSF CN quantizer "memory" */
if ((w_txdtx_ctrl & TX_SP_FLAG) == 0
&& (w_txdtx_ctrl & TX_PREV_HANGOVER_ACTIVE) != 0) {
update_w_lsf_p_CN(w_lsf_old_tx, w_lsf_p_CN);
}
if ((w_txdtx_ctrl & TX_SID_UPDATE) != 0) {
/* New SID frame is to be sent:
Compute average of the current LSFs and the LSFs in the history */
w_aver_lsf_history(w_lsf_old_tx, lsf1, lsf2, lsf_aver);
}
/* Update LSF history with unquantized LSFs when no w_speech activity
is present */
if ((w_txdtx_ctrl & TX_SP_FLAG) == 0) {
w_update_lsf_history(lsf1, lsf2, w_lsf_old_tx);
}
if ((w_txdtx_ctrl & TX_SID_UPDATE) != 0) {
/* Compute LSF weighting factors for lsf2, using averaged LSFs */
/* Set LSF weighting factors for lsf1 to w_zero */
/* Replace lsf1 and lsf2 by the averaged LSFs */
w_Lsf_wt(lsf_aver, wf2);
for (i = 0; i < M; i++) {
wf1[i] = 0;
lsf1[i] = lsf_aver[i];
lsf2[i] = lsf_aver[i];
}
} else {
/* Compute LSF weighting factors */
w_Lsf_wt(lsf1, wf1);
w_Lsf_wt(lsf2, wf2);
}
/* Compute w_predicted LSF and w_prediction w_error */
if ((w_txdtx_ctrl & TX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
lsf_p[i] =
w_add(w_mean_lsf[i],
w_mult(w_past_r2_q[i], PRED_FAC));
lsf_r1[i] = w_sub(lsf1[i], lsf_p[i]);
lsf_r2[i] = w_sub(lsf2[i], lsf_p[i]);
}
} else {
for (i = 0; i < M; i++) {
lsf_r1[i] = w_sub(lsf1[i], w_lsf_p_CN[i]);
lsf_r2[i] = w_sub(lsf2[i], w_lsf_p_CN[i]);
}
}
/*---- Split-VQ of w_prediction w_error ----*/
indice[0] = Vq_w_subvec(&lsf_r1[0], &lsf_r2[0], w_dico1_lsf,
&wf1[0], &wf2[0], DICO1_SIZE);
indice[1] = Vq_w_subvec(&lsf_r1[2], &lsf_r2[2], w_dico2_lsf,
&wf1[2], &wf2[2], DICO2_SIZE);
indice[2] = Vq_w_subvec_s(&lsf_r1[4], &lsf_r2[4], w_dico3_lsf,
&wf1[4], &wf2[4], DICO3_SIZE);
indice[3] = Vq_w_subvec(&lsf_r1[6], &lsf_r2[6], w_dico4_lsf,
&wf1[6], &wf2[6], DICO4_SIZE);
indice[4] = Vq_w_subvec(&lsf_r1[8], &lsf_r2[8], w_dico5_lsf,
&wf1[8], &wf2[8], DICO5_SIZE);
/* Compute quantized LSFs and update the past quantized residual */
/* In case of no w_speech activity, skip computing the quantized LSFs,
and set w_past_r2_q to w_zero (initial value) */
if ((w_txdtx_ctrl & TX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
lsf1_q[i] = w_add(lsf_r1[i], lsf_p[i]);
lsf2_q[i] = w_add(lsf_r2[i], lsf_p[i]);
w_past_r2_q[i] = lsf_r2[i];
}
/* verification that LSFs has minimum distance of LSF_GAP */
w_Reorder_lsf(lsf1_q, LSF_GAP, M);
w_Reorder_lsf(lsf2_q, LSF_GAP, M);
/* Update LSF history with quantized LSFs
when hangover period is active */
if ((w_txdtx_ctrl & TX_HANGOVER_ACTIVE) != 0) {
w_update_lsf_history(lsf1_q, lsf2_q, w_lsf_old_tx);
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf1_q, lsp1_q, M);
w_Lsf_lsp(lsf2_q, lsp2_q, M);
} else {
for (i = 0; i < M; i++) {
w_past_r2_q[i] = 0;
}
}
return;
}
int16_t Vq_w_subvec_s(int16_t * lsf_r1, /* input : 1st LSF residual vector */
int16_t * lsf_r2, /* input : and LSF residual vector */
const int16_t * dico, /* input : quantization codebook */
int16_t * wf1, /* input : 1st LSF weighting factors */
int16_t * wf2, /* input : 2nd LSF weighting factors */
int16_t dico_size)
{ /* input : size of quantization codebook */
int16_t i, index = 0, sign = 0, temp = 0;
const int16_t *p_dico;
int32_t dist_min, dist;
dist_min = MAX_32;
p_dico = dico;
for (i = 0; i < dico_size; i++) {
/* w_test positive */
temp = w_sub(lsf_r1[0], *p_dico++);
temp = w_mult(wf1[0], temp);
dist = w_L_w_mult(temp, temp);
temp = w_sub(lsf_r1[1], *p_dico++);
temp = w_mult(wf1[1], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[0], *p_dico++);
temp = w_mult(wf2[0], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[1], *p_dico++);
temp = w_mult(wf2[1], temp);
dist = w_L_mac(dist, temp, temp);
if (w_L_w_sub(dist, dist_min) < (int32_t) 0) {
dist_min = dist;
index = i;
sign = 0;
}
/* w_test negative */
p_dico -= 4;
temp = w_add(lsf_r1[0], *p_dico++);
temp = w_mult(wf1[0], temp);
dist = w_L_w_mult(temp, temp);
temp = w_add(lsf_r1[1], *p_dico++);
temp = w_mult(wf1[1], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_add(lsf_r2[0], *p_dico++);
temp = w_mult(wf2[0], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_add(lsf_r2[1], *p_dico++);
temp = w_mult(wf2[1], temp);
dist = w_L_mac(dist, temp, temp);
if (w_L_w_sub(dist, dist_min) < (int32_t) 0) {
dist_min = dist;
index = i;
sign = 1;
}
}
/* Reading the selected vector */
p_dico = &dico[w_shl(index, 2)];
if (sign == 0) {
lsf_r1[0] = *p_dico++;
lsf_r1[1] = *p_dico++;
lsf_r2[0] = *p_dico++;
lsf_r2[1] = *p_dico++;
} else {
lsf_r1[0] = w_negate(*p_dico++);
lsf_r1[1] = w_negate(*p_dico++);
lsf_r2[0] = w_negate(*p_dico++);
lsf_r2[1] = w_negate(*p_dico++);
}
index = w_shl(index, 1);
index = w_add(index, sign);
return index;
}
void w_D_plsf_5(int16_t * indice, /* input : quantization indices of 5 w_submatrices */
int16_t * lsp1_q, /* output: quantized 1st LSP vector */
int16_t * lsp2_q, /* output: quantized 2nd LSP vector */
int16_t bfi, /* input : bad frame indicator (set to 1 if a bad
frame is received) */
int16_t w_rxdtx_ctrl, /* input : RX DTX control word */
int16_t w_w_rx_dtx_w_state /* input : w_state of the comfort noise insertion
period */
)
{
int16_t i;
const int16_t *p_dico;
int16_t temp, sign;
int16_t lsf1_r[M], lsf2_r[M];
int16_t lsf1_q[M], lsf2_q[M];
/* Update comfort noise LSF quantizer memory */
if ((w_rxdtx_ctrl & RX_UPD_SID_QUANT_MEM) != 0) {
update_w_lsf_p_CN(w_lsf_old_rx, w_lsf_p_CN);
}
/* Handle cases of comfort noise LSF decoding in which past
valid SID frames are repeated */
if (((w_rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
|| ((w_rxdtx_ctrl & RX_INVALID_SID_FRAME) != 0)
|| ((w_rxdtx_ctrl & RX_LOST_SID_FRAME) != 0)) {
if ((w_rxdtx_ctrl & RX_NO_TRANSMISSION) != 0) {
/* DTX active: no transmission. Interpolate LSF values in memory */
w_interpolate_CN_lsf(w_lsf_old_CN, w_lsf_new_CN, lsf2_q,
w_w_rx_dtx_w_state);
} else { /* Invalid or lost SID frame: use LSFs
from last good SID frame */
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = w_lsf_new_CN[i];
lsf2_q[i] = w_lsf_new_CN[i];
v_past_r2_q[i] = 0;
}
}
for (i = 0; i < M; i++) {
w_past_lsf_q[i] = lsf2_q[i];
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf2_q, lsp2_q, M);
return;
}
if (bfi != 0) { /* if bad frame */
/* use the past LSFs slightly shifted towards their mean */
for (i = 0; i < M; i++) {
/* lsfi_q[i] = ALPHA*w_past_lsf_q[i] + ONE_ALPHA*w_mean_lsf[i]; */
lsf1_q[i] = w_add(w_mult(w_past_lsf_q[i], ALPHA),
w_mult(w_mean_lsf[i], ONE_ALPHA));
lsf2_q[i] = lsf1_q[i];
}
/* estimate past quantized residual to be used in next frame */
for (i = 0; i < M; i++) {
/* temp = w_mean_lsf[i] + v_past_r2_q[i] * PRED_FAC; */
temp =
w_add(w_mean_lsf[i],
w_mult(v_past_r2_q[i], PRED_FAC));
v_past_r2_q[i] = w_sub(lsf2_q[i], temp);
}
} else
/* if good LSFs received */
{
/* decode w_prediction residuals from 5 received indices */
p_dico = &w_dico1_lsf[w_shl(indice[0], 2)];
lsf1_r[0] = *p_dico++;
lsf1_r[1] = *p_dico++;
lsf2_r[0] = *p_dico++;
lsf2_r[1] = *p_dico;
p_dico = &w_dico2_lsf[w_shl(indice[1], 2)];
lsf1_r[2] = *p_dico++;
lsf1_r[3] = *p_dico++;
lsf2_r[2] = *p_dico++;
lsf2_r[3] = *p_dico;
sign = indice[2] & 1;
i = w_shr(indice[2], 1);
p_dico = &w_dico3_lsf[w_shl(i, 2)];
if (sign == 0) {
lsf1_r[4] = *p_dico++;
lsf1_r[5] = *p_dico++;
lsf2_r[4] = *p_dico++;
lsf2_r[5] = *p_dico++;
} else {
lsf1_r[4] = w_negate(*p_dico++);
lsf1_r[5] = w_negate(*p_dico++);
lsf2_r[4] = w_negate(*p_dico++);
lsf2_r[5] = w_negate(*p_dico++);
}
p_dico = &w_dico4_lsf[w_shl(indice[3], 2)];
lsf1_r[6] = *p_dico++;
lsf1_r[7] = *p_dico++;
lsf2_r[6] = *p_dico++;
lsf2_r[7] = *p_dico;
p_dico = &w_dico5_lsf[w_shl(indice[4], 2)];
lsf1_r[8] = *p_dico++;
lsf1_r[9] = *p_dico++;
lsf2_r[8] = *p_dico++;
lsf2_r[9] = *p_dico++;
/* Compute quantized LSFs and update the past quantized residual */
/* Use w_lsf_p_CN as w_predicted LSF vector in case of no w_speech
activity */
if ((w_rxdtx_ctrl & RX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
temp =
w_add(w_mean_lsf[i],
w_mult(v_past_r2_q[i], PRED_FAC));
lsf1_q[i] = w_add(lsf1_r[i], temp);
lsf2_q[i] = w_add(lsf2_r[i], temp);
v_past_r2_q[i] = lsf2_r[i];
}
} else { /* Valid SID frame */
for (i = 0; i < M; i++) {
lsf2_q[i] = w_add(lsf2_r[i], w_lsf_p_CN[i]);
/* Use the dequantized values of lsf2 also for lsf1 */
lsf1_q[i] = lsf2_q[i];
v_past_r2_q[i] = 0;
}
}
}
/* verification that LSFs have minimum distance of LSF_GAP Hz */
w_Reorder_lsf(lsf1_q, LSF_GAP, M);
w_Reorder_lsf(lsf2_q, LSF_GAP, M);
if ((w_rxdtx_ctrl & RX_FIRST_SID_UPDATE) != 0) {
for (i = 0; i < M; i++) {
w_lsf_new_CN[i] = lsf2_q[i];
}
}
if ((w_rxdtx_ctrl & RX_CONT_SID_UPDATE) != 0) {
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = w_lsf_new_CN[i];
w_lsf_new_CN[i] = lsf2_q[i];
}
}
if ((w_rxdtx_ctrl & RX_SP_FLAG) != 0) {
/* Update lsf history with quantized LSFs
when w_speech activity is present. If the current frame is
a bad one, update with most recent good comfort noise LSFs */
if (bfi == 0) {
w_update_lsf_history(lsf1_q, lsf2_q, w_lsf_old_rx);
} else {
w_update_lsf_history(w_lsf_new_CN, w_lsf_new_CN,
w_lsf_old_rx);
}
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = lsf2_q[i];
}
} else {
w_interpolate_CN_lsf(w_lsf_old_CN, w_lsf_new_CN, lsf2_q,
w_w_rx_dtx_w_state);
}
for (i = 0; i < M; i++) {
w_past_lsf_q[i] = lsf2_q[i];
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf1_q, lsp1_q, M);
w_Lsf_lsp(lsf2_q, lsp2_q, M);
return;
}
int16_t w_Autocorr(int16_t x[], /* (i) : Input signal */
int16_t m, /* (i) : LPC order */
int16_t r_h[], /* (o) : w_Autocorrelations (msb) */
int16_t r_l[], /* (o) : w_Autocorrelations (lsb) */
int16_t wind[] /* (i) : window for LPC analysis */
)
{
int16_t i, j, norm;
int16_t y[L_WINDOW];
int32_t sum;
int16_t overfl, overfl_shft;
/* Windowing of signal */
for (i = 0; i < L_WINDOW; i++) {
y[i] = w_w_mult_r(x[i], wind[i]);
}
/* Compute r[0] and w_test for overflow */
overfl_shft = 0;
do {
overfl = 0;
sum = 0L;
for (i = 0; i < L_WINDOW; i++) {
sum = w_L_mac(sum, y[i], y[i]);
}
/* If overflow divide y[] by 4 */
if (w_L_w_sub(sum, MAX_32) == 0L) {
overfl_shft = w_add(overfl_shft, 4);
overfl = 1; /* Set the overflow flag */
for (i = 0; i < L_WINDOW; i++) {
y[i] = w_shr(y[i], 2);
}
}
}
while (overfl != 0);
sum = L_w_add(sum, 1L); /* Avoid the case of all w_zeros */
/* Normalization of r[0] */
norm = w_norm_l(sum);
sum = w_L_w_shl(sum, norm);
w_L_Extract(sum, &r_h[0], &r_l[0]); /* Put in DPF format (see oper_32b) */
/* r[1] to r[m] */
for (i = 1; i <= m; i++) {
sum = 0;
for (j = 0; j < L_WINDOW - i; j++) {
sum = w_L_mac(sum, y[j], y[j + i]);
}
sum = w_L_w_shl(sum, norm);
w_L_Extract(sum, &r_h[i], &r_l[i]);
}
norm = w_sub(norm, overfl_shft);
return norm;
}
void w_build_code(int16_t codvec[], /* (i) : position of pulses */
int16_t sign[], /* (i) : sign of d[n] */
int16_t cod[], /* (o) : innovative code vector */
int16_t h[], /* (i) : impulse response of weighted w_w_synthesis filter */
int16_t y[], /* (o) : filtered innovative code */
int16_t indx[] /* (o) : index of 10 pulses (sign+position) */
)
{
int16_t i, j, k, track, index, _sign[NB_PULSE];
int16_t *p0, *p1, *p2, *p3, *p4, *p5, *p6, *p7, *p8, *p9;
int32_t s;
for (i = 0; i < L_CODE; i++) {
cod[i] = 0;
}
for (i = 0; i < NB_TRACK; i++) {
indx[i] = -1;
}
for (k = 0; k < NB_PULSE; k++) {
/* read pulse position */
i = codvec[k];
/* read sign */
j = sign[i];
index = w_mult(i, 6554); /* index = pos/5 */
/* track = pos%5 */
track =
w_sub(i, w_extract_l(w_L_w_shr(w_L_w_mult(index, 5), 1)));
if (j > 0) {
cod[i] = w_add(cod[i], 4096);
_sign[k] = 8192;
} else {
cod[i] = w_sub(cod[i], 4096);
_sign[k] = -8192;
index = w_add(index, 8);
}
if (indx[track] < 0) {
indx[track] = index;
} else {
if (((index ^ indx[track]) & 8) == 0) {
/* sign of 1st pulse == sign of 2nd pulse */
if (w_sub(indx[track], index) <= 0) {
indx[track + 5] = index;
} else {
indx[track + 5] = indx[track];
indx[track] = index;
}
} else {
/* sign of 1st pulse != sign of 2nd pulse */
if (w_sub((indx[track] & 7), (index & 7)) <= 0) {
indx[track + 5] = indx[track];
indx[track] = index;
} else {
indx[track + 5] = index;
}
}
}
}
p0 = h - codvec[0];
p1 = h - codvec[1];
p2 = h - codvec[2];
p3 = h - codvec[3];
p4 = h - codvec[4];
p5 = h - codvec[5];
p6 = h - codvec[6];
p7 = h - codvec[7];
p8 = h - codvec[8];
p9 = h - codvec[9];
for (i = 0; i < L_CODE; i++) {
s = 0;
s = w_L_mac(s, *p0++, _sign[0]);
s = w_L_mac(s, *p1++, _sign[1]);
s = w_L_mac(s, *p2++, _sign[2]);
s = w_L_mac(s, *p3++, _sign[3]);
s = w_L_mac(s, *p4++, _sign[4]);
s = w_L_mac(s, *p5++, _sign[5]);
s = w_L_mac(s, *p6++, _sign[6]);
s = w_L_mac(s, *p7++, _sign[7]);
s = w_L_mac(s, *p8++, _sign[8]);
s = w_L_mac(s, *p9++, _sign[9]);
y[i] = w_round(s);
}
}
void w_search_10i40(int16_t dn[], /* (i) : correlation between target and h[] */
int16_t rr[][L_CODE], /* (i) : matrix of autocorrelation */
int16_t ipos[], /* (i) : starting position for each pulse */
int16_t pos_max[], /* (i) : position of maximum of dn[] */
int16_t codvec[] /* (o) : algebraic codebook vector */
)
{
int16_t i0, i1, i2, i3, i4, i5, i6, i7, i8, i9;
int16_t i, j, k, pos, ia, ib;
int16_t psk, ps, ps0, ps1, ps2, sq, sq2;
int16_t alpk, alp, alp_16;
int16_t rrv[L_CODE];
int32_t s, alp0, alp1, alp2;
/* fix i0 on maximum of correlation position */
i0 = pos_max[ipos[0]];
/*------------------------------------------------------------------*
* i1 loop: *
*------------------------------------------------------------------*/
/* Default value */
psk = -1;
alpk = 1;
for (i = 0; i < NB_PULSE; i++) {
codvec[i] = i;
}
for (i = 1; i < NB_TRACK; i++) {
i1 = pos_max[ipos[1]];
ps0 = w_add(dn[i0], dn[i1]);
alp0 = w_L_w_mult(rr[i0][i0], _1_16);
alp0 = w_L_mac(alp0, rr[i1][i1], _1_16);
alp0 = w_L_mac(alp0, rr[i0][i1], _1_8);
/*----------------------------------------------------------------*
* i2 and i3 loop: *
*----------------------------------------------------------------*/
/* initialize 4 indices for next loop. */
/* initialize "rr[i3][i3]" pointer */
/* initialize "rr[i0][i3]" pointer */
/* initialize "rr[i1][i3]" pointer */
/* initialize "rrv[i3]" pointer */
for (i3 = ipos[3]; i3 < L_CODE; i3 += STEP) {
s = w_L_w_mult(rr[i3][i3], _1_8); /* index incr= STEP+L_CODE */
s = w_L_mac(s, rr[i0][i3], _1_4); /* index increment = STEP */
s = w_L_mac(s, rr[i1][i3], _1_4); /* index increment = STEP */
rrv[i3] = w_round(s);
}
/* Default value */
sq = -1;
alp = 1;
ps = 0;
ia = ipos[2];
ib = ipos[3];
/* initialize 4 indices for i2 loop. */
/* initialize "dn[i2]" pointer */
/* initialize "rr[i2][i2]" pointer */
/* initialize "rr[i0][i2]" pointer */
/* initialize "rr[i1][i2]" pointer */
for (i2 = ipos[2]; i2 < L_CODE; i2 += STEP) {
/* index increment = STEP */
ps1 = w_add(ps0, dn[i2]);
/* index incr= STEP+L_CODE */
alp1 = w_L_mac(alp0, rr[i2][i2], _1_16);
/* index increment = STEP */
alp1 = w_L_mac(alp1, rr[i0][i2], _1_8);
/* index increment = STEP */
alp1 = w_L_mac(alp1, rr[i1][i2], _1_8);
/* initialize 3 indices for i3 inner loop */
/* initialize "dn[i3]" pointer */
/* initialize "rrv[i3]" pointer */
/* initialize "rr[i2][i3]" pointer */
for (i3 = ipos[3]; i3 < L_CODE; i3 += STEP) {
/* index increment = STEP */
ps2 = w_add(ps1, dn[i3]);
/* index increment = STEP */
alp2 = w_L_mac(alp1, rrv[i3], _1_2);
/* index increment = STEP */
alp2 = w_L_mac(alp2, rr[i2][i3], _1_8);
sq2 = w_mult(ps2, ps2);
alp_16 = w_round(alp2);
s = w_L_msu(w_L_w_mult(alp, sq2), sq, alp_16);
if (s > 0) {
sq = sq2;
ps = ps2;
alp = alp_16;
ia = i2;
ib = i3;
}
}
}
i2 = ia;
i3 = ib;
/*----------------------------------------------------------------*
* i4 and i5 loop: *
*----------------------------------------------------------------*/
ps0 = ps;
alp0 = w_L_w_mult(alp, _1_2);
/* initialize 6 indices for next loop (see i2-i3 loop) */
for (i5 = ipos[5]; i5 < L_CODE; i5 += STEP) {
s = w_L_w_mult(rr[i5][i5], _1_8);
s = w_L_mac(s, rr[i0][i5], _1_4);
s = w_L_mac(s, rr[i1][i5], _1_4);
s = w_L_mac(s, rr[i2][i5], _1_4);
s = w_L_mac(s, rr[i3][i5], _1_4);
rrv[i5] = w_round(s);
}
/* Default value */
sq = -1;
alp = 1;
ps = 0;
ia = ipos[4];
ib = ipos[5];
/* initialize 6 indices for i4 loop (see i2-i3 loop) */
for (i4 = ipos[4]; i4 < L_CODE; i4 += STEP) {
ps1 = w_add(ps0, dn[i4]);
alp1 = w_L_mac(alp0, rr[i4][i4], _1_32);
alp1 = w_L_mac(alp1, rr[i0][i4], _1_16);
alp1 = w_L_mac(alp1, rr[i1][i4], _1_16);
alp1 = w_L_mac(alp1, rr[i2][i4], _1_16);
alp1 = w_L_mac(alp1, rr[i3][i4], _1_16);
/* initialize 3 indices for i5 inner loop (see i2-i3 loop) */
for (i5 = ipos[5]; i5 < L_CODE; i5 += STEP) {
ps2 = w_add(ps1, dn[i5]);
alp2 = w_L_mac(alp1, rrv[i5], _1_4);
alp2 = w_L_mac(alp2, rr[i4][i5], _1_16);
sq2 = w_mult(ps2, ps2);
alp_16 = w_round(alp2);
s = w_L_msu(w_L_w_mult(alp, sq2), sq, alp_16);
if (s > 0) {
sq = sq2;
ps = ps2;
alp = alp_16;
ia = i4;
ib = i5;
}
}
}
i4 = ia;
i5 = ib;
/*----------------------------------------------------------------*
* i6 and i7 loop: *
*----------------------------------------------------------------*/
ps0 = ps;
alp0 = w_L_w_mult(alp, _1_2);
/* initialize 8 indices for next loop (see i2-i3 loop) */
for (i7 = ipos[7]; i7 < L_CODE; i7 += STEP) {
s = w_L_w_mult(rr[i7][i7], _1_16);
s = w_L_mac(s, rr[i0][i7], _1_8);
s = w_L_mac(s, rr[i1][i7], _1_8);
s = w_L_mac(s, rr[i2][i7], _1_8);
s = w_L_mac(s, rr[i3][i7], _1_8);
s = w_L_mac(s, rr[i4][i7], _1_8);
s = w_L_mac(s, rr[i5][i7], _1_8);
rrv[i7] = w_round(s);
}
/* Default value */
sq = -1;
alp = 1;
ps = 0;
ia = ipos[6];
ib = ipos[7];
/* initialize 8 indices for i6 loop (see i2-i3 loop) */
for (i6 = ipos[6]; i6 < L_CODE; i6 += STEP) {
ps1 = w_add(ps0, dn[i6]);
alp1 = w_L_mac(alp0, rr[i6][i6], _1_64);
alp1 = w_L_mac(alp1, rr[i0][i6], _1_32);
alp1 = w_L_mac(alp1, rr[i1][i6], _1_32);
alp1 = w_L_mac(alp1, rr[i2][i6], _1_32);
alp1 = w_L_mac(alp1, rr[i3][i6], _1_32);
alp1 = w_L_mac(alp1, rr[i4][i6], _1_32);
alp1 = w_L_mac(alp1, rr[i5][i6], _1_32);
/* initialize 3 indices for i7 inner loop (see i2-i3 loop) */
for (i7 = ipos[7]; i7 < L_CODE; i7 += STEP) {
ps2 = w_add(ps1, dn[i7]);
alp2 = w_L_mac(alp1, rrv[i7], _1_4);
alp2 = w_L_mac(alp2, rr[i6][i7], _1_32);
sq2 = w_mult(ps2, ps2);
alp_16 = w_round(alp2);
s = w_L_msu(w_L_w_mult(alp, sq2), sq, alp_16);
if (s > 0) {
sq = sq2;
ps = ps2;
alp = alp_16;
ia = i6;
ib = i7;
}
}
}
i6 = ia;
i7 = ib;
/*----------------------------------------------------------------*
* i8 and i9 loop: *
*----------------------------------------------------------------*/
ps0 = ps;
alp0 = w_L_w_mult(alp, _1_2);
/* initialize 10 indices for next loop (see i2-i3 loop) */
for (i9 = ipos[9]; i9 < L_CODE; i9 += STEP) {
s = w_L_w_mult(rr[i9][i9], _1_16);
s = w_L_mac(s, rr[i0][i9], _1_8);
s = w_L_mac(s, rr[i1][i9], _1_8);
s = w_L_mac(s, rr[i2][i9], _1_8);
s = w_L_mac(s, rr[i3][i9], _1_8);
s = w_L_mac(s, rr[i4][i9], _1_8);
s = w_L_mac(s, rr[i5][i9], _1_8);
s = w_L_mac(s, rr[i6][i9], _1_8);
s = w_L_mac(s, rr[i7][i9], _1_8);
rrv[i9] = w_round(s);
}
/* Default value */
sq = -1;
alp = 1;
ia = ipos[8];
ib = ipos[9];
/* initialize 10 indices for i8 loop (see i2-i3 loop) */
for (i8 = ipos[8]; i8 < L_CODE; i8 += STEP) {
ps1 = w_add(ps0, dn[i8]);
alp1 = w_L_mac(alp0, rr[i8][i8], _1_128);
alp1 = w_L_mac(alp1, rr[i0][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i1][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i2][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i3][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i4][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i5][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i6][i8], _1_64);
alp1 = w_L_mac(alp1, rr[i7][i8], _1_64);
/* initialize 3 indices for i9 inner loop (see i2-i3 loop) */
for (i9 = ipos[9]; i9 < L_CODE; i9 += STEP) {
ps2 = w_add(ps1, dn[i9]);
alp2 = w_L_mac(alp1, rrv[i9], _1_8);
alp2 = w_L_mac(alp2, rr[i8][i9], _1_64);
sq2 = w_mult(ps2, ps2);
alp_16 = w_round(alp2);
s = w_L_msu(w_L_w_mult(alp, sq2), sq, alp_16);
if (s > 0) {
sq = sq2;
alp = alp_16;
ia = i8;
ib = i9;
}
}
}
/*----------------------------------------------------------------*
* memorise codevector if this one is better than the last one. *
*----------------------------------------------------------------*/
s = w_L_msu(w_L_w_mult(alpk, sq), psk, alp);
if (s > 0) {
psk = sq;
alpk = alp;
codvec[0] = i0;
codvec[1] = i1;
codvec[2] = i2;
codvec[3] = i3;
codvec[4] = i4;
codvec[5] = i5;
codvec[6] = i6;
codvec[7] = i7;
codvec[8] = ia;
codvec[9] = ib;
}
/*----------------------------------------------------------------*
* Cyclic permutation of i1,i2,i3,i4,i5,i6,i7,i8 and i9. *
*----------------------------------------------------------------*/
pos = ipos[1];
for (j = 1, k = 2; k < NB_PULSE; j++, k++) {
ipos[j] = ipos[k];
}
ipos[NB_PULSE - 1] = pos;
}
}
void w_set_sign(int16_t dn[], /* (i/o): correlation between target and h[] */
int16_t cn[], /* (i) : residual after long term w_prediction */
int16_t sign[], /* (o) : sign of d[n] */
int16_t pos_max[], /* (o) : position of maximum correlation */
int16_t ipos[] /* (o) : starting position for each pulse */
)
{
int16_t i, j;
int16_t val, cor, k_cn, k_dn, max, max_of_all, pos = 0;
int16_t en[L_CODE]; /* correlation vector */
int32_t s;
/* calculate energy for normalization of cn[] and dn[] */
s = 256;
for (i = 0; i < L_CODE; i++) {
s = w_L_mac(s, cn[i], cn[i]);
}
s = w_Inv_sqrt(s);
k_cn = w_extract_h(w_L_w_shl(s, 5));
s = 256;
for (i = 0; i < L_CODE; i++) {
s = w_L_mac(s, dn[i], dn[i]);
}
s = w_Inv_sqrt(s);
k_dn = w_extract_h(w_L_w_shl(s, 5));
for (i = 0; i < L_CODE; i++) {
val = dn[i];
cor =
w_round(w_L_w_shl
(w_L_mac(w_L_w_mult(k_cn, cn[i]), k_dn, val), 10));
if (cor >= 0) {
sign[i] = 32767; /* sign = +1 */
} else {
sign[i] = -32767; /* sign = -1 */
cor = w_negate(cor);
val = w_negate(val);
}
/* modify dn[] according to the fixed sign */
dn[i] = val;
en[i] = cor;
}
max_of_all = -1;
for (i = 0; i < NB_TRACK; i++) {
max = -1;
for (j = i; j < L_CODE; j += STEP) {
cor = en[j];
val = w_sub(cor, max);
if (val > 0) {
max = cor;
pos = j;
}
}
/* store maximum correlation position */
pos_max[i] = pos;
val = w_sub(max, max_of_all);
if (val > 0) {
max_of_all = max;
/* starting position for i0 */
ipos[0] = i;
}
}
/*----------------------------------------------------------------*
* Set starting position of each pulse. *
*----------------------------------------------------------------*/
pos = ipos[0];
ipos[5] = pos;
for (i = 1; i < NB_TRACK; i++) {
pos = w_add(pos, 1);
if (w_sub(pos, NB_TRACK) >= 0) {
pos = 0;
}
ipos[i] = pos;
ipos[i + 5] = pos;
}
}
/** Loads a weight list specified in powerup.xml. The different position
* classes must be loaded in the right order
* \param root The root node of powerup.xml
* \param class_name The name of the position class to load.
* \param position_class The class for which the weights are read.
*/
void PowerupManager::loadWeights(const XMLNode &root,
unsigned int num_karts,
const std::string &class_name,
PositionClass position_class)
{
const XMLNode *node = root.getNode(class_name), *node2;
std::string s(""), s_multi(""), k;
std::string w("w"), w_multi("w-multi"), w_add("");
/** Adds to w the suffixe of the num_karts_class
* associated with num_karts
*/
if(position_class!=POSITION_BATTLE_MODE
&& position_class!=POSITION_TUTORIAL_MODE)
{
node2 = root.getNode("range_kart");
k = StringUtils::toString(num_karts);
k = "k" + k;//Xml nodes can't start with a number
//k now contains XML attributes containing kart number class
if(node2) node2->get(k, &w_add);
if(!node2)
{
Log::error("[PowerupManager]","No class of weights found"
"for %d karts - probabilities will be incorrect",
num_karts);
}
if(race_manager->isFollowMode())
{
w_add = "f";
}
if(w_add=="")
{
w_add = "d";//fallback values
Log::warn("[PowerupManager]","powerup.xml do not support"
"%d karts - fallback probabilities will be used",
num_karts);
}
w = w + w_add;
w_multi = w_multi + w_add;
}//w is changed to associate with an arbitrary class of kart numbers
if(node) node->get(w, &s );
if(node) node->get(w_multi, &s_multi);
if(!node || s=="" || s_multi=="")
{
Log::error("[PowerupManager]", "No weights found for class '%s'"
" - probabilities will be incorrect.",
class_name.c_str());
return;
}
std::vector<std::string> weight_list = StringUtils::split(s+" "+s_multi,' ');
std::vector<std::string>::iterator i=weight_list.begin();
while(i!=weight_list.end())
{
if(*i=="")
{
std::vector<std::string>::iterator next=weight_list.erase(i);
i=next;
}
else
i++;
}
// Fill missing entries with 0s
while(weight_list.size()<2*(int)POWERUP_LAST)
weight_list.push_back(0);
if(weight_list.size()!=2*(int)POWERUP_LAST)
{
Log::error("[PowerupManager]", "Incorrect number of weights found in class '%s':",
class_name.c_str());
Log::error("[PowerupManager]", "%d instead of %d - probabilities will be incorrect.",
(int)weight_list.size(), (int)POWERUP_LAST);
return;
}
for(unsigned int i=0; i<weight_list.size(); i++)
{
int w = atoi(weight_list[i].c_str());
m_weights[position_class].push_back(w);
}
} // loadWeights