/* ================================================================== */
/*                                                                    */ 
/*    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.                                            */
/*                                                                    */ 
/* ================================================================== */

/*

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.

*/

/* =========================== */
/* lpc_lib.c: LPC subroutines. */
/* =========================== */

/*	compiler include files	*/

#include <stdio.h>

#include "sc1200.h"
#include "macro.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "math_lib.h"
#include "mat_lib.h"
#include "lpc_lib.h"
#include "constant.h"
#include "global.h"
#include "dsp_sub.h"


#define ALMOST_ONE_Q14		16382                            /* ((1 << 14)-2) */
#define ONE_Q25				33554431L                            /* (1 << 25) */
#define ONE_Q26				67108864L                            /* (1 << 26) */
#define LOW_LIMIT			54  /* lower limit of return value for lpc_aejw() */
                                 /* to prevent overflow of weighting function */
#define MAX_LOOPS			10
#define DFTLENGTH			512
#define DFTLENGTH_D2		(DFTLENGTH/2)
#define DFTLENGTH_D4		(DFTLENGTH/4)

/* Prototype */

static void		lsp_to_freq(Shortword lsp[], Shortword freq[], Shortword order);
static Shortword	lpc_refl2pred(Shortword refc[], Shortword lpc[],
								  Shortword order);


/* LPC_ACOR                                                                   */
/*		Compute autocorrelations based on windowed speech frame               */
/*                                                                            */
/*	Synopsis: lpc_acor(input, window, r, hf_correction, order, npts)          */
/*		Input:                                                                */
/*			input- input vector (npts samples, s[0..npts-1])                  */
/*			win_cof- window vector (npts samples, s[0..npts-1])               */
/*			hf_correction- high frequency correction value                    */
/*			order- order of lpc filter                                        */
/*			npts- number of elements in window                                */
/*		Output:                                                               */
/*			autocorr- output autocorrelation vector (order + 1 samples,       */
/*                    autocorr[0..order])                                     */
/*                                                                            */
/*	Q values: input - Q0, win_cof - Q15, hf_correction - Q15                  */

void lpc_acor(Shortword input[], const Shortword win_cof[],
			  Shortword autocorr[], Shortword hf_correction, Shortword order,
			  Shortword npts)
{
	/* Lag window coefficients */
	static const Shortword		lagw_cof[EN_FILTER_ORDER - 1] = {
		32756, 32721, 32663, 32582, 32478, 32351, 32201, 32030, 31837, 31622,
		31387, 31131, 30855, 30560, 30246, 29914
	};
	register Shortword	i, j;
	Longword	L_temp;
	Shortword	*inputw;
	Shortword	norm_var, scale_fact, temp;


	/* window optimized for speed and readability.  does windowing and        */
	/* autocorrelation sequentially and in the usual manner                   */

	inputw = v_get(npts);
	for (i = 0; i < npts; i++){
		inputw[i] = mult(win_cof[i], shr(input[i], 4));
	}

	/* Find scaling factor */
	L_temp = L_v_magsq(inputw, npts, 0, 1);
	if (L_temp){
		norm_var = norm_l(L_temp);
		norm_var = sub(4, shr(norm_var, 1));
		if (norm_var < 0)
			norm_var = 0;
	} else
		norm_var = 0;

	for (i = 0; i < npts; i++){
		inputw[i] = shr(mult(win_cof[i], input[i]), norm_var);
	}

	/* Compute r[0] */
	L_temp = L_v_magsq(inputw, npts, 0, 1);
	if (L_temp > 0){
		/* normalize with 1 bit of headroom */
		norm_var = norm_l(L_temp);
		norm_var = sub(norm_var, 1);
		L_temp = L_shl(L_temp, norm_var);

		/* High frequency correction */
		L_temp = L_add(L_temp, L_mpy_ls(L_temp, hf_correction));

		/* normalize result */
		temp = norm_s(extract_h(L_temp));
		L_temp = L_shl(L_temp, temp);
		norm_var = add(norm_var, temp);
		autocorr[0] = round(L_temp);

		/* Multiply by 1/autocorr[0] for full normalization */
		scale_fact = divide_s(ALMOST_ONE_Q14, autocorr[0]);
		L_temp = L_shl(L_mpy_ls(L_temp, scale_fact), 1);
		autocorr[0] = round(L_temp);

	} else {
		norm_var = 0;
		autocorr[0] = ONE_Q15;                                    /* 1 in Q15 */
		scale_fact = 0;
	}

	/* Compute remaining autocorrelation terms */
	for (j = 1; j <= order; j++){
		L_temp = 0;
		for (i = j; i < npts; i++)
			L_temp = L_mac(L_temp, inputw[i], inputw[i - j]);
		L_temp = L_shl(L_temp, norm_var);

		/* Scaling */
		L_temp = L_shl(L_mpy_ls(L_temp, scale_fact), 1);

		/* Lag windowing */
		L_temp = L_mpy_ls(L_temp, lagw_cof[j - 1]);

		autocorr[j] = round(L_temp);
	}
	v_free(inputw);
}


