/* ================================================================== */
/*                                                                    */ 
/*    Microsoft Speech coder     ANSI-C Source Code                   */
/*    SC1200 1200 bps speech coder                                    */
/*    Fixed Point Implementation      Version 7.0                     */
/*    Copyright (C) 2000, Microsoft Corp.                             */
/*    All rights reserved.                                            */
/*                                                                    */ 
/* ================================================================== */

/*------------------------------------------------------------------*/
/*																	*/
/* File:		qnt12.c 											*/
/*																	*/
/* Description: quantization for 1200bps							*/
/*																	*/
/*------------------------------------------------------------------*/

#include "sc1200.h"
#include "lpc_lib.h"
#include "vq_lib.h"
#include "global.h"
#include "macro.h"
#include "qnt12_cb.h"
#include "mat_lib.h"
#include "math_lib.h"
#include "qnt12.h"
#include "constant.h"
#include "mathhalf.h"
#include "msvq_cb.h"
#include "fsvq_cb.h"
#include "dsp_sub.h"
#include "melp_sub.h"

#define	LSP_INP_CAND		5
#define NF_X_NUM_GAINFR		(NF * NUM_GAINFR)

#define X0333_Q15			10923                        /* (1/3) * (1 << 15) */
#define X0667_Q15			21845                        /* (2/3) * (1 << 15) */

/* ------ Local prototypes ------ */
static void		wvq1(Shortword target[], Shortword weights[],
					 const Shortword codebook[], Shortword dim,
					 Shortword cbsize, Shortword index[], Longword dist[],
					 Shortword cand);
static Shortword	InsertCand(Shortword c1, Shortword s1, Shortword dMin[], 
							Shortword distortion, Shortword entry, 
							Shortword nextIndex[], Shortword index[]);
static Shortword	wvq2(Shortword target[], Shortword weights[],
						 Shortword codebook[], Shortword dim,
						 Shortword index[], Longword dist[], Shortword cand);
static Shortword	WeightedMSE(Shortword n, Shortword weight[], 
						const Shortword x[], Shortword target[],
						Shortword max_dMin);
static void		lspVQ(Shortword target[], Shortword weight[], Shortword qout[],
					  const Shortword codebook[], Shortword tos,
					  const Shortword cb_size[], Shortword cb_index[],
					  Shortword dim, BOOLEAN flag);


