/*

2.4 kbps MELP Proposed Federal Standard speech coder

Fixed-point C code, version 1.0

Copyright (c) 1998, Texas Instruments, Inc.

Texas Instruments has intellectual property rights on the MELP
algorithm.	The Texas Instruments contact for licensing issues for
commercial and non-government use is William Gordon, Director,
Government Contracts, Texas Instruments Incorporated, Semiconductor
Group (phone 972 480 7442).

The fixed-point version of the voice codec Mixed Excitation Linear
Prediction (MELP) is based on specifications on the C-language software
simulation contained in GSM 06.06 which is protected by copyright and
is the property of the European Telecommunications Standards Institute
(ETSI). This standard is available from the ETSI publication office
tel. +33 (0)4 92 94 42 58. ETSI has granted a license to United States
Department of Defense to use the C-language software simulation contained
in GSM 06.06 for the purposes of the development of a fixed-point
version of the voice codec Mixed Excitation Linear Prediction (MELP).
Requests for authorization to make other use of the GSM 06.06 or
otherwise distribute or modify them need to be addressed to the ETSI
Secretariat fax: +33 493 65 47 16.

*/

/* ===================================== */
/* melp_sub.c: MELP-specific subroutines */
/* ===================================== */

#include "sc1200.h"
#include "mathhalf.h"
#include "math_lib.h"
#include "mat_lib.h"
#include "dsp_sub.h"
#include "melp_sub.h"
#include "pit_lib.h"
#include "constant.h"
#include "coeff.h"
#include "global.h"

/* Filter orders */

#define CONSTANT_SCALE		FALSE
#define MINGAIN				0
#define LOG10OF2X2			1233                         /* 2*log10(2) in Q11 */
#define GAIN_INT_DB_Q8		1280                              /* 5 * (1 << 8) */
#define ONE_Q8				256                                   /* (1 << 8) */
#define THREE_Q8			768                               /* 3 * (1 << 8) */
#define SIX_Q12				24576                            /* 6 * (1 << 12) */
#define TEN_Q11				20480                           /* 10 * (1 << 11) */
#define X0001_Q8			0                             /* 0.001 * (1 << 8) */
#define X01_Q14				1638                           /* 0.1 * (1 << 14) */
#define N2_Q11				-4096                           /* -2 * (1 << 11) */
#define M01_Q15				-3276                         /* -0.1 * (1 << 15) */
#define M10_Q11				-20480                         /* -10 * (1 << 11) */


/* Name: bpvc_ana.c                                                           */
/*  Description: Bandpass voicing analysis                                    */
/*  Inputs:                                                                   */
/*    speech[] - input speech signal                                          */
/*    fpitch[] - initial (floating point) pitch estimates                     */
/*  Outputs:                                                                  */
/*    bpvc[] - bandpass voicing decisions                                     */
/*    pitch[] - frame pitch estimates                                         */
/*  Returns: void                                                             */
/*                                                                            */
/*  Copyright (c) 1995,1997 by Texas Instruments, Inc.  All rights reserved.  */
/*                                                                            */
/* Q values: speech - Q0, fpitch - Q7, bpvc - Q14, pitch - Q7                 */

