/* ================================================================== */
/*                                                                    */ 
/*    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:		"postfilt.c"										*/
/*																	*/
/* Description: 	postfilter and postprocessing in decoder		*/
/*																	*/
/* Function:														*/
/*		postfilt()													*/
/*		postproc()													*/
/*																	*/
/*------------------------------------------------------------------*/

#include "sc1200.h"
#include "constant.h"
#include "postfilt.h"
#include "dsp_sub.h"
#include "mat_lib.h"
#include "math_lib.h"
#include "lpc_lib.h"
#include "mathhalf.h"

/* ---- Postfilter ---- */
#define ALPH			18678                                    /* 0.57, Q15 */
#define BETA			24576                                    /* 0.75, Q15 */

#define SMOOTH_LEN		20
#define X05_Q12			2048

/* ===== Prototypes ===== */
static void		hpf60(Shortword speech[]);
static void		lpf3500(Shortword speech[]);


/****************************************************************
**
** Function:		postfilt()
**
** Description: 	postfilter including short term postfilter
**				and tilt compensation
**
** Arguments:
**
**	Shortword	speech[]	: input/output synthesized speech (Q0)
**
** Return value:	None
**
*****************************************************************/

void postfilt(Shortword speech[], Shortword prev_lsf[], Shortword cur_lsf[])
{
	register Shortword	i, j, k;
	static const Shortword	syn_inp[SYN_SUBNUM] = {                    /* Q15 */
								4096, 12288, 20480, 28672
	};
	static BOOLEAN	postfilt_firsttime = TRUE;
	static Shortword	hpm = 0;                                        /* Q0 */
	static Shortword	mem1[LPC_ORD] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	static Shortword	mem2[LPC_ORD] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	static Shortword	gain = 0;                                      /* Q14 */
	static Shortword	alphaipFIR[LPC_ORD];                           /* Q12 */
	static Shortword	alphaipIIR[LPC_ORD];
	static Shortword	window[SMOOTH_LEN];                            /* Q15 */
	Shortword	temp, temp_shift;
	Shortword	temp1, temp2;
	Longword	L_temp;
	Shortword	synLPC[LPC_ORD];                                       /* Q12 */
	Shortword	inplsf[LPC_ORD];

	/* ---- High frequency emphasis ---- */
	Shortword	emph;                                                  /* Q15 */
	Shortword	synhp[SYN_SUBFRAME];                                    /* Q0 */

	/* ---- FIR, IIR filter ---- */
	Longword	L_sum;
	Shortword	mem1old[LPC_ORD];
	Shortword	mem2old[LPC_ORD];

	Shortword	nokori[SMOOTH_LEN];
	Shortword	sp, sp_shift;
	Shortword	op, op_shift;


	if (postfilt_firsttime){
		/* ======== Compute smoothing window ======== */
		temp = 0;                                                      /* Q15 */
		for (i = 0; i < SMOOTH_LEN; i++){
			window[i] = temp;
			temp = add(temp, 1638);            /* 1638 is 1/SMOOTH_LEN in Q15 */
		}
		postfilt_firsttime = FALSE;
	}

	/* Computing sp, the sum of squares of speech[].  We would treat speech[] */
	/* as if it is Q15, and we will do the same thing when computing op.      */

	sp = 0;
	for (i = 0; i < FRAME; i++){
		temp = abs_s(speech[i]);
		if (sp < temp)
			sp = temp;
	}
	temp_shift = norm_s(sp);
	L_sum = 0;
	for (i = 0; i < FRAME; i++){
		temp = shl(speech[i], temp_shift);                             /* Q15 */
		L_temp = L_mult(temp, temp);                                   /* Q31 */
		L_temp = L_shr(L_temp, 8);                                     /* Q23 */
		L_sum = L_add(L_sum, L_temp);                                  /* Q23 */
	}
	temp_shift = shl(temp_shift, 1);                  /* Squaring of speech[] */
	sp_shift = sub(8, temp_shift);                   /* Aligning Q23 with Q31 */
	temp_shift = norm_l(L_sum);
	sp_shift = sub(sp_shift, temp_shift);
	sp = extract_h(L_shl(L_sum, temp_shift));                          /* Q15 */

	/* ======== Compute filter coefficients ======== */
	for (i = 0; i < SYN_SUBNUM; i++){
		for (j = 0; j < LPC_ORD; j++){
			temp = sub(ONE_Q15, syn_inp[i]);                           /* Q15 */
			temp1 = mult(prev_lsf[j], temp);                           /* Q15 */
			temp2 = mult(cur_lsf[j], syn_inp[i]);                      /* Q15 */
			inplsf[j] = add(temp1, temp2);                             /* Q15 */
		}
		lpc_lsp2pred(inplsf, synLPC, LPC_ORD);

		/* ======== Filter main loop ======== */

		/* ------ Tilt compesation ------ */
		temp = mult(X015_Q15, synLPC[1]);                              /* Q12 */
		if (temp > X05_Q12)
			temp = X05_Q12;
		if (temp < 0)
			temp = 0;
		emph = shl(temp, 3);                                           /* Q15 */

		/* It is unlikely for synhp[] to saturate -- emph is confined between */
		/* 0 and 0.5.  To saturate synhp[] the input speech[] should be of a  */
		/* large magnitude and the next sample should be of opposite sign,    */
		/* which should not be frequent.                                      */

		for (j = 0; j < SYN_SUBFRAME; j++){
			temp = mult(emph, hpm);                                     /* Q0 */
			hpm = speech[i*SYN_SUBFRAME + j];
			synhp[j] = sub(hpm, temp);                                  /* Q0 */
		}

		/* ------ Short-term postfilter ------ */

		/* Here we assume that SMOOTH_LEN (== 20) is smaller than             */
		/* SYN_SUBFRAME (== 45) so we only need to compute nokori[] when k is */
		/* 0.                                                                 */

		if (i == 0){
			v_equ(mem1old, mem1, LPC_ORD);
			v_equ(mem2old, mem2, LPC_ORD);

			for (j = 0; j < SMOOTH_LEN; j++){
				L_sum = 0;
				for (k = 0; k < LPC_ORD; k++){
					L_temp = L_mult(mem1old[k], alphaipFIR[k]);        /* Q13 */
					L_sum = L_add(L_sum, L_temp);                      /* Q13 */
				}
				for (k = LPC_ORD - 1; k > 0; k--)
					mem1old[k] = mem1old[k - 1];
				mem1old[0] = synhp[j];
				L_temp = L_shl(L_deposit_l(synhp[j]), 13);             /* Q13 */
				L_sum = L_add(L_sum, L_temp);

				for (k = 0; k < LPC_ORD; k++){
					L_temp = L_mult(mem2old[k], alphaipIIR[k]);        /* Q13 */
					L_sum = L_sub(L_sum, L_temp);
				}
				for (k = LPC_ORD - 1; k > 0; k--)
					mem2old[k] = mem2old[k - 1];
				temp1 = extract_l(L_shr(L_sum, 13));                    /* Q0 */
				mem2old[0] = temp1;                                     /* Q0 */
				temp = sub(ONE_Q15, window[j]);

				/* Experiments based on the 16 speech data used for testing   */
				/* show that nokori[] is within the range -15000 to 12000 or  */
				/* so.  Therefore using Q0 for nokori[] should be             */
				/* appropriate.                                               */

				temp1 = mult(temp, temp1);                              /* Q0 */