/*************************************************************************/
/*                                                                       */
/* Copyright (c) 2000-2004 Linuos Design                                 */
/*                                     領(lǐng)馳設(shè)計(jì)中心  版權(quán)所有 2000-2004  */
/*                                                                       */
/* PROPRIETARY RIGHTS of Linuos Design  are involved in the subject      */
/* matter of this material.  All manufacturing, reproduction, use, and   */
/* sales rights pertaining to this subject matter are governed by the    */
/* license agreement.  The recipient of this software implicitly accepts */ 
/* the terms of the license.                                             */
/* 本軟件文檔資料是領(lǐng)馳設(shè)計(jì)中心的資產(chǎn),任何人士閱讀和使用本資料必須獲得   */
/* 相應(yīng)的書(shū)面授權(quán),承擔(dān)保密責(zé)任和接受相應(yīng)的法律約束.                      */
/*                                                                       */
/*************************************************************************/

/*
* ===================================================================
*  TS 26.104
*  REL-5 V5.4.0 2004-03
*  REL-6 V6.1.0 2004-03
*  3GPP AMR Floating-point Speech Codec
* ===================================================================
*
*/

/*
* sp_enc.c
*
*
* Project:
*    AMR Floating-Point Codec
*
* Contains:
*    This module contains all the functions needed encoding 160
*    16-bit speech samples to AMR encoder parameters.
*
*/
//#include <stdlib.h>
//#include <stdio.h>
//#include <memory.h>
#include "stdafx.h"
#include <math.h>
#include <float.h>
//#include "sp_enc.h"
//#include "rom_enc.h"


/*
* Exponential Window coefficients used to weight the autocorrelation
* coefficients for 60 Hz bandwidth expansion of high pitched voice
* before Levinson-Durbin recursion to compute the LPC coefficients.
*
* lagwindow[i] =  exp( -0.5*(2*pi*F0*(i+1)/Fs)^2 ); i = 0,...,9
* F0 = 60 Hz, Fs = 8000 Hz
*/
static float lag_wind[LP_ORDER] =
{
	0.99889028F,
		0.99556851F,
		0.99005681F,
		0.98239160F,
		0.97262347F,
		0.96081644F,
		0.94704735F,
		0.93140495F,
		0.91398895F,
		0.89490914F
};


/* 1/6 resolution interpolation filter  (-3 dB at 3600 Hz)
* Note: the 1/3 resolution filter is simply a subsampled
*       version of the 1/6 resolution filter, i.e. it uses
*       every second coefficient:
*
*       inter_6(1/3)[k] = inter_6(1/3)[2*k], 0 <= k <= 3*L_INTER10
*/
static float b60[UP_SAMP_MAX*(INTERPOL_LEN-1)+1] =
{
	0.898529F,
		0.865051F,
		0.769257F,
		0.624054F,
		0.448639F,
		0.265289F,
		0.0959167F,
		- 0.0412598F,
		- 0.134338F,
		- 0.178986F,
		- 0.178528F,
		- 0.142609F,
		- 0.0849304F,
		- 0.0205078F,
		0.0369568F,
		0.0773926F,
		0.0955200F,
		0.0912781F,
		0.0689392F,
		0.0357056F,
		0.000000F,
		- 0.0305481F,
		- 0.0504150F,
		- 0.0570068F,
		- 0.0508423F,
		- 0.0350037F,
		- 0.0141602F,
		0.00665283F,
		0.0230713F,
		0.0323486F,
		0.0335388F,
		0.0275879F,
		0.0167847F,
		0.00411987F,
		- 0.00747681F,
		- 0.0156860F,
		- 0.0193481F,
		- 0.0183716F,
		- 0.0137634F,
		- 0.00704956F,
		0.000000F,
		0.00582886F,
		0.00939941F,
		0.0103760F,
		0.00903320F,
		0.00604248F,
		0.00238037F,
		- 0.00109863F,
		- 0.00366211F,
		- 0.00497437F,
		- 0.00503540F,
		- 0.00402832F,
		- 0.00241089F,
		- 0.000579834F,
		0.00103760F,
		0.00222778F,
		0.00277710F,
		0.00271606F,
		0.00213623F,
		0.00115967F,
		0.000000F
};