void bpvc_ana(Shortword speech[], Shortword fpitch[], Shortword bpvc[],
			  Shortword *pitch)
{
	register Shortword	i, section;
	static Shortword	bpfsp[NUM_BANDS][PITCH_FR - FRAME];
	static Shortword	bpfdelin[NUM_BANDS][BPF_ORD];
	static Shortword	bpfdelout[NUM_BANDS][BPF_ORD];
	static Shortword	envdel[NUM_BANDS][ENV_ORD];
	static Shortword	envdel2[NUM_BANDS];
	static BOOLEAN	firstTime = TRUE;
	Shortword	sigbuf[BPF_ORD + PITCH_FR];
	Shortword	pcorr, temp, scale;
	Shortword	filt_index;



	if (firstTime){
		for (i = 0; i < NUM_BANDS; i++){
			v_zap(bpfsp[i], PITCH_FR - FRAME);
			v_zap(bpfdelin[i], BPF_ORD);
			v_zap(bpfdelout[i], BPF_ORD);
		}
		for (i = 0; i < NUM_BANDS; i++)
			v_zap(envdel[i], ENV_ORD);
		v_zap(envdel2, NUM_BANDS);
		firstTime = FALSE;
	}

	/* Filter lowest band and estimate pitch */
	v_equ(&sigbuf[BPF_ORD], bpfsp[0], PITCH_FR - FRAME);
	v_equ(&sigbuf[BPF_ORD + PITCH_FR - FRAME],
		  &speech[PITCH_FR - FRAME - PITCHMAX], FRAME);

	for (section = 0; section < BPF_ORD/2; section ++){
		iir_2nd_s(&sigbuf[BPF_ORD + PITCH_FR - FRAME],
				  &bpf_den[section*3], &bpf_num[section*3],
				  &sigbuf[BPF_ORD + PITCH_FR - FRAME], &bpfdelin[0][section*2],
				  &bpfdelout[0][section*2], FRAME);
	}
	v_equ(bpfsp[0], &sigbuf[BPF_ORD + FRAME], PITCH_FR - FRAME);

	/* Scale signal for pitch correlations */
	f_pitch_scale(&sigbuf[BPF_ORD], &sigbuf[BPF_ORD], PITCH_FR);

	*pitch = frac_pch(&sigbuf[BPF_ORD + PITCHMAX], bpvc, fpitch[0], 5,
					  PITCHMIN, PITCHMAX, PITCHMIN_Q7, PITCHMAX_Q7,
					  MINLENGTH);

	for (i = 1; i < NUM_PITCHES; i++){
		temp = frac_pch(&sigbuf[BPF_ORD + PITCHMAX], &pcorr, fpitch[i], 5,
						PITCHMIN, PITCHMAX, PITCHMIN_Q7, PITCHMAX_Q7,
						MINLENGTH);

		/* choose largest correlation value */
		if (pcorr > bpvc[0]){
			*pitch = temp;
			bpvc[0] = pcorr;
		}
	}

	/* Calculate bandpass voicing for frames */
	for (i = 1; i < NUM_BANDS; i++){
		/* Bandpass filter input speech */
		v_equ(&sigbuf[BPF_ORD], bpfsp[i], PITCH_FR - FRAME);
		v_equ(&sigbuf[BPF_ORD + PITCH_FR - FRAME],
			  &speech[PITCH_FR - FRAME - PITCHMAX], FRAME);

		for (section = 0; section < BPF_ORD/2; section++){
			filt_index = (Shortword) (i*(BPF_ORD/2)*3 + section*3);
			iir_2nd_s(&sigbuf[BPF_ORD + PITCH_FR - FRAME],
					  &bpf_den[filt_index], &bpf_num[filt_index],
					  &sigbuf[BPF_ORD + PITCH_FR - FRAME],
					  &bpfdelin[i][section*2], &bpfdelout[i][section*2],
					  FRAME);
		}
		v_equ(bpfsp[i], &sigbuf[BPF_ORD + FRAME], PITCH_FR - FRAME);

		/* Scale signal for pitch correlations */
		scale = f_pitch_scale(&sigbuf[BPF_ORD], &sigbuf[BPF_ORD], PITCH_FR);

		/* Check correlations for each frame */
		temp = frac_pch(&sigbuf[BPF_ORD + PITCHMAX], &bpvc[i], *pitch, 0,
						PITCHMIN, PITCHMAX, PITCHMIN_Q7, PITCHMAX_Q7,
						MINLENGTH);

		/* Calculate envelope of bandpass filtered input speech */
		/* update delay buffers without scaling */
		temp = shr(envdel2[i], scale);
		envdel2[i] = shr(sigbuf[BPF_ORD + FRAME - 1], (Shortword) (-scale));
		v_equ_shr(&sigbuf[BPF_ORD - ENV_ORD], envdel[i], scale, ENV_ORD);
		envelope(&sigbuf[BPF_ORD], temp, &sigbuf[BPF_ORD], PITCH_FR);
		v_equ_shr(envdel[i], &sigbuf[BPF_ORD + FRAME - ENV_ORD],
				  (Shortword) (-scale), ENV_ORD);

		/* Scale signal for pitch correlations */
		f_pitch_scale(&sigbuf[BPF_ORD], &sigbuf[BPF_ORD], PITCH_FR);

		/* Check correlations for each frame */
		temp = frac_pch(&sigbuf[BPF_ORD + PITCHMAX], &pcorr, *pitch, 0,
						PITCHMIN, PITCHMAX, PITCHMIN_Q7, PITCHMAX_Q7,
						MINLENGTH);

		/* reduce envelope correlation */
		pcorr = sub(pcorr, X01_Q14);

		if (pcorr > bpvc[i])
			bpvc[i] = pcorr;
	}
}


