/* ================================================================== */
/*                                                                    */ 
/*    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:		pitch.c												*/
/*																	*/
/* Description: new pitch estimation routines						*/
/*																	*/
/*------------------------------------------------------------------*/


#include "sc1200.h"
#include "cprv.h"
#include "macro.h"
#include "global.h"
#include "mat_lib.h"
#include "pitch.h"
#include "mathhalf.h"
#include "math_lib.h"
#include "constant.h"
#include "dsp_sub.h"
#include "melp_sub.h"

#define PIT_WIN			(PIT_COR_LEN - MAXPITCH)
#define PIT_WEIGHT		200                                 /* 100 * (1 << 1) */
#define PIT_WEIGHT_Q5	3200										/* 100 Q5 */

#define ONE_Q5			32                                        /* (1 << 5) */
#define ONE_Q10			1024                                     /* (1 << 10) */
#define THREE_Q10		3072                                 /* 3 * (1 << 10) */


/* ========== Prototypes ========== */
static void		lpfilt(Shortword inbuf[], Shortword lpbuf[], Shortword len);
static void		ivfilt(Shortword ivbuf[], Shortword lpbuf[], Shortword len);
static void		corPeak(Shortword inbuf[], pitTrackParam *pitTrack,
													classParam *classStat);

void minCostIndex(Shortword *costBuf, Shortword *index1, Shortword *index2);

/****************************************************************************
**
** Function:		pitchAuto()
**
** Description: 	Calculating time domain pitch estimation
**					structure.
**
** Arguments:
**
**	Shortword inbuf[]		---- speech buffer (Q0)
**	pitTrackParam *pitTrack ---- pitch pitTrackParam structure
**	classParam *classStat	---- classification parameters
**
** Return value:	None
**
*****************************************************************************/
void pitchAuto(Shortword inbuf[], pitTrackParam *pitTrack,
			   classParam *classStat)
{
	static BOOLEAN	firstTime = TRUE;
	static Shortword	lpbuf[PIT_COR_LEN];     /* low pass filter buffer, Q0 */
	static Shortword	ivbuf[PIT_COR_LEN];     /* inverse filter buffer, Q12 */


	if (firstTime){                                 /* initialize the buffers */
		v_zap(lpbuf, PIT_COR_LEN);
		v_zap(ivbuf, PIT_COR_LEN);
		firstTime = FALSE;
	}

	/* The input inbuf[] is not modified in pitchAuto() and the functions     */
	/* pitchAuto() directly or indirectly calls.  On the other hand, an       */
	/* inspection of these functions shows that the output ivbuf[] for        */
	/* ivfilt() is in direct proportion to the input lpbuf[] and inside       */
	/* corPeak() we remove the DC component of ivbuf[] and then compute the   */
	/* normalized crosscorrelations.  This means we can scale lpbuf[] and     */
	/* ivbuf[] any way we want.                                               */

	/* ------ 800Hz low pass filter ------ */
	lpfilt(inbuf, lpbuf, PIT_SUBFRAME);

	/* ------ Two order inverse filter ------- */
	ivfilt(ivbuf, lpbuf, PIT_SUBFRAME);

	/* ------ Calculate the autocorrelation function ------- */
	corPeak(ivbuf, pitTrack, classStat);
}


/*============================================================*
 *				  LPFILT: low pass filter					  *
 * inbuf[]	---- input	speech data (Q0)					  *
 * lpbuf	---- ouput	low pass filtered data				  *
 * len		---- update buffer length						  *
 *============================================================*/