/* track table for algebraic code book search (MR475, MR515) */
static INT8 trackTable[4 * 5] =
{
	/* subframe 1; track to code; -1 do not code this position */ 0,
	1,
		0,
		1,
		- 1,
		/* subframe 2 */ 0,
		- 1,
		1,
		0,
		1,
		/* subframe 3 */ 0,
		1,
		0,
		- 1,
		1,
		/* subframe 4 */ 0,
		1,
		- 1,
		0,
		1
};



/*
* Dotproduct40
*
*
* Parameters:
*    x                 I: First input
*    y                 I: Second input
* Function:
*    Computes dot product size 40
*
* Returns:
*    acc                dot product
*/
static double Dotproduct40( float *x, float *y )
{
	double acc;
	
	
	acc = x[0] * y[0] + x[1] * y[1] + x[2] * y[2] + x[3] * y[3];
	acc += x[4] * y[4] + x[5] * y[5] + x[6] * y[6] + x[7] * y[7];
	acc += x[8] * y[8] + x[9] * y[9] + x[10] * y[10] + x[11] * y[11];
	acc += x[12] * y[12] + x[13] * y[13] + x[14] * y[14] + x[15] * y[15];
	acc += x[16] * y[16] + x[17] * y[17] + x[18] * y[18] + x[19] * y[19];
	acc += x[20] * y[20] + x[21] * y[21] + x[22] * y[22] + x[23] * y[23];
	acc += x[24] * y[24] + x[25] * y[25] + x[26] * y[26] + x[27] * y[27];
	acc += x[28] * y[28] + x[29] * y[29] + x[30] * y[30] + x[31] * y[31];
	acc += x[32] * y[32] + x[33] * y[33] + x[34] * y[34] + x[35] * y[35];
	acc += x[36] * y[36] + x[37] * y[37] + x[38] * y[38] + x[39] * y[39];
	return( acc );
}


/*
* Autocorr
*
*
* Parameters:
*    x                 I: Input signal
*    r                 O: Autocorrelations
*    wind              I: Window for LPC analysis
* Function:
*    Calculate autocorrelation with window, LPC order = LP_ORDER
*
* Returns:
*    void
*/
static void Autocorr( float x[], float r[], const float wind[] )
{
	INT32 i, j;   /* Counters */
	float y[LP_WINDOW + LP_ORDER + 1];   /* Windowed signal */
	double sum;   /* temp */
	
	
				   /*
				   * Windowing of signal
    */
	for ( i = 0; i < LP_WINDOW; i++ ) {
		y[i] = x[i] * wind[i];
	}
	
	/*
    * Zero remaining memory
    */
	memset( &y[LP_WINDOW], 0, 44 );
	
	/*
    * Autocorrelation
    */
	for ( i = 0; i <= LP_ORDER; i++ ) {
		sum = 0;
		
		for ( j = 0; j < LP_WINDOW; j += 40 ) {
			sum += Dotproduct40( &y[j], &y[j + i] );
		}
		r[i] = (float)sum;
	}
}