/* Name: dc_rmv.c                                                             */
/*	Description: remove DC from input signal                                  */
/*	Inputs:                                                                   */
/*	  sigin[] - input signal                                                  */
/*	  dcdel[] - filter delay history (size DC_ORD)                            */
/*	  frame - number of samples to filter                                     */
/*	Outputs:                                                                  */
/*	  sigout[] - output signal                                                */
/*	  dcdel[] - updated filter delay history                                  */
/*	Returns: void                                                             */
/*                                                                            */
/*	Copyright (c) 1995,1997 by Texas Instruments, Inc.	All rights reserved.  */

/* DC removal filter                                                          */
/* 4th order Chebychev Type II 60 Hz removal filter                           */
/* cutoff = 60 Hz, stop = -30 dB                                              */
/* matlab commands: [z, p, k] = cheby2(4, 30, 0.015, 'high');                 */
/*						sos = zp2sos(z, p, k);                                */
/* order of sections swapped                                                  */

void dc_rmv(Shortword sigin[], Shortword sigout[], Shortword delin[],
			Shortword delout_hi[], Shortword delout_lo[],
			Shortword frame)
{
	static const Shortword	dc_num[(DC_ORD/2)*3] = {             /* Maybe Q13 */
#if NEW_DC_FILTER
		8192,  -16376, 8192,
		8192,  -16370, 8192,
		7353,  -14706, 7353
#else
		7769, -15534, 7769,
		8007, -15999, 8007
#endif
	};

	/* Signs of coefficients for dc_den are reversed.  dc_den[0] and          */
	/* dc_den[3] are ignored.                                                 */
	static const Shortword	dc_den[(DC_ORD/2)*3] = {
#if NEW_DC_FILTER
		-8192, 15729, -7569,
		-8192, 16111, -7959,
		-8192, 15520, -7353
#else
		-8192, 15521, -7358,
		-8192, 15986, -7836
#endif
	};
	register Shortword	section;

	/* Remove DC from input speech */
	v_equ(sigout, sigin, frame);
	for (section = 0; section < DC_ORD/2; section++)
		iir_2nd_d(sigout, &dc_den[section*3], &dc_num[section*3], sigout,
				  &delin[section*2], &delout_hi[section*2],
				  &delout_lo[section*2], frame);
}


/* This function removes the DC component of a given signal sigin[].  It      */
/* computes the offset Sum(sigin[])/len by                                    */
/* Sum(sigin[])/len = Sum(sigin[])/2^up_shift * (two_power_down/len) * 2      */
/* where two_power_down <= len < 2^up_shift.  This way we ensure that         */
/* Sum(sigin[])/up_shift does not overflow, (two_power_down/len) is a Q15     */
/* and the multiplication by 2 is straightforward.                            */
/*                                                                            */
/* Originally this function was written in such a way that we use a Shortword */
/* for "sum", and it is important to make sure there is no overflow with      */
/* "sum".  However, later it was found a siginificant inaccuracy is           */
/* introduced if we shift all sigin[] by up_shift (losing significant digits) */
/* and then add them.  This means when "len" is large the result could be     */
/* wrong.  Now "sum" is declared a Longword and all these problems disappear. */

void remove_dc(Shortword sigin[], Shortword sigout[], Shortword len)
{
	register Shortword	i;
	Shortword	up_shift, two_power_down;
	Longword	sum;
	Shortword	temp;
	Shortword	offset;


	/* Find up_shift and two_power_down.  Note that two_power_down can be     */
	/* equal to len.                                                          */

	temp = norm_s(len);
	up_shift = sub(15, temp);
	temp = sub(up_shift, 1);
	two_power_down = shl(1, temp);

	sum = 0;
	for (i = 0; i < len; i++)
		sum = L_add(sum, L_deposit_l(sigin[i]));
	sum = L_shr(sum, up_shift);

	/* if (len > two_power_down) */
	/* if condition has been removed 'cos it is always true */
	{
		temp = divide_s(two_power_down, len);                          /* Q15 */
		offset = mult(extract_l(sum), temp);
	}
	offset = shl(offset, 1);

	for (i = 0; i < len; i++)
		sigout[i] = sub(sigin[i], offset);
}