static void lpfilt(Shortword inbuf[], Shortword lpbuf[], Shortword len)
{
	register Shortword	i, j;
	static const Shortword		lpar[4] = {                            /* Q15 */
		20113, -20113, 9437, -1720
	};
	Longword	L_sum;

	/* ====== Shift the lpbuf ====== */
	v_equ(lpbuf, &(lpbuf[len]), (Shortword) (PIT_COR_LEN - len));

	/* ====== low pass filter ====== */

	/* lpbuf[] is a shifted output of the following filter with inbuf[] being */
	/* the input:                                                             */
	/*    H(z) = 0.3069/(1 - 2.4552 z^{-1} + 2.4552 z^{-2} - 1.152 z^{-3} +   */
	/*                   0.2099 z^{-4}).                                      */
	/* First we drop the factor 0.3069 in the numerator because we can afford */
	/* scale the input arbitrarily.  Then we divide everything by 4 to obtain */
	/*    H(z) = 1.0/(0.25 - 0.6138 z^{-1} + 0.6138 z^{-2} - 0.288 z^{-3} +   */
	/*                0.052475 z^{-4}).                                       */

	for (i = 0; i < len; i++){
		L_sum = L_shr(L_deposit_h(inbuf[i]), 3);
		for (j = 0; j < 4; j++){
			/*	sum += lpbuf[PIT_COR_LEN - len + i - j - 1] * lpar[j]; */
			L_sum= L_mac(L_sum, lpbuf[PIT_COR_LEN-len+i-j-1], lpar[j]); /* Q0 */
		}
		lpbuf[PIT_COR_LEN - len + i] = round(L_sum);                    /* Q0 */
	}
}

/*============================================================*
 *				  IVFILT: inverse filter					  *
 * lpbuf ---- input speech data that through low-pass filter  *
 * ivbuf ---- residaul values of lpbuf through inverse filer  *
 * len	 ---- update buffer length							  *
 *============================================================*/
static void ivfilt(Shortword ivbuf[], Shortword lpbuf[], Shortword len)
{
	register Shortword	i, j;
	Word40		L40_sum;
	Longword	L_temp;
	Shortword	shift, rc1, temp1, temp2, temp3;
	Shortword	r_coeff[3];                                            /* Q15 */
	Shortword	pc1, pc2;                                              /* Q12 */

	/* ====== Shift the ivbuf ====== */
	v_equ(ivbuf, &(ivbuf[len]), (Shortword) (PIT_COR_LEN - len));

	/* compute pc1 and pc2 in Q12 as                                          */
    /*       r(0)*r(1)-r(1)*r(2)           r(0)*r(2)-r(1)**2                  */
    /*  pc1 = ------------------- , pc2 = ----------------- .                 */
    /*        r(0)**2-r(1)**2               r(0)**2-r(1)**2                   */

	L40_sum = 0;
	for(i = 0; i < PIT_COR_LEN; i++)
		L40_sum = L40_mac(L40_sum, lpbuf[i], lpbuf[i]);
	shift = norm32(L40_sum);
	L_temp = (Longword) L40_shl(L40_sum, shift);
	r_coeff[0] = round(L_temp);						/* normalized r0 */
	for (i = 1; i < 3; i++){
		L40_sum = 0;
		for (j = i; j < PIT_COR_LEN; j++)
			L40_sum = L40_mac(L40_sum, lpbuf[j], lpbuf[j-i]);
		L_temp = (Longword) L40_shl(L40_sum, shift);
		r_coeff[i] = round(L_temp);
	}

	/* Now compute pc1 and pc2 */
	if (r_coeff[0] == 0){
		pc1 = 0;
		pc2 = 0;
	} else {
        /* rc1 = r[1] / r[0];								*/
		/* rc2 =(r[2] - rc1 * r[1]) / (r[0] - rc1 * r[1]);	*/
        /* pc1 = rc1 - rc1 * rc2;							*/
        /* pc2 = rc2;										*/
		rc1 = divide_s(r_coeff[1], r_coeff[0]);				/* Q15 */
		temp1 = mult(rc1, r_coeff[1]);						/* Q15 */
		temp2 = sub(r_coeff[0], temp1);
		temp3 = sub(r_coeff[2], temp1);
		temp1 = abs_s(temp3);
		if (temp1 > temp2){
			pc2 = (Shortword) -4096;						/* Q12 */
		} else {
			pc2 = divide_s(temp1, temp2);
			if (temp3 < 0)
				pc2 = negate(pc2);							/* Q15 */
			pc2 = shr(pc2, 3);								/* Q12 */
		}

		temp1 = sub(0x1000, pc2);							/* 1.0 - pc2 */
		pc1 = mult(rc1, pc2);								/* Q12 */
	}

	/* --- inverse filter ---- */
	for (i = 0; i < len; i++){
		/*	ivbuf[i] = lpbuf[i] - pc1 * lpbuf[i - 1] - pc2 * lpbuf[i - 2]; */
		L_temp = L_shl(L_deposit_l(lpbuf[PIT_COR_LEN-len+i]), 13);
		L_temp = L_sub(L_temp, L_mult(pc1, lpbuf[PIT_COR_LEN-len+i-1]));
		L_temp = L_sub(L_temp, L_mult(pc2, lpbuf[PIT_COR_LEN-len+i-2]));
		ivbuf[PIT_COR_LEN-len+i] = round(L_shl(L_temp, 3));
	}
}

