/*
 * Copyright 1992 by Jutta Degener and Carsten Bormann, Technische
 * Universitaet Berlin.  See the accompanying file "COPYRIGHT" for
 * details.  THERE IS ABSOLUTELY NO WARRANTY FOR THIS SOFTWARE.
 */

/* $Header: n:\\development/speak_freely/GSM/LPC.C,v 1.1.1.1 1998/10/15 00:47:39 Administrator Exp $ */

#include <stdio.h>
#include <assert.h>

#include "private.h"

#include "gsm.h"
#include "proto.h"

#undef	P

/*
 *	4.2.4 .. 4.2.7 LPC ANALYSIS SECTION
 */

/* 4.2.4 */


static void Autocorrelation P2((s, L_ACF),
		word	 * s,			/* [0..159] 	IN/OUT	*/
		longword * L_ACF)		/* [0..8]		OUT 	*/
/*
 *	The goal is to compute the array L_ACF[k].	The signal s[i] must
 *	be scaled in order to avoid an overflow situation.
 */
{
		register int	k, i;

		word			temp, smax, scalauto;
//		longword		L_temp;

#ifdef	USE_FLOAT_MUL
		float			float_s[160];
//		longword		L_temp2;
#else
//		longword		L_temp2;
#endif

		/*	Dynamic scaling of the array  s[0..159]
		 */

		/*	Search for the maximum.
		 */
		smax = 0;
		for (k = 0; k <= 159; k++) {
				temp = GSM_ABS( s[k] );
				if (temp > smax) smax = temp;
		}

		/*	Computation of the scaling factor.
		 */
		if (smax == 0) scalauto = 0;
		else {
				assert(smax > 0);
				scalauto = 4 - gsm_norm( (longword)smax << 16 );/* sub(4,..) */
		}

		/*	Scaling of the array s[0...159]
		 */

		if (scalauto > 0) {

# ifdef USE_FLOAT_MUL
#   define SCALE(n)     \
        case n: for (k = 0; k <= 159; k++) \
                        float_s[k] = (float)    \
                                (s[k] = GSM_MULT_R(s[k], 16384 >> (n-1)));\
				break;
# else 
#   define SCALE(n)     \
        case n: for (k = 0; k <= 159; k++) \
                        s[k] = GSM_MULT_R( s[k], 16384 >> (n-1) );\
				break;
# endif /* USE_FLOAT_MUL */

				switch (scalauto) {
				SCALE(1)
				SCALE(2)
				SCALE(3)
				SCALE(4)
				}
# undef SCALE
		}
# ifdef USE_FLOAT_MUL
		else for (k = 0; k <= 159; k++) float_s[k] = (float) s[k];
# endif

		/*	Compute the L_ACF[..].
		 */
		{
# ifdef USE_FLOAT_MUL
				register float * sp = float_s;
				register float	 sl = *sp;
# else
				word  * sp = s;
				word	sl = *sp;
# endif

#		define STEP(k)	 L_ACF[k] += (longword)((longword) sl * sp[ -(k) ]);
#		define NEXTI	 sl = *++sp


		for (k = 9; k--; L_ACF[k] = 0) ;

		STEP (0);
		NEXTI;
		STEP(0); STEP(1);
		NEXTI;
		STEP(0); STEP(1); STEP(2);
		NEXTI;
		STEP(0); STEP(1); STEP(2); STEP(3);
		NEXTI;
		STEP(0); STEP(1); STEP(2); STEP(3); STEP(4);
		NEXTI;
		STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5);
		NEXTI;
		STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5); STEP(6);
		NEXTI;
		STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5); STEP(6); STEP(7);

		for (i = 8; i <= 159; i++) {

				NEXTI;

				STEP(0);
				STEP(1); STEP(2); STEP(3); STEP(4);
				STEP(5); STEP(6); STEP(7); STEP(8);
		}

		for (k = 9; k--; L_ACF[k] <<= 1) ; 

		}
		/*	 Rescaling of the array s[0..159]
		 */
		if (scalauto > 0) {
				assert(scalauto <= 4); 
				for (k = 160; k--; *s++ <<= scalauto) ;
		}
}

#if defined(USE_FLOAT_MUL) && defined(FAST)

static void Fast_Autocorrelation P2((s, L_ACF),
		word * s,				/* [0..159] 	IN/OUT	*/
		longword * L_ACF)		/* [0..8]		OUT 	*/
{
		register int	k, i;
		float f_L_ACF[9];
		float scale;

		float		   s_f[160];
		register float *sf = s_f;

		for (i = 0; i < 160; ++i) sf[i] = s[i];
		for (k = 0; k <= 8; k++) {
				register float L_temp2 = 0;
				register float *sfl = sf - k;
				for (i = k; i < 160; ++i) L_temp2 += sf[i] * sfl[i];
				f_L_ACF[k] = L_temp2;
		}
		scale = MAX_LONGWORD / f_L_ACF[0];

		for (k = 0; k <= 8; k++) {
				L_ACF[k] = f_L_ACF[k] * scale;
		}
}
#endif	/* defined (USE_FLOAT_MUL) && defined (FAST) */

/* 4.2.5 */

