/*
 * Copyright (c) 1991-1994 Regents of the University of California.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by the Computer Systems
 *	Engineering Group at Lawrence Berkeley Laboratory.
 * 4. Neither the name of the University nor of the Laboratory may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

/*
 * These routines provide the linkage between vat & the TU-Berlin GSM
 * encoder/decoder.  These routines could be avoided but there would
 * be extra ulaw-to-linear and linear-to-ulaw steps.
 */

static const char rcsid[] =
    "@(#) $Header: /work/projects/tove/cvs/src/testing/vat/vat-gsm.c,v 1.1 1997/12/08 17:22:55 parnanen Exp $ (LBL)";

/*XXX*/
#define SASR 1

#include "config.h"
#include <string.h>
#include "private.h"
#include "gsm.h"
#include "mulaw.h"

static void preprocess_ulaw(gsm_state* S, const ulaw_byte* s, word* so)
{
	word       z1 = S->z1;
	longword L_z2 = S->L_z2;
	word 	   mp = S->mp;

	word 	   	s1;
	longword      L_s2;

	longword      L_temp;

	word		msp, lsp;
	word		SO;

	longword	ltmp;		/* for   ADD */
	ulongword	utmp;		/* for L_ADD */

	register int		k = 160;

	for (k = 160; --k >= 0; ) {

		/*
		 * 4.2.1   Downscaling of the input signal
		 */
		SO = SASR( *s, 3 ) << 2;
		SO = SASR(mulawtolin[*s++], 3) << 2;


		/*  4.2.2   Offset compensation
		 * 
		 *  This part implements a high-pass filter and
		 *  requires extended arithmetic precision for
		 *  the recursive part of this filter.	The input
		 *  of this procedure is the array so[0...159]
		 *  and the output the array sof[ 0...159 ].
		 *
		 *   Compute the non-recursive part
		 */

		s1 = SO - z1;			/* s1 = gsm_sub( *so, z1 ); */
		z1 = SO;

		/*   Compute the recursive part */
		L_s2 = s1;
		L_s2 <<= 15;

		/*   do a 31 by 16 bit multiplication */

		msp = SASR( L_z2, 15 );
		lsp = L_z2-((longword)msp<<15);

		L_s2  += GSM_MULT_R( lsp, 32735 );
		L_temp = (longword)msp * 32735;
		L_z2   = GSM_L_ADD( L_temp, L_s2 );

		/*    Compute sof[k] with rounding */
		L_temp = GSM_L_ADD( L_z2, 16384 );

		/*   4.2.3  Preemphasis
		 */
		msp   = GSM_MULT_R( mp, -28180 );
		mp    = SASR( L_temp, 15 );
		*so++ = GSM_ADD( mp, msp );
	}

	S->z1   = z1;
	S->L_z2 = L_z2;
	S->mp   = mp;
}


