vq_lib.c

MELPe 1200 bps, fixed point

				/* L_temp is Q31, p_distortion is same Q as n_d, p_e is Q15 */
				L_temp = L_deposit_h(add(*p_distortion++, uhatw_sq));
				for (i = 0; i < order; i++)
					L_temp = L_mac(L_temp, *p_e++, uhatw[i]);

				/* d_cj is Q15 */
				d_cj = extract_h(L_temp);

				/* determine if d is less than the maximum candidate          */
				/* distortion.  i.e., is the distortion found better than the */
				/* so-called worst of the best */
				if (d_cj <= n_d[p_max]){
					/* replace the worst with the values just found */
					/* n_d is now a Q16 */
					n_d[p_max] = d_cj;

					i = add(shr(extract_l(L_mult(p_max, stages)), 1), s);
					n_indices[i] = j;
					n_parents[p_max] = c;

					/* want to limit the number of times the inner loop is    */
					/* entered (to reduce the *maximum* complexity) */
					if (inner_counter < max_inner){
						inner_counter = add(inner_counter, 1);
						if (inner_counter < max_inner){
							p_max = 0;
							/* find the new maximum */
							for (i = 1; i < ma; i++){
								if (n_d[i] > n_d[p_max])
									p_max = i;
							}
						} else {                /* inner_counter == max_inner */
							/* The inner loop counter now exceeds the         */
							/* maximum, and the inner loop will now not be    */
							/* entered.  Instead of quitting the search or    */
							/* doing something drastic, we simply keep track  */
							/* of the best candidate (rather than the M best) */
							/* by setting p_max to equal the index of the     */
							/* minimum distortion i.e. only keep one          */
							/* candidate around the MINIMUM distortion */
							for (i = 1; i < ma; i++){
								if (n_d[i] < n_d[p_max])
									p_max = i;
							}
						}
					}
				}
			} /* for c */
		} /* for j */

		/* compute the error vectors for each node */
		for (c = 0; c < ma; c++){
			/* get the error from the parent node and subtract off the        */
			/* codebook value */
			(void) v_equ(&n_errors[c*order], &p_errors[n_parents[c]*order],
						 order);
			(void) v_sub(&n_errors[c*order],
						 &cb_currentstage[n_indices[c*stages + s]*order],
						 order);
			/* get the indices that were used for the parent node */
			(void) v_equ(&n_indices[c*stages],
						 &p_indices[n_parents[c]*stages], s);
		}

		m = (Shortword) (m*levels[s]);
		if (m > ma)
			m = ma;
	} /* for s */

	/* find the optimum candidate c */
	for (i = 1, c = 0; i < ma; i++){
		if (n_d[i] < n_d[c])
			c = i;
	}

	d_opt = n_d[c];

	if (a_indices){
		(void) v_equ(a_indices, &n_indices[c*stages], stages);
	}
	if (u_hat){
		if (u_est)
			(void) v_equ(u_hat, u_est, order);
		else
			(void) v_zap(u_hat, order);

		cb_currentstage = cb;
		for (s = 0; s < stages; s++){
			cb_table = &cb_currentstage[n_indices[c*stages + s]*order];
			for (i = 0; i < order; i++)
				u_hat[i] = add(u_hat[i], shr(cb_table[i], 2));
			cb_currentstage += levels[s]*order;
		}
	}

	v_free(tmp_p_e);
	v_free(parents);
	v_free(d);
	v_free(uhatw);
	v_free(errors);
	v_free(indices);

	return(d_opt);
}


/*  VQ_MSD2 -                                                                 */
/*      Tree search multi-stage VQ decoder                                    */
/*                                                                            */
/*  Synopsis: vq_msd(cb, u, u_est, a, indices, levels, stages, p, conversion) */
/*      Input:                                                                */
/*          cb- one dimensional linear codebook array (codebook is structured */
/*              as [stages][levels for each stage][p])                        */
/*          indices- the codebook indices (for each stage)                    */
/*                   indices[0..stages-1]                                     */
/*          levels- the number of levels (for each stage) levels[0..stages-1] */
/*          u_est- dimension p, the estimated parameters or mean (if NULL,    */
/*                 assume estimate is the all zero vector) (u_est[0..p-1])    */
/*          stages- the number of stages of msvq                              */
/*          p- the predictor order                                            */
/*          conversion- the conversion constant (see lpc.h, lpc_conv.c)       */
/*          diff_Q- the difference between Q value of codebook and u_est      */
/*      Output:                                                               */
/*          u- dimension p, the decoded parameters (if NULL, use alternate    */
/*             temporary storage) (u[0..p-1])                                 */
/*          a- predictor parameters (a[0..p]), if NULL, don't compute         */
/*      Returns:                                                              */
/*          pointer to reconstruction vector (u)                              */
/*      Parameters:                                                           */
/*                                                                            */
/*      Note:                                                                 */
/*          The coder does not use the returned value from vq_lspw() at all.  */