/* Name: lpc_aejw- Compute square of A(z) evaluated at exp(jw)                */
/* Description:                                                               */
/*		Compute the magnitude squared of the z-transform of                   */
/*                                                                            */
/*		A(z) = 1 + a(1)z^-1 + ... + a(p)z^-p                                  */
/*                                                                            */
/*		evaluated at z = exp(jw)                                              */
/*	 Inputs:                                                                  */
/*		lpc (a)- LPC filter (a[0] is undefined, a[1..p])                      */
/*		omega (w)- radian frequency                                           */
/*		order (p)- predictor order                                            */
/*	 Returns:                                                                 */
/* 		|A(exp(jw))|^2                                                        */
/*	 See_Also: cos(3), sin(3)                                                 */
/*	 Includes:                                                                */
/*		lpc.h                                                                 */
/*	 Systems and Info. Science Lab                                            */
/*	 Copyright (c) 1995 by Texas Instruments, Inc.	All rights reserved.      */
/*                                                                            */
/* Q values:                                                                  */
/*      lpc - Q12, omega - Q15, return - Q19                                  */

Longword lpc_aejw(Shortword lpc[], Shortword omega, Shortword order)
{
	register Shortword	i;
	Shortword	c_re, c_im;
	Shortword	cs, sn, temp;
	Shortword	temp1, temp2;
	Longword	L_temp;


	if (order == 0)
		return((Longword) ONE_Q19);

	/* use horners method                                                     */
	/* A(exp(jw)) = 1+ e(-jw)[a(1) + e(-jw)[a(2) + e(-jw)[a(3) +..            */
	/*				...[a(p-1) + e(-jw)a(p)]]]]                               */

	cs = cos_fxp(omega);                                          /* Q15 */
	sn = negate(sin_fxp(omega));                               /* Q15 */

	temp1 = lpc[order - 1];
	c_re = shr(mult(cs, temp1), 3);                            /* -> Q9 */
	c_im = shr(mult(sn, temp1), 3);                            /* -> Q9 */

	for (i = sub(order, 2); i >= 0; i--){
		/* add a[i] */
		temp = shr(lpc[i], 3);
		c_re = add(c_re, temp);

		/* multiply by exp(-jw) */
		temp = c_im;
		temp1 = mult(cs, temp);                                /* temp1 in Q9 */
		temp2 = mult(sn, c_re);                               /* temp2 in Q9 */
		c_im = add(temp1, temp2);
		temp1 = mult(cs, c_re);                            /* temp1 in Q9 */
		temp2 = mult(sn, temp);                           /* temp2 in Q9 */
		c_re = sub(temp1, temp2);
	}

	/* add one */
	c_re = add(c_re, ONE_Q9);

	/* L_temp in Q19 */
	L_temp = L_add(L_mult(c_re, c_re), L_mult(c_im, c_im));
	if (L_temp < LOW_LIMIT)
		L_temp = (Longword) LOW_LIMIT;

	return(L_temp);
}


/* Name: lpc_bwex- Move the zeros of A(z) toward the origin.                  */
/*	Aliases: lpc_bw_expand                                                    */
/*	Description:                                                              */
/*		Expand the zeros of the LPC filter by gamma, which                    */
/*		moves each zero radially into the origin.                             */
/*                                                                            */
/*		for j = 1 to p                                                        */
/*			aw[j] = a[j]*gamma^j                                              */
/*		(Can also be used to perform an exponential windowing procedure).     */
/*	Inputs:                                                                   */
/*		lpc (a)- lpc vector (order p, a[1..p])                                */
/*		gamma- the bandwidth expansion factor                                 */
/*		order (p)- order of lpc filter                                        */
/*	Outputs:                                                                  */
/*		aw- the bandwidth expanded LPC filter                                 */
/*	Returns: NULL                                                             */
/*	See_Also: lpc_lagw(3l)                                                    */
/*	Includes:                                                                 */
/*		lpc.h                                                                 */
/*                                                                            */
/*	Systems and Info. Science Lab                                             */
/*	Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.       */
/*                                                                            */
/*	Q values: lpc[], aw[] - Q12, gamma - Q15, gk - Q15                        */

Shortword lpc_bwex(Shortword lpc[], Shortword aw[], Shortword gamma,
				   Shortword order)
{
	register Shortword	i;
	Shortword	gk;                                              /* gk is Q15 */

	gk = gamma;

	for (i = 0; i < order; i++){
		aw[i] = mult(lpc[i], gk);
		gk = mult(gk, gamma);
	}
	return(0);
}


/* Name: lpc_clmp- Sort and ensure minimum separation in LSPs.                */
/*	Aliases: lpc_clamp                                                        */
/*	Description:                                                              */
/*		Ensure that all LSPs are ordered and separated                        */
/*		by at least delta.	The algorithm isn't guarenteed                    */
/*		to work, so it prints an error message when it fails                  */
/*		to sort the LSPs properly.                                            */
/*	Inputs:                                                                   */
/*		lsp (w)- lsp vector (order p, w[1..p])                                */
/*		delta- the clamping factor                                            */
/*		order (p)- order of lpc filter                                        */
/*	Outputs:                                                                  */
/*		lsp (w)- the sorted and clamped lsps                                  */
/*	Returns: NULL                                                             */
/*	See_Also:                                                                 */
/*	Includes:                                                                 */
/*		lpc.h                                                                 */
/*	Bugs:                                                                     */