/*
* Levinson
*
*
* Parameters:
*    old_A             I: Vector of old LP coefficients [LP_ORDER+1]
*    r                 I: Vector of autocorrelations    [LP_ORDER+1]
*    a                 O: LP coefficients               [LP_ORDER+1]
*    rc                O: Reflection coefficients       [4]
* Function:
*    Levinson-Durbin algorithm
*
* Returns:
*    void
*
*/
static void Levinson( float *old_A, float *r, float *A, float *rc )
{
	float sum, at, err;
	INT32 l, j, i;
	float rct[LP_ORDER];   /* temporary reflection coefficients  0,...,m-1 */
	
	
	rct[0] = ( -r[1] ) / r[0];
	A[0] = 1.0F;
	A[1] = rct[0];
	err = r[0] + r[1] * rct[0];
	
	if ( err <= 0.0 )
		err = 0.01F;
	
	for ( i = 2; i <= LP_ORDER; i++ ) {
		sum = 0.0F;
		
		for ( j = 0; j < i; j++ )
			sum += r[i - j] * A[j];
		rct[i - 1] = ( -sum ) / ( err );
		
		for ( j = 1; j <= ( i / 2 ); j++ ) {
			l = i - j;
			at = A[j] + rct[i - 1] *A[l];
			A[l] += rct[i - 1] *A[j];
			A[j] = at;
		}
		A[i] = rct[i - 1];
		err += rct[i - 1] *sum;
		
		if ( err <= 0.0 )
			err = 0.01F;
	}
	memcpy( rc, rct, 4 * sizeof( float ) );
	memcpy( old_A, A, LP_ORDER_PLUS * sizeof( float ) );
}


/*
* lpc
*
*
* Parameters:
*    old_A             O: Vector of old LP coefficients [LP_ORDER+1]
*    x                 I: Input signal
*    x_12k2            I: Input signal 12.2k
*    a                 O: predictor coefficients
*    mode              I: AMR mode
* Function:
*    LP analysis
*
*    In 12.2 kbit/s mode linear prediction (LP) analysis is performed
*    twice per speech frame using the auto-correlation approach with
*    30 ms asymmetric windows. No lookahead is used in
*    the auto-correlation computation.
*
*    In other modes analysis is performed once per speech frame
*    using the auto-correlation approach with 30 ms asymmetric windows.
*    A lookahead of 40 samples (5 ms) is used in the auto-correlation computation.
*
*    The auto-correlations of windowed speech are converted to the LP
*    coefficients using the Levinson-Durbin algorithm.
*    Then the LP coefficients are transformed to the Line Spectral Pair
*    (LSP) domain  for quantization and interpolation purposes.
*    The interpolated quantified and unquantized filter coefficients
*    are converted back to the LP filter coefficients
*    (to construct the synthesis and weighting filters at each subframe).
*
* Returns:
*    void
*
*/
static void lpc( float *old_A, float x[], float x_12k2[], float a[], enum Mode
				mode )
{
	INT32 i;
	float r[LP_ORDER_PLUS];
	float rc[4];
	
	
	if ( mode == MR122 ) {
		Autocorr( x_12k2, r, window_160_80 );
		
		/*
		* Lag windowing
		*/
		for ( i = 1; i <= LP_ORDER; i++ ) {
			r[i] = r[i] * lag_wind[i - 1];
		}
		r[0] *= 1.0001F;
		
		if ( r[0] < 1.0F )
			r[0] = 1.0F;
		
			/*
			* Levinson Durbin
		*/
		Levinson( old_A, r, &a[LP_ORDER_PLUS], rc );
		
		/*
		* Autocorrelations
		*/
		Autocorr( x_12k2, r, window_232_8 );
		
		/*
		* Lag windowing
		*/
		for ( i = 1; i <= LP_ORDER; i++ ) {
			r[i] = r[i] * lag_wind[i - 1];
		}
		r[0] *= 1.0001F;
		
		if ( r[0] < 1.0F )
			r[0] = 1.0F;
		
			/*
			* Levinson Durbin
		*/
		Levinson( old_A, r, &a[LP_ORDER_PLUS * 3], rc );
	}
	else {
	/*
	* Autocorrelations
		*/
		Autocorr( x, r, window_200_40 );
		
		/*
		* a 60 Hz bandwidth expansion is used by lag windowing
		* the auto-correlations. Further, auto-correlation[0] is
		* multiplied by the white noise correction factor 1.0001
		* which is equivalent to adding a noise floor at -40 dB.
		*/
		for ( i = 1; i <= LP_ORDER; i++ ) {
			r[i] = r[i] * lag_wind[i - 1];
		}
		r[0] *= 1.0001F;
		
		if ( r[0] < 1.0F )
			r[0] = 1.0F;
		
			/*
			* Levinson Durbin
		*/
		Levinson( old_A, r, &a[LP_ORDER_PLUS * 3], rc );
	}
}