void vq_msd2(const Shortword *cb, Shortword *u_hat, const Shortword *u_est,
			 Shortword *indices, const Shortword levels[], Shortword stages,
			 Shortword p, Shortword diff_Q)
{
	register Shortword	i, j;
	const Shortword		*cb_currentstage, *cb_table;
	Longword	*L_u_hat, L_temp;


	/* allocate memory (if required) */
	L_u_hat = L_v_get(p);

	/* add estimate on (if non-null), or clear vector */
	if (u_est)
		(void) v_equ(u_hat, u_est, p);
	else
		(void) v_zap(u_hat, p);

	/* put u_hat to a long buffer */
	for (i = 0; i < p; i++)
		L_u_hat[i] = L_shl(L_deposit_l(u_hat[i]), diff_Q);

	/* add the contribution of each stage */
	cb_currentstage = cb;
	for (i = 0; i < stages; i++){
		/*	(void) v_add(u_hat, &cb_currentstage[indices[i]*p], p);           */
		cb_table = &cb_currentstage[indices[i]*p];
		for (j = 0; j < p; j++){
			L_temp = L_deposit_l(cb_table[j]);
			L_u_hat[j] = L_add(L_u_hat[j], L_temp);
		}
		cb_currentstage += levels[i]*p;
	}

	/* convert long buffer back to u_hat */
	for (i = 0; i < p; i++)
		u_hat[i] = extract_l(L_shr(L_u_hat[i], diff_Q));

	v_free(L_u_hat);
}


/* VQ_ENC -                                                                   */
/*  encode vector with full VQ using unweighted Euclidean distance            */
/*  Synopsis: vq_enc(codebook, u, levels, order, u_hat, indices)              */
/*      Input:                                                                */
/*          codebook- one dimensional linear codebook array                   */
/*          u- dimension order, the parameters to be encoded                  */
/*             (u[0 .. order-1])                                              */
/*          levels- the number of levels                                      */
/*          order- the predictor order                                        */
/*      Output:                                                               */
/*          u_hat-	 the reconstruction vector (if non null)                  */
/*          a_indices- the codebook indices (for each stage)                  */
/*                     a_indices[0..stages-1]                                 */
/*      Parameters:                                                           */
/*                                                                            */
/*      Q values:                                                             */
/*          codebook - Q13, u - Q13, u_hat - Q13                              */
/*                                                                            */
/*      Note:                                                                 */
/*          The coder does not use the returned value from vq_lspw() at all.  */

Longword vq_enc(const Shortword codebook[], Shortword u[], Shortword levels,
				Shortword order, Shortword u_hat[], Shortword *indices)
{
	register Shortword	i, j;
	const Shortword		*p_cb;
	Shortword	index;
	Longword	d, dmin;
	Shortword	temp;


	/* Search codebook for minimum distance */
	index = 0;
	dmin = LW_MAX;
	p_cb = codebook;
	for (i = 0; i < levels; i++){
		d = 0;
		for (j = 0; j < order; j++){
			temp = sub(u[j], *p_cb);
			d = L_mac(d, temp, temp);
			p_cb++;
		}
		if (d < dmin){
			dmin = d;
			index = i;
		}
	}

	/* Update index and quantized value, and return minimum distance */
	*indices = index;
	v_equ(u_hat, &codebook[order*index], order);

	return(dmin);
}


/* ========================================================================== */
/* vq_fsw() computes the weights for Euclidean distance of Fourier harmonics. */
/*    Q values:                                                               */
/*       w_fs - Q14, pitch - Q9                                               */
/* ========================================================================== */
void vq_fsw(Shortword w_fs[], Shortword num_harm, Shortword pitch)
{
	register Shortword	i;
	register Shortword	temp;
	Shortword	tempw0, denom;
	Longword	L_temp;

	/* Calculate fundamental frequency */
	/* w0 = TWOPI/pitch */
	/* tempw0 = w0/(0.25*PI) = 8/pitch */

	tempw0 = divide_s(EIGHT_Q11, pitch);                     /* tempw0 in Q17 */
	for (i = 0; i < num_harm; i++){

		/* Bark-scale weighting */
		/* w_fs[i] = 117.0/(25.0 + 75.0* pow(1.0 + */
		/*           1.4*SQR(w0*(i+1)/(0.25*PI)),0.69)) */

		temp = add(i, 1);
		temp = shl(temp, 11);                                   /* (i+1), Q11 */
		L_temp = L_mult(tempw0, temp);                                 /* Q29 */
		L_temp = L_shl(L_temp, 1);                                     /* Q30 */
		temp = extract_h(L_temp);                    /* w0*(i+1)/0.25*PI, Q14 */
		temp = mult(temp, temp);                               /* SQR(*), Q13 */
		/* Q28 for L_temp = 1.0 + 1.4*SQR(*) */
		L_temp = L_mult(X14_Q14, temp);                                /* Q28 */
		L_temp = L_add(ONE_Q28, L_temp);                               /* Q28 */
		temp = L_pow_fxp(L_temp, X069_Q15, 28, 13);                    /* Q13 */
		temp = mult(X75_Q8, temp);                                      /* Q6 */
		denom = add(X25_Q6, temp);                                      /* Q6 */
		w_fs[i] = divide_s(X117_Q5, denom);                            /* Q14 */
	}
}