static void Reflection_coefficients P2( (L_ACF, r),
		longword		* L_ACF,				/* 0...8		IN		*/
		register word	* r 					/* 0...7		OUT 	*/
)
{
		register int	i, m, n;
		register word	temp;
		register longword ltmp;
		word			ACF[9]; /* 0..8 */
		word			P[	9]; /* 0..8 */
		word			K[	9]; /* 2..8 */

		/*	Schur recursion with 16 bits arithmetic.
		 */

		if (L_ACF[0] == 0) {	/* everything is the same. */
				for (i = 8; i--; *r++ = 0) ;
				return;
		}

		assert( L_ACF[0] != 0 );
		temp = gsm_norm( L_ACF[0] );

		assert(temp >= 0 && temp < 32);

		/* ? overflow ? */
		for (i = 0; i <= 8; i++) ACF[i] = ((word) (SASR( L_ACF[i] << temp, 16 )));

		/*	 Initialize array P[..] and K[..] for the recursion.
		 */

		for (i = 1; i <= 7; i++) K[ i ] = ACF[ i ];
		for (i = 0; i <= 8; i++) P[ i ] = ACF[ i ];

		/*	 Compute reflection coefficients
		 */
		for (n = 1; n <= 8; n++, r++) {

				temp = P[1];
				temp = GSM_ABS(temp);
				if (P[0] < temp) {
						for (i = n; i <= 8; i++) *r++ = 0;
						return;
				}

				*r = gsm_div( temp, P[0] );

				assert(*r >= 0);
				if (P[1] > 0) *r = -*r; 		/* r[n] = sub(0, r[n]) */
				assert (*r != MIN_WORD);
				if (n == 8) return; 

				/*	Schur recursion
				 */
				temp = GSM_MULT_R( P[1], *r );
				P[0] = (word) GSM_ADD( P[0], temp );

				for (m = 1; m <= 8 - n; m++) {
						temp	 = GSM_MULT_R( K[ m   ],	*r );
						P[m]	 = (word) GSM_ADD(    P[ m+1 ],  temp );

						temp	 = GSM_MULT_R( P[ m+1 ],	*r );
						K[m]	 = (word) GSM_ADD(    K[ m   ],  temp );
				}
		}
}

/* 4.2.6 */

static void Transformation_to_Log_Area_Ratios P1((r),
		register word	* r 					/* 0..7    IN/OUT */
)
/*
 *	The following scaling for r[..] and LAR[..] has been used:
 *
 *	r[..]	= integer( real_r[..]*32768. ); -1 <= real_r < 1.
 *	LAR[..] = integer( real_LAR[..] * 16384 );
 *	with -1.625 <= real_LAR <= 1.625
 */
{
		register word	temp;
		register int	i;


		/* Computation of the LAR[0..7] from the r[0..7]
		 */
		for (i = 1; i <= 8; i++, r++) {

				temp = *r;
				temp = GSM_ABS(temp);
				assert(temp >= 0);

				if (temp < 22118) {
						temp >>= 1;
				} else if (temp < 31130) {
						assert( temp >= 11059 );
						temp -= 11059;
				} else {
						assert( temp >= 26112 );
						temp -= 26112;
						temp <<= 2;
				}

				*r = *r < 0 ? -temp : temp;
				assert( *r != MIN_WORD );
		}
}

/* 4.2.7 */

static void Quantization_and_coding P1((LAR),
		register word * LAR 	/* [0..7]		IN/OUT	*/
)
{
		register word	temp;
		longword		ltmp;
//		ulongword		utmp;


		/*	This procedure needs four tables; the following equations
		 *	give the optimum scaling for the constants:
		 *	
		 *	A[0..7] = integer( real_A[0..7] * 1024 )
		 *	B[0..7] = integer( real_B[0..7] *  512 )
		 *	MAC[0..7] = maximum of the LARc[0..7]
		 *	MIC[0..7] = minimum of the LARc[0..7]
		 */

#		undef STEP
#       define  STEP( A, B, MAC, MIC )          \
                temp = (word) GSM_MULT( A,   *LAR );   \
                temp = (word) GSM_ADD(  temp,   B );   \
                temp = (word) GSM_ADD(  temp, 256 );   \
                temp = SASR(     temp,   9 );   \
                *LAR  =  temp>MAC ? MAC - MIC : (temp<MIC ? 0 : temp - MIC); \
				LAR++;

		STEP(  20480,	  0,  31, -32 );
		STEP(  20480,	  0,  31, -32 );
		STEP(  20480,  2048,  15, -16 );
		STEP(  20480, -2560,  15, -16 );

		STEP(  13964,	 94,   7,  -8 );
		STEP(  15360, -1792,   7,  -8 );
		STEP(	8534,  -341,   3,  -4 );
		STEP(	9036, -1144,   3,  -4 );

#		undef	STEP
}

void Gsm_LPC_Analysis P3((S, s,LARc),
		struct gsm_state *S,
		word			 * s,			/* 0..159 signals		IN/OUT	*/
        word             * LARc)        /* 0..7   LARc's        OUT     */
{
		longword		L_ACF[9];

#if defined(USE_FLOAT_MUL) && defined(FAST)
		if (S->fast) Fast_Autocorrelation (s,	  L_ACF );
		else
#endif
		Autocorrelation 				  (s,	  L_ACF );
		Reflection_coefficients 		  (L_ACF, LARc	);
		Transformation_to_Log_Area_Ratios (LARc);
		Quantization_and_coding 		  (LARc);
}