void Vat_Gsm_Encoder(gsm_state* S, const ulaw_byte* s, gsm_byte* c)
{
	word	 	LARc[8], Nc[4], Mc[4], bc[4], xmaxc[4], xmc[13*4];
	int	k;
	word	* dp  = S->dp0 + 120;	/* [ -120...-1 ] */
	word	* dpp = dp;		/* [ 0...39 ]	 */

	static word	e [50] = {0};

	word	so[160];

	preprocess_ulaw		(S, s, so);

	Gsm_LPC_Analysis		(S, so, LARc);
	Gsm_Short_Term_Analysis_Filter	(S, LARc, so);

	for (k = 0; k <= 3; k++) {

		Gsm_Long_Term_Predictor	(S,
					 so+k*40, /* d      [0..39] IN	*/
					 dp,	  /* dp  [-120..-1] IN	*/
					e + 5,	  /* e      [0..39] OUT	*/
					dpp,	  /* dpp    [0..39] OUT */
					 &Nc[k],
					 &bc[k]);

		Gsm_RPE_Encoding	( S,
					e + 5,	/* e	  ][0..39][ IN/OUT */
					  &xmaxc[k], &Mc[k], &xmc[13*k]);

		/*
		 * Gsm_Update_of_reconstructed_short_time_residual_signal
		 *			( dpp, e + 5, dp );
		 */

		{ register int i;
		  register longword ltmp;
		  for (i = 0; i <= 39; i++)
			dp[ i ] = GSM_ADD( e[5 + i], dpp[i] );
		}
		dp  += 40;
		dpp += 40;

	}
	(void)memcpy( S->dp0, S->dp0 + 160, 120 * sizeof(*S->dp0) );

	*c++ =   ((GSM_MAGIC & 0xF) << 4)		/* 1 */
	       | ((LARc[0] >> 2) & 0xF);
	*c++ =   ((LARc[0] & 0x3) << 6)
	       | (LARc[1] & 0x3F);
	*c++ =   ((LARc[2] & 0x1F) << 3)
	       | ((LARc[3] >> 2) & 0x7);
	*c++ =   ((LARc[3] & 0x3) << 6)
	       | ((LARc[4] & 0xF) << 2)
	       | ((LARc[5] >> 2) & 0x3);
	*c++ =   ((LARc[5] & 0x3) << 6)
	       | ((LARc[6] & 0x7) << 3)
	       | (LARc[7] & 0x7);
	*c++ =   ((Nc[0] & 0x7F) << 1)
	       | ((bc[0] >> 1) & 0x1);
	*c++ =   ((bc[0] & 0x1) << 7)
	       | ((Mc[0] & 0x3) << 5)
	       | ((xmaxc[0] >> 1) & 0x1F);
	*c++ =   ((xmaxc[0] & 0x1) << 7)
	       | ((xmc[0] & 0x7) << 4)
	       | ((xmc[1] & 0x7) << 1)
	       | ((xmc[2] >> 2) & 0x1);
	*c++ =   ((xmc[2] & 0x3) << 6)
	       | ((xmc[3] & 0x7) << 3)
	       | (xmc[4] & 0x7);
	*c++ =   ((xmc[5] & 0x7) << 5)			/* 10 */
	       | ((xmc[6] & 0x7) << 2)
	       | ((xmc[7] >> 1) & 0x3);
	*c++ =   ((xmc[7] & 0x1) << 7)
	       | ((xmc[8] & 0x7) << 4)
	       | ((xmc[9] & 0x7) << 1)
	       | ((xmc[10] >> 2) & 0x1);
	*c++ =   ((xmc[10] & 0x3) << 6)
	       | ((xmc[11] & 0x7) << 3)
	       | (xmc[12] & 0x7);
	*c++ =   ((Nc[1] & 0x7F) << 1)
	       | ((bc[1] >> 1) & 0x1);
	*c++ =   ((bc[1] & 0x1) << 7)
	       | ((Mc[1] & 0x3) << 5)
	       | ((xmaxc[1] >> 1) & 0x1F);
	*c++ =   ((xmaxc[1] & 0x1) << 7)
	       | ((xmc[13] & 0x7) << 4)
	       | ((xmc[14] & 0x7) << 1)
	       | ((xmc[15] >> 2) & 0x1);
	*c++ =   ((xmc[15] & 0x3) << 6)
	       | ((xmc[16] & 0x7) << 3)
	       | (xmc[17] & 0x7);
	*c++ =   ((xmc[18] & 0x7) << 5)
	       | ((xmc[19] & 0x7) << 2)
	       | ((xmc[20] >> 1) & 0x3);
	*c++ =   ((xmc[20] & 0x1) << 7)
	       | ((xmc[21] & 0x7) << 4)
	       | ((xmc[22] & 0x7) << 1)
	       | ((xmc[23] >> 2) & 0x1);
	*c++ =   ((xmc[23] & 0x3) << 6)
	       | ((xmc[24] & 0x7) << 3)
	       | (xmc[25] & 0x7);
	*c++ =   ((Nc[2] & 0x7F) << 1)			/* 20 */
	       | ((bc[2] >> 1) & 0x1);
	*c++ =   ((bc[2] & 0x1) << 7)
	       | ((Mc[2] & 0x3) << 5)
	       | ((xmaxc[2] >> 1) & 0x1F);
	*c++ =   ((xmaxc[2] & 0x1) << 7)
	       | ((xmc[26] & 0x7) << 4)
	       | ((xmc[27] & 0x7) << 1)
	       | ((xmc[28] >> 2) & 0x1);
	*c++ =   ((xmc[28] & 0x3) << 6)
	       | ((xmc[29] & 0x7) << 3)
	       | (xmc[30] & 0x7);
	*c++ =   ((xmc[31] & 0x7) << 5)
	       | ((xmc[32] & 0x7) << 2)
	       | ((xmc[33] >> 1) & 0x3);
	*c++ =   ((xmc[33] & 0x1) << 7)
	       | ((xmc[34] & 0x7) << 4)
	       | ((xmc[35] & 0x7) << 1)
	       | ((xmc[36] >> 2) & 0x1);
	*c++ =   ((xmc[36] & 0x3) << 6)
	       | ((xmc[37] & 0x7) << 3)
	       | (xmc[38] & 0x7);
	*c++ =   ((Nc[3] & 0x7F) << 1)
	       | ((bc[3] >> 1) & 0x1);
	*c++ =   ((bc[3] & 0x1) << 7)
	       | ((Mc[3] & 0x3) << 5)
	       | ((xmaxc[3] >> 1) & 0x1F);
	*c++ =   ((xmaxc[3] & 0x1) << 7)
	       | ((xmc[39] & 0x7) << 4)
	       | ((xmc[40] & 0x7) << 1)
	       | ((xmc[41] >> 2) & 0x1);
	*c++ =   ((xmc[41] & 0x3) << 6)			/* 30 */
	       | ((xmc[42] & 0x7) << 3)
	       | (xmc[43] & 0x7);
	*c++ =   ((xmc[44] & 0x7) << 5)
	       | ((xmc[45] & 0x7) << 2)
	       | ((xmc[46] >> 1) & 0x3);
	*c++ =   ((xmc[46] & 0x1) << 7)
	       | ((xmc[47] & 0x7) << 4)
	       | ((xmc[48] & 0x7) << 1)
	       | ((xmc[49] >> 2) & 0x1);
	*c++ =   ((xmc[49] & 0x3) << 6)
	       | ((xmc[50] & 0x7) << 3)
	       | (xmc[51] & 0x7);
}