/*
* Chebps
*
*
* Parameters:
*    x                 I: Cosine value for the freqency given
*    f                 I: angular freqency
* Function:
*    Evaluates the Chebyshev polynomial series
*
* Returns:
*    result of polynomial evaluation
*/
static float Chebps( float x, float f[] )
{
	float b0, b1, b2, x2;
	INT32 i;
	
	
	x2 = 2.0F * x;
	b2 = 1.0F;
	b1 = x2 + f[1];
	
	for ( i = 2; i < 5; i++ ) {
		b0 = x2 * b1 - b2 + f[i];
		b2 = b1;
		b1 = b0;
	}
	return( x * b1 - b2 + f[i] );
}


/*
* Az_lsp
*
*
* Parameters:
*    a                 I: Predictor coefficients              [LP_ORDER_PLUS]
*    lsp               O: Line spectral pairs                 [LP_ORDER]
*    old_lsp           I: Old lsp, in case not found 10 roots [LP_ORDER]
*
* Function:
*    LP to LSP conversion
*
*    The LP filter coefficients A[], are converted to the line spectral pair
*    (LSP) representation for quantization and interpolation purposes.
*
* Returns:
*    void
*/
static void Az_lsp( float a[], float lsp[], float old_lsp[] )
{
	INT32 i, j, nf, ip;
	float xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
	float y;
	float *coef;
	float f1[6], f2[6];
	
	
	/*
    *  find the sum and diff. pol. F1(z) and F2(z)
    */
	f1[0] = 1.0F;
	f2[0] = 1.0F;
	
	for ( i = 0; i < ( LP_ORDER_BY2 ); i++ ) {
		f1[i + 1] = a[i + 1] +a[LP_ORDER - i] - f1[i];
		f2[i + 1] = a[i + 1] -a[LP_ORDER - i] + f2[i];
	}
	f1[LP_ORDER_BY2] *= 0.5F;
	f2[LP_ORDER_BY2] *= 0.5F;
	
	/*
    * find the LSPs using the Chebychev pol. evaluation
    */
	nf = 0;   /* number of found frequencies */
	ip = 0;   /* indicator for f1 or f2 */
	coef = f1;
	xlow = grid[0];
	ylow = Chebps( xlow, coef );
	j = 0;
	
	while ( ( nf < LP_ORDER ) && ( j < 60 ) ) {
		j++;
		xhigh = xlow;
		yhigh = ylow;
		xlow = grid[j];
		ylow = Chebps( xlow, coef );
		
		if ( ylow * yhigh <= 0 ) {
			/* divide 4 times the interval */
			for ( i = 0; i < 4; i++ ) {
				xmid = ( xlow + xhigh ) * 0.5F;
				ymid = Chebps( xmid, coef );
				
				if ( ylow * ymid <= 0.0F ) {
					yhigh = ymid;
					xhigh = xmid;
				}
				else {
					ylow = ymid;
					xlow = xmid;
				}
			}
			
			/*
			* Linear interpolation
			* xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow)
			*/
			y = yhigh - ylow;
			
			if ( y == 0 ) {
				xint = xlow;
			}
			else {
				y = ( xhigh - xlow ) / ( yhigh - ylow );
				xint = xlow - ylow * y;
			}
			lsp[nf] = xint;
			xlow = xint;
			nf++;
			
			if ( ip == 0 ) {
				ip = 1;
				coef = f2;
			}
			else {
				ip = 0;
				coef = f1;
			}
			ylow = Chebps( xlow, coef );
		}
	}
	
	/* Check if LP_ORDER roots found */
	if ( nf < LP_ORDER ) 
	{
		memcpy( lsp, old_lsp, LP_ORDER <<2 );
	}
	return;
}