/*==============================================================*
 *			corPeak: fill pitTrack structure 					*
 *	inbuf		---- input data buffer (Q12)					*
 *	pitTrack	---- pitch pitTrackParam structure				*
 *	classStat	---- classification paramters					*
 *==============================================================*/
static void corPeak(Shortword inbuf[], pitTrackParam *pitTrack,
											classParam *classStat)
{
	register	Shortword	i, j;
	Shortword	temp, temp1, temp2, shift;
	Shortword	proBuf[PIT_COR_LEN];                                   /* Q15 */
	Longword	L_temp;                                                 /* Q0 */
	Shortword	index[MAXPITCH + 1];
	Shortword	lowStart, highStart;
	Word40		ACC_r0, ACC_rk, ACC_A;         /* Emulating 40Bit-Accumulator */
	Longword	L_r0, L_rk;
	Shortword	r0_shift, rk_shift, root;
	Shortword	gp[MAXPITCH + 1], peak[MAXPITCH + 1], corx[NODE];      /* Q15 */


	/* ------ Remove DC component. ------ */
	remove_dc(inbuf, proBuf, PIT_COR_LEN);

	/* ------ Caculate the autocorrelation function ------- */
	ACC_r0 = 0;
	for (i = 0; i < PIT_COR_LEN - MAXPITCH; i++){
		ACC_r0 = L40_mac(ACC_r0, proBuf[i], proBuf[i]);  /* Q31 */
	}
	if (ACC_r0 == 0)
		ACC_r0 = 1;
	r0_shift = norm32(ACC_r0);
	ACC_r0 = L40_shl(ACC_r0, r0_shift);
	L_r0 = (Longword) ACC_r0;

	ACC_rk = 0;
	for (i = MAXPITCH ; i < PIT_COR_LEN; i++){
		ACC_rk = L40_mac(ACC_rk, proBuf[i], proBuf[i]);  /* Q31 */
	}
	if (ACC_rk == 0)
		ACC_rk = 1;
	rk_shift = norm32(ACC_rk);
	ACC_rk = L40_shl(ACC_rk, rk_shift);
	L_rk = (Longword) ACC_rk;

	ACC_A = 0;
	for (i = 0; i < PIT_COR_LEN - MAXPITCH; i++){
		ACC_A = L40_mac(ACC_A, proBuf[i], proBuf[i+MAXPITCH]); /* Q31 */
	}
	shift = add(r0_shift, rk_shift);
	if (shift & 1){
		L_r0 = L_shr(L_r0, 1);
		r0_shift = sub(r0_shift, 1);
		shift = add(r0_shift, rk_shift);
	}
	shift = shr(shift, 1);
	ACC_A = L40_shl(ACC_A, shift);
	temp = mult(extract_h(L_r0), extract_h(L_rk));
	root = sqrt_Q15(temp);
	L_temp = (Longword) ACC_A;
	temp = extract_h(L_temp);
	if (temp < 0)
		temp = 0; /* Negative Autocorrelation doesn't make sense here */
	gp [MAXPITCH] = divide_s(temp, root); 
	temp = gp[MAXPITCH];

	/* ==== Here comes the Main loop ==== */
	
	lowStart = 0;
	highStart = MAXPITCH;
	for (i = MAXPITCH - 1; i >= MINPITCH; i--){
		if (i % 2 == 0){
			ACC_r0 = L_r0;
			ACC_r0 = L40_shr(ACC_r0, r0_shift);
			ACC_r0 = L40_msu(ACC_r0, proBuf[lowStart], proBuf[lowStart]);
			ACC_r0 = L40_mac(ACC_r0, proBuf[lowStart + PIT_WIN], 
												proBuf[lowStart + PIT_WIN]); 
			if (ACC_r0 == 0)
				ACC_r0 = 1;
			r0_shift = norm32(ACC_r0);
			ACC_r0 = L40_shl(ACC_r0, r0_shift);
			L_r0 = (Longword) ACC_r0;
			lowStart++;
		} else {
			highStart--;
			ACC_rk = L_rk;
			ACC_rk = L40_shr(ACC_rk, rk_shift);
			ACC_rk = L40_mac(ACC_rk, proBuf[highStart], proBuf[highStart]);
			ACC_rk = L40_msu(ACC_rk, proBuf[highStart+PIT_WIN], 
													proBuf[highStart+PIT_WIN]); 
			if (ACC_rk == 0)
				ACC_rk = 1;
			rk_shift = norm32(ACC_rk);
			ACC_rk = L40_shl(ACC_rk, rk_shift);
			L_rk = (Longword) ACC_rk;
		}
		ACC_A = 0;
		for (j = lowStart; j < lowStart + PIT_WIN; j++){
			ACC_A = L40_mac(ACC_A, proBuf[j], proBuf[j+i]);
		}
		shift = add(r0_shift, rk_shift);
		if (shift & 1){
			L_r0 = L_shr(L_r0, 1);
			r0_shift = sub(r0_shift, 1);
			shift = add(r0_shift, rk_shift);
		}
		shift = shr(shift, 1);
		ACC_A = L40_shl(ACC_A, shift);
		temp = mult(extract_h(L_r0), extract_h(L_rk));
		root = sqrt_Q15(temp);
		L_temp = (Longword) ACC_A;
		temp = extract_h(L_temp);
		if (temp < 0)
			temp = 0; /* ignore negative autocorrelation */
		gp [i] = divide_s(temp, root); 
	} /* Main loop ends */

	/* ------ Find the local peak of gp function ------- */
	if (gp[MINPITCH + 1] < gp[MINPITCH]) 
		peak[MINPITCH] = gp[MINPITCH];
	else
		peak[MINPITCH] = 0;

	if (gp[MAXPITCH] > gp[MAXPITCH - 1]) 
		peak[MAXPITCH] = gp[MAXPITCH];
	else	
		peak[MAXPITCH] = 0;

	for (i = MINPITCH + 1; i < MAXPITCH; i++){
		if ((gp[i] > gp[i - 1]) && (gp[i] > gp[i + 1]))
			peak[i] = gp[i];
		else
			peak[i] = 0;
	}

	/* --- Fill in the TRACK struct using the first NODE peaks --- */
	v_zap(index, MAXPITCH + 1);

	for (i = 0; i < NODE; i++){
		temp = MAXPITCH;
		for (j = MAXPITCH - 1; j >= MINPITCH; j--){
			if (peak[j] > peak[temp])
				temp = j;
		}
		index[temp] = (Shortword) (i + 1);
		corx[i] = peak[temp];
		peak[temp] = 0;
		if (i == 0)
			classStat->pitch = temp;