static void Postprocessing(gsm_state* S, word* s, unsigned char* u)
{
#ifdef USE_FLOAT_MUL
	register int		k;
	register float		msr = S->msr;
	register float		scalef = 28180./32768.;
	register short		samp;

	for (k = 160; --k >= 0; ) {
		register float sf = *s++;
		msr = sf + msr * scalef;  /* Deemphasis */
		samp = msr + msr;
		*u++ = lintomulaw[samp & 0xfff8];
	}
	S->msr = msr;
#else
	register int		k;
	register word		msr = S->msr;
	register longword	ltmp;	/* for GSM_ADD */

	for (k = 160; --k >= 0; ) {
		register short samp = *s++;
		/* Deemphasis */
		msr = GSM_ADD(samp, GSM_MULT_R(msr, 28180));
		/* Truncation & Upscaling */
		*u++  = lintomulaw[GSM_ADD(msr, msr) & 0xFFF8];
	}
	S->msr = msr;
#endif
}


void Vat_Gsm_Decoder(gsm_state* S, ulaw_byte* u, const gsm_byte* c)
{
	int		j, k;
	word		erp[40], wt[160], s[160];
	word		* drp = S->dp0 + 120;
	word  	LARc[8], Nc[4], Mc[4], bc[4], xmaxc[4], xmc[13*4];

	/* GSM_MAGIC  = (*c >> 4) & 0xF; */

	if (((*c >> 4) & 0x0F) != GSM_MAGIC) return;

	LARc[0]  = (*c++ & 0xF) << 2;		/* 1 */
	LARc[0] |= (*c >> 6) & 0x3;
	LARc[1]  = *c++ & 0x3F;
	LARc[2]  = (*c >> 3) & 0x1F;
	LARc[3]  = (*c++ & 0x7) << 2;
	LARc[3] |= (*c >> 6) & 0x3;
	LARc[4]  = (*c >> 2) & 0xF;
	LARc[5]  = (*c++ & 0x3) << 2;
	LARc[5] |= (*c >> 6) & 0x3;
	LARc[6]  = (*c >> 3) & 0x7;
	LARc[7]  = *c++ & 0x7;
	Nc[0]  = (*c >> 1) & 0x7F;
	bc[0]  = (*c++ & 0x1) << 1;
	bc[0] |= (*c >> 7) & 0x1;
	Mc[0]  = (*c >> 5) & 0x3;
	xmaxc[0]  = (*c++ & 0x1F) << 1;
	xmaxc[0] |= (*c >> 7) & 0x1;
	xmc[0]  = (*c >> 4) & 0x7;
	xmc[1]  = (*c >> 1) & 0x7;
	xmc[2]  = (*c++ & 0x1) << 2;
	xmc[2] |= (*c >> 6) & 0x3;
	xmc[3]  = (*c >> 3) & 0x7;
	xmc[4]  = *c++ & 0x7;
	xmc[5]  = (*c >> 5) & 0x7;
	xmc[6]  = (*c >> 2) & 0x7;
	xmc[7]  = (*c++ & 0x3) << 1;		/* 10 */
	xmc[7] |= (*c >> 7) & 0x1;
	xmc[8]  = (*c >> 4) & 0x7;
	xmc[9]  = (*c >> 1) & 0x7;
	xmc[10]  = (*c++ & 0x1) << 2;
	xmc[10] |= (*c >> 6) & 0x3;
	xmc[11]  = (*c >> 3) & 0x7;
	xmc[12]  = *c++ & 0x7;
	Nc[1]  = (*c >> 1) & 0x7F;
	bc[1]  = (*c++ & 0x1) << 1;
	bc[1] |= (*c >> 7) & 0x1;
	Mc[1]  = (*c >> 5) & 0x3;
	xmaxc[1]  = (*c++ & 0x1F) << 1;
	xmaxc[1] |= (*c >> 7) & 0x1;
	xmc[13]  = (*c >> 4) & 0x7;
	xmc[14]  = (*c >> 1) & 0x7;
	xmc[15]  = (*c++ & 0x1) << 2;
	xmc[15] |= (*c >> 6) & 0x3;
	xmc[16]  = (*c >> 3) & 0x7;
	xmc[17]  = *c++ & 0x7;
	xmc[18]  = (*c >> 5) & 0x7;
	xmc[19]  = (*c >> 2) & 0x7;
	xmc[20]  = (*c++ & 0x3) << 1;
	xmc[20] |= (*c >> 7) & 0x1;
	xmc[21]  = (*c >> 4) & 0x7;
	xmc[22]  = (*c >> 1) & 0x7;
	xmc[23]  = (*c++ & 0x1) << 2;
	xmc[23] |= (*c >> 6) & 0x3;
	xmc[24]  = (*c >> 3) & 0x7;
	xmc[25]  = *c++ & 0x7;
	Nc[2]  = (*c >> 1) & 0x7F;
	bc[2]  = (*c++ & 0x1) << 1;		/* 20 */
	bc[2] |= (*c >> 7) & 0x1;
	Mc[2]  = (*c >> 5) & 0x3;
	xmaxc[2]  = (*c++ & 0x1F) << 1;
	xmaxc[2] |= (*c >> 7) & 0x1;
	xmc[26]  = (*c >> 4) & 0x7;
	xmc[27]  = (*c >> 1) & 0x7;
	xmc[28]  = (*c++ & 0x1) << 2;
	xmc[28] |= (*c >> 6) & 0x3;
	xmc[29]  = (*c >> 3) & 0x7;
	xmc[30]  = *c++ & 0x7;
	xmc[31]  = (*c >> 5) & 0x7;
	xmc[32]  = (*c >> 2) & 0x7;
	xmc[33]  = (*c++ & 0x3) << 1;
	xmc[33] |= (*c >> 7) & 0x1;
	xmc[34]  = (*c >> 4) & 0x7;
	xmc[35]  = (*c >> 1) & 0x7;
	xmc[36]  = (*c++ & 0x1) << 2;
	xmc[36] |= (*c >> 6) & 0x3;
	xmc[37]  = (*c >> 3) & 0x7;
	xmc[38]  = *c++ & 0x7;
	Nc[3]  = (*c >> 1) & 0x7F;
	bc[3]  = (*c++ & 0x1) << 1;
	bc[3] |= (*c >> 7) & 0x1;
	Mc[3]  = (*c >> 5) & 0x3;
	xmaxc[3]  = (*c++ & 0x1F) << 1;
	xmaxc[3] |= (*c >> 7) & 0x1;
	xmc[39]  = (*c >> 4) & 0x7;
	xmc[40]  = (*c >> 1) & 0x7;
	xmc[41]  = (*c++ & 0x1) << 2;
	xmc[41] |= (*c >> 6) & 0x3;
	xmc[42]  = (*c >> 3) & 0x7;
	xmc[43]  = *c++ & 0x7;			/* 30  */
	xmc[44]  = (*c >> 5) & 0x7;
	xmc[45]  = (*c >> 2) & 0x7;
	xmc[46]  = (*c++ & 0x3) << 1;
	xmc[46] |= (*c >> 7) & 0x1;
	xmc[47]  = (*c >> 4) & 0x7;
	xmc[48]  = (*c >> 1) & 0x7;
	xmc[49]  = (*c++ & 0x1) << 2;
	xmc[49] |= (*c >> 6) & 0x3;
	xmc[50]  = (*c >> 3) & 0x7;
	xmc[51]  = *c & 0x7;			/* 33 */

	for (j=0; j <= 3; j++) {
		Gsm_RPE_Decoding( S, xmaxc[j], Mc[j], &xmc[13*j], erp );
		Gsm_Long_Term_Synthesis_Filtering( S, Nc[j], bc[j], erp, drp );

		for (k = 0; k < 40; k++)
			wt[ j * 40 + k ] =  drp[ k ];
	}

	Gsm_Short_Term_Synthesis_Filter( S, LARc, wt, s );
	Postprocessing(S, s, u);
}

void Vat_Gsm_InitS(struct gsm_state *S)
{
	memset(S, 0, sizeof(struct gsm_state));
	S->nrp = 40;
}

void Vat_Gsm_InitR(struct gsm_state *S)
{
	memset(S, 0, sizeof(struct gsm_state));
	S->nrp = 40;
}