/****************************************************************************
**
** Function:		pitch_vq
**
** Description: 	Pitch values of three frames are vector quantized
**
** Arguments:
**
**	melp_param *par ---- input/output melp parameters
**
** Return value:	None
**
*****************************************************************************/
void pitch_vq(struct melp_param *par)
{
	register Shortword	i;
	static BOOLEAN	prev_uv_flag = TRUE;
	static Shortword	prev_pitch = LOG_UV_PITCH_Q12;                 /* Q12 */
	static Shortword	prev_qpitch = LOG_UV_PITCH_Q12;                /* Q12 */
	const Shortword		*codebook;
	Shortword	cnt, size, pitch_index;
	Shortword	temp1, temp2;
	Longword	L_temp;
	Shortword	dcb[PITCH_VQ_CAND * NF];                               /* Q12 */
	Shortword	target[NF], deltp[NF];                                 /* Q12 */
	Shortword	deltw[NF];                                              /* Q0 */
	Shortword	weights[NF];                                            /* Q0 */
	Shortword	indexlist[PITCH_VQ_CAND];
	Longword	distlist[PITCH_VQ_CAND];                               /* Q25 */


	/* ---- Compute pitch in log domain ---- */
	for (i = 0; i < NF; i++)
		target[i] = log10_fxp(par[i].pitch, 7);                        /* Q12 */

	cnt = 0;
	for (i = 0; i < NF; i++){
		if (par[i].uv_flag)
			weights[i] = 0;                                             /* Q0 */
		else {
			weights[i] = 1;
			cnt++;
		}
	}

	/* ---- calculate delta ---- */
	for (i = 0; i < NF; i++){
		if (prev_uv_flag || par[i].uv_flag){
			deltp[i] = 0;
			deltw[i] = 0;
		} else {
			deltp[i] = sub(target[i], prev_pitch);
			deltw[i] = DELTA_PITCH_WEIGHT_Q0;
		}
		prev_pitch = target[i];
		prev_uv_flag = par[i].uv_flag;
	}

	if (cnt == 0){

		for (i = 0; i < NF; i++)
			par[i].pitch = UV_PITCH;
		prev_qpitch = LOG_UV_PITCH_Q12;

	} else if (cnt == 1){

		for (i = 0; i < NF; i++){
			if (!par[i].uv_flag){
				quant_u(&target[i], &(quant_par.pitch_index), PIT_QLO_Q12,
						PIT_QUP_Q12, PIT_QLEV_M1, PIT_QLEV_M1_Q8, TRUE, 7);
				quant_u_dec(quant_par.pitch_index, &par[i].pitch,
							PIT_QLO_Q12, PIT_QUP_Q12, PIT_QLEV_M1_Q8, 7);
			} else
				par[i].pitch = LOG_UV_PITCH_Q12;
		}

		/* At this point par[].pitch temporarily holds the pitches in the     */
		/* log domain with Q12.                                               */

		prev_qpitch = par[NF - 1].pitch;                               /* Q12 */

		for (i = 0; i < NF; i++)
			par[i].pitch = pow10_fxp(par[i].pitch, 7);                  /* Q7 */

	} else if (cnt > 1){                     /* cnt == 2, 3, ......, (NF - 1) */
		/* ----- set pointer -----*/
		if (cnt == NF){                          /* All NF frames are voiced. */
			codebook = pitch_vq_cb_vvv;
			size = PITCH_VQ_LEVEL_VVV;
		} else {
			codebook = pitch_vq_cb_uvv;
			size = PITCH_VQ_LEVEL_UVV;
		} /* This part changed !!! (12/13/99) */

		/* ---- select candidate using static pitch distortion ---- */
		wvq1(target, weights, codebook, NF, size, indexlist, distlist,
			 PITCH_VQ_CAND);

		/* -- select index using static and delta pitch distortion -- */
		temp1 = 0;
		for (i = 0; i < PITCH_VQ_CAND; i++){
			L_temp = L_mult(indexlist[i], NF);
			L_temp = L_shr(L_temp, 1);
			temp2 = extract_l(L_temp);

			/* Now temp1 is (i*NF) and temp2 is (indexlist[i]*NF).            */

			dcb[temp1] = sub(codebook[temp2], prev_qpitch);            /* Q12 */
			v_equ(&dcb[temp1 + 1], &codebook[temp2 + 1], NF - 1);
			v_sub(&dcb[temp1 + 1], &codebook[temp2], NF - 1);
			temp1 = add(temp1, NF);
		}

		pitch_index = wvq2(deltp, deltw, dcb, NF, indexlist, distlist,
						   PITCH_VQ_CAND);

		if (par[NF - 1].uv_flag)
			prev_qpitch = LOG_UV_PITCH_Q12;
		else
			prev_qpitch = codebook[pitch_index*NF + NF - 1];           /* Q12 */

		for (i = 0; i < NF; i++){
			if (par[i].uv_flag)
				par[i].pitch = UV_PITCH_Q7;
			else
				par[i].pitch = pow10_fxp(codebook[pitch_index*NF + i], 7);
		}

		quant_par.pitch_index = pitch_index;
	}
}


/****************************************************************************
**
** Function:		wvq1
**
** Description: 	Pitch vq the first stage
**
**    The purpose of wvq1() is to loop through all the "cbsize" entries of
**    codebook[] (PITCH_VQ_LEVEL_VVV or PITCH_VQ_LEVEL_UVV, 512 or 2048) and
**    record the "cand" entries (PITCH_VQ_CAND, 16) which yields the minimum
**    errors.
**
** Arguments:
**	Shortword target[] 	: target vector (Q12)
**	Shortword weights[]	: weighting vector (Q0)
**	Shortword codebook[]: codebook (Q12)
**	Shortword dim 		: vector dimension
**	Shortword cbsize	: codebook size
**	Shortword index[]	: output candidate index list
**	Longword dist[]		: output candidate distortion list (Q25)
**	Shortword cand		: number of output candidates
**
** Return value:	None
**
*****************************************************************************/
static void		wvq1(Shortword target[], Shortword weights[],
					 const Shortword codebook[], Shortword dim,
					 Shortword cbsize, Shortword index[], Longword dist[],
					 Shortword cand)
{
	register Shortword	i, j;
	Shortword	maxindex;
	Longword	err, maxdist;                                          /* Q25 */
	Longword	L_temp;
	Shortword	temp;                                                  /* Q12 */


	/* ------ Initialize the distortion ------ */
	L_fill(dist, LW_MAX, cand);

	maxdist = LW_MAX;
	maxindex = 0;

	/* ------ Search the codebook ------ */
	for (i = 0; i < cbsize; i++){

		err = 0;

		/* Here the for loop computes the distortion between target[] and     */
		/* codebook[] and stores the result in Longword err.  If              */
		/* (err < maxdist) then we execute some actions.  If err is already   */
		/* larger than or equal to maxdist, there is no need to keep          */
		/* computing the distortion.  This improvement is only marginal       */
		/* because "dim" is small (NF == 3).                                  */

		for (j = 0; j < dim; j++){
			if (weights[j] > 0){               /* weights[] is either 1 or 0. */
				/*	err += SQR(target[j] - codebook[j]); */
				temp = sub(target[j], codebook[j]);                    /* Q12 */
				L_temp = L_mult(temp, temp);                           /* Q25 */
				L_temp = L_shr(L_temp, 2);                             /* Q23 */
				err = L_add(err, L_temp);
				if (err >= maxdist)
					break;
			}
		}
		if (err < maxdist){
			index[maxindex] = i;
			dist[maxindex] = err;

			/* The following loop forgets maxindex and maxdist and finds them */
			/* from scratch.  This is very inefficient because "cand" is      */
			/* PITCH_VQ_CAND (== 16) and we know the maximum is always        */
			/* replaced just now by the new "err".  However, an attempt of    */
			/* keeping dist[] sorted (so the following loop is not needed     */
			/* every time we update maxdist) only shows minimal improvement.  */

			maxdist = 0;
			for (j = 0; j < cand; j++){
				if (dist[j] > maxdist){
					maxdist = dist[j];
					maxindex = j;
				}
			}
		}
		codebook += dim;                              /* Pointer arithmetics. */
	}
}


/****************************************************************************
**
** Function:		wvq2
**
** Description: 	Pitch vq the second stage
**
** Arguments:
**	Shortword target[] 	: target vector (Q12)
**	Shortword weights[]	: weighting vector (Q0)
**	Shortword codebook[]: codebook (Q12)
**	Shortword dim 		: vector dimension
**	Shortword index[]	: codebook index
**	Longword dist[]		: distortion (Q25)
**	Shortword cand		: number of input candidates
**
** Return value:	Shortword ---- the final codebook index
**
*****************************************************************************/
static Shortword	wvq2(Shortword target[], Shortword weights[],
						 Shortword codebook[], Shortword dim,
						 Shortword index[], Longword dist[], Shortword cand)
{
	register Shortword	i, j;
	Shortword	ind;
	Longword	err, min;        
	Longword	L_temp;
	Shortword	temp;


	/* To reduce the complexity, we should try to increase the opportunity of */
	/* making (err >= min).  In other words, set "min" as small as possible   */
	/* before the loop begins.  One idea is to find the i which minimizes     */
	/* dist[] and use it to compute "min" and "ind" first, then loop through  */
	/* all candidates except this i.  This scheme only reduces about 1/100 of */
	/* the execution time, so it is not implemented here.                     */

	min = LW_MAX;
	ind = 0;

	for (i = 0; i < cand; i++){
		err = dist[i];

		for (j = 0; j < dim; j++){
			if (weights[j] > 0){

				/* weights[] are either 0 or a positive constant              */
				/* DELTA_PITCH_WEIGHT_Q0 == 1.  Note that the following code  */
				/* segment no longer works correctly if DELTA_PITCH_WEIGHT_Q0 */
				/* is changed to other value.                                 */

				/*	err += weights[j] * SQR(target[j] - codebook[j]); */

				temp = sub(target[j], codebook[j]);                    /* Q12 */
				L_temp = L_mult(temp, temp);                           /* Q25 */
				L_temp = L_shr(L_temp, 2);                             /* Q23 */
				err = L_add(err, L_temp);                              /* Q23 */

				/* Exit the loop if (err >= min). */
				if (err >= min)