/*
* Get_lsp_pol
*
*
* Parameters:
*    lsp                 I: line spectral frequencies
*    f                   O: polynomial F1(z) or F2(z)
*
* Function:
*    Find the polynomial F1(z) or F2(z) from the LSPs.
*
*    F1(z) = product ( 1 - 2 lsp[i] z^-1 + z^-2 )
*             i=0,2,4,6,8
*    F2(z) = product   ( 1 - 2 lsp[i] z^-1 + z^-2 )
*             i=1,3,5,7,9
*
*    where lsp[] is the LSP vector in the cosine domain.
*
*    The expansion is performed using the following recursion:
*
*    f[0] = 1
*    b = -2.0 * lsp[0]
*    f[1] = b
*    for i=2 to 5 do
*       b = -2.0 * lsp[2*i-2];
*       f[i] = b*f[i-1] + 2.0*f[i-2];
*       for j=i-1 down to 2 do
*          f[j] = f[j] + b*f[j-1] + f[j-2];
*       f[1] = f[1] + b;
*
* Returns:
*    void
*/
static void Get_lsp_pol( float *lsp, float *f )
{
	INT32 i, j;
	float T0;
	
	
	f[0] = 1.0F;
	f[1] = -2.0F * lsp[0];
	
	for ( i = 2; i <= 5; i++ ) {
		T0 = -2.0F * lsp[2 * i - 2];
		f[i] = ( float )( T0 * f[i - 1] +2.0F * f[i - 2] );
		
		for ( j = i - 1; j >= 2; j-- ) {
			f[j] = f[j] + T0 * f[j - 1] +f[j - 2];
		}
		f[1] = f[1] + T0;
	}
	return;
}


/*
* Lsp_Az
*
*
* Parameters:
*    lsp                 I: Line spectral frequencies
*    a                   O: Predictor coefficients
*
* Function:
*    Converts from the line spectral pairs (LSP) to LP coefficients,
*    for a 10th order filter.
*
* Returns:
*    void
*/
static void Lsp_Az( float lsp[], float a[] )
{
	float f1[6], f2[6];
	INT32 i, j;
	
	
	Get_lsp_pol( &lsp[0], f1 );
	Get_lsp_pol( &lsp[1], f2 );
	
	for ( i = 5; i > 0; i-- ) {
		f1[i] += f1[i - 1];
		f2[i] -= f2[i - 1];
	}
	a[0] = 1;
	
	for ( i = 1, j = 10; i <= 5; i++, j-- ) {
		a[i] = ( float )( ( f1[i] + f2[i] ) * 0.5F );
		a[j] = ( float )( ( f1[i] - f2[i] ) * 0.5F );
	}
	return;
}


/*
* Int_lpc_1and3_2
*
*
* Parameters:
*    lsp_old        I: LSP vector at the 4th subfr. of past frame      [LP_ORDER]
*    lsp_mid        I: LSP vector at the 2nd subframe of present frame [LP_ORDER]
*    lsp_new        I: LSP vector at the 4th subframe of present frame [LP_ORDER]
*    az             O: interpolated LP parameters in subframes 1 and 3
*                                                                [AZ_SIZE]
*
* Function:
*    Interpolation of the LPC parameters. Same as the Int_lpc
*    function but we do not recompute Az() for subframe 2 and
*	   4 because it is already available.
*
* Returns:
*    void
*/
static void Int_lpc_1and3_2( float lsp_old[], float lsp_mid[], float
							lsp_new[], float az[] )
{
	float lsp[LP_ORDER];
	INT32 i;
	
	
	for ( i = 0; i < LP_ORDER; i += 2 ) {
		lsp[i] = ( lsp_mid[i] + lsp_old[i] ) * 0.5F;
		lsp[i + 1] = ( lsp_mid[i + 1] +lsp_old[i+1] )*0.5F;
	}
	
	/* Subframe 1 */
	Lsp_Az( lsp, az );
	az += LP_ORDER_PLUS * 2;
	
	for ( i = 0; i < LP_ORDER; i += 2 ) {
		lsp[i] = ( lsp_mid[i] + lsp_new[i] ) * 0.5F;
		lsp[i + 1] = ( lsp_mid[i + 1] +lsp_new[i+1] )*0.5F;
	}
	
	/* Subframe 3 */
	Lsp_Az( lsp, az );
	return;
}


/*
* Lsp_lsf
*
*
* Parameters:
*    lsp               I: LSP vector
*    lsf               O: LSF vector
*
* Function:
*    Transformation lsp to lsf, LPC order LP_ORDER
*
* Returns:
*    void
*/
static void Lsp_lsf( float lsp[], float lsf[] )
{
	INT32 i;
	
	
	for ( i = 0; i < LP_ORDER; i++ ) {
		lsf[i] = ( float )( acos( lsp[i] )*SCALE_LSP_FREQ );
	}
	return;
}


/*
* Lsf_wt
*
*
* Parameters:
*    lsf               I: LSF vector
*    wf                O: square of weighting factors
*
* Function:
*    Compute LSF weighting factors
*
* Returns:
*    void
*/
static void Lsf_wt( float *lsf, float *wf )
{
	float temp;
	INT32 i;
	
	
	wf[0] = lsf[1];
	
	for ( i = 1; i < 9; i++ ) {
		wf[i] = lsf[i + 1] -lsf[i - 1];
	}
	wf[9] = 4000.0F - lsf[8];
	
	for ( i = 0; i < 10; i++ ) {
		if ( wf[i] < 450.0F ) {
			temp = 3.347F - SLOPE1_WGHT_LSF * wf[i];
		}
		else {
			temp = 1.8F - SLOPE2_WGHT_LSF * ( wf[i] - 450.0F );
		}
		wf[i] = temp * temp;
	}
	return;
}


/*
* Vq_subvec
*
*
* Parameters:
*    lsf_r1            I: 1st LSF residual vector
*    lsf_r2            I: 2nd LSF residual vector
*    dico              I: quantization codebook
*    wf1               I: 1st LSF weighting factors
*    wf2               I: 2nd LSF weighting factors
*    dico_size         I: size of quantization codebook
* Function:
*    Quantization of a 4 dimensional subvector
*
* Returns:
*    index             quantization index
*/
static INT16 Vq_subvec( float *lsf_r1, float *lsf_r2, const float *dico,
						float *wf1, float *wf2, INT16 dico_size )
{
	double temp, dist, dist_min;
	const float *p_dico;
	INT32 i, index = 0;
	
	
	dist_min = DBL_MAX;
	p_dico = dico;
	
	for ( i = 0; i < dico_size; i++ ) 
	{
		temp = lsf_r1[0] - *p_dico++;
		dist = temp * temp * wf1[0];
		temp = lsf_r1[1] - *p_dico++;
		dist += temp * temp * wf1[1];
		temp = lsf_r2[0] - *p_dico++;
		dist += temp * temp * wf2[0];
		temp = lsf_r2[1] - *p_dico++;
		dist += temp * temp * wf2[1];
		
		if ( dist < dist_min ) 
		{
			dist_min = dist;
			index = i;
		}
	}
	
	/* Reading the selected vector */
	p_dico = &dico[index << 2];
	lsf_r1[0] = *p_dico++;
	lsf_r1[1] = *p_dico++;
	lsf_r2[0] = *p_dico++;
	lsf_r2[1] = *p_dico++;
	return( INT16 )index;
}


/*
* Vq_subvec_s
*
*
* Parameters:
*    lsf_r1            I: 1st LSF residual vector
*    lsf_r2            I: 2nd LSF residual vector
*    dico              I: quantization codebook
*    wf1               I: 1st LSF weighting factors
*    wf2               I: 2nd LSF weighting factors
*    dico_size         I: size of quantization codebook
* Function:
*    Quantization of a 4 dimensional subvector with a signed codebook
*
* Returns:
*    index             quantization index
*/
static INT16 Vq_subvec_s( float *lsf_r1, float *lsf_r2, const float *dico, 
						  float *wf1, float *wf2, INT16 dico_size )
{
	double dist_min, dist1, dist2, temp1, temp2;
	const float *p_dico;
	INT32 i, index = 0;
	INT16 sign = 0;
	
	
	dist_min = DBL_MAX;
	p_dico = dico;
	
	for ( i = 0; i < dico_size; i++ ) 
	{
		temp1 = lsf_r1[0] - *p_dico;
		temp2 = lsf_r1[0] + *p_dico++;
		dist1 = temp1 * temp1 * wf1[0];
		dist2 = temp2 * temp2 * wf1[0];
		temp1 = lsf_r1[1] - *p_dico;
		temp2 = lsf_r1[1] + *p_dico++;
		dist1 += temp1 * temp1 * wf1[1];
		dist2 += temp2 * temp2 * wf1[1];
		temp1 = lsf_r2[0] - *p_dico;
		temp2 = lsf_r2[0] + *p_dico++;
		dist1 += temp1 * temp1 * wf2[0];
		dist2 += temp2 * temp2 * wf2[0];
		temp1 = lsf_r2[1] - *p_dico;
		temp2 = lsf_r2[1] + *p_dico++;
		dist1 += temp1 * temp1 * wf2[1];
		dist2 += temp2 * temp2 * wf2[1];
		
		if ( dist1 < dist_min ) 
		{
			dist_min = dist1;
			index = i;
			sign = 0;
		}
		
		if ( dist2 < dist_min ) 
		{
			dist_min = dist2;
			index = i;
			sign = 1;
		}
	}
	
	/* Reading the selected vector */
	p_dico = &dico[index << 2];
	
	if ( sign == 0 ) 
	{
		lsf_r1[0] = *p_dico++;
		lsf_r1[1] = *p_dico++;
		lsf_r2[0] = *p_dico++;
		lsf_r2[1] = *p_dico++;
	}
	else
	{
		lsf_r1[0] = -( *p_dico++ );
		lsf_r1[1] = -( *p_dico++ );
		lsf_r2[0] = -( *p_dico++ );
		lsf_r2[1] = -( *p_dico++ );
	}
	index = index << 1;
	index = index + sign;
	return( INT16 )index;
}


/*
* Reorder_lsf
*
*
* Parameters:
*    lsf               B: vector of LSFs
*    min_dist          I: minimum required distance
*
* Function:
*    Make sure that the LSFs are properly ordered and to keep a certain minimum
*    distance between adjacent LSFs. LPC order = LP_ORDER.
*
* Returns:
*    void
*/
static void Reorder_lsf( float *lsf, float min_dist )
{
	float lsf_min;
	INT32 i;
	
	
	lsf_min = min_dist;
	
	for ( i = 0; i < LP_ORDER; i++ )
	{
		if ( lsf[i] < lsf_min )
		{
			lsf[i] = lsf_min;
		}
		lsf_min = lsf[i] + min_dist;
	}
}


/*
* Lsf_lsp
*
*
* Parameters:
*    lsf               I: vector of LSFs
*    lsp	            O: vector of LSPs
*
* Function:
*    Transformation lsf to lsp, order LP_ORDER
*
* Returns:
*    void
*/
static void Lsf_lsp( float lsf[], float lsp[] )
{
	INT32 i;
	
	
	for ( i = 0; i < LP_ORDER; i++ )
	{
		lsp[i] = ( float )cos( SCALE_FREQ_LSP * lsf[i] );
	}
	return;
}


/*
* Vq_subvec3
*
*
* Parameters:
*    lsf_r1            I: 1st LSF residual vector
*    dico              I: quantization codebook
*    wf1               I: 1st LSF weighting factors
*    dico_size         I: size of quantization codebook
*    use_half          I: use every second entry in codebook
*
* Function:
*    Quantization of a 3 dimensional subvector
*
* Returns:
*    index             quantization index
*/
static INT16 Vq_subvec3( float *lsf_r1, const float *dico, float *wf1,
						 INT16 dico_size, INT32 use_half )
{
	double dist, dist_min;
	float temp;
	const float *p_dico;
	INT32 i, index = 0;
	
	
	dist_min = FLT_MAX;
	p_dico = dico;
	
	if ( use_half == 0 ) 
	{
		for ( i = 0; i < dico_size; i++ )
		{
			temp = lsf_r1[0] - *p_dico++;
			temp *= wf1[0];
			dist = temp * temp;
			temp = lsf_r1[1] - *p_dico++;
			temp *= wf1[1];
			dist += temp * temp;
			temp = lsf_r1[2] - *p_dico++;
			temp *= wf1[2];
			dist += temp * temp;
			
			if ( dist < dist_min )
			{
				dist_min = dist;
				index = i;
			}
		}
		p_dico = &dico[( 3 * index )];
	}
	else
	{
		for ( i = 0; i < dico_size; i++ )
		{
			temp = lsf_r1[0] - *p_dico++;
			temp *= wf1[0];
			dist = temp * temp;
			temp = lsf_r1[1] - *p_dico++;
			temp *= wf1[1];
			dist += temp * temp;
			temp = lsf_r1[2] - *p_dico++;
			temp *= wf1[2];
			dist += temp * temp;
			
			if ( dist < dist_min ) 
			{
				dist_min = dist;
				index = i;
			}
			p_dico = p_dico + 3;
		}
		p_dico = &dico[6 * index];
	}
	
	/* Reading the selected vector */
	lsf_r1[0] = *p_dico++;
	lsf_r1[1] = *p_dico++;
	lsf_r1[2] = *p_dico++;
	return( INT16 )index;
}


/*
* Vq_subvec4
*
*
* Parameters:
*    lsf_r1            I: 1st LSF residual vector
*    dico              I: quantization codebook
*    wf1               I: 1st LSF weighting factors
*    dico_size         I: size of quantization codebook
*
* Function:
*    Quantization of a 4 dimensional subvector
*
* Returns:
*    index             quantization index
*/
static INT16 Vq_subvec4( float *lsf_r1, const float *dico, 
						 float *wf1, INT16 dico_size )
{
	double dist, dist_min;
	float temp;
	const float *p_dico;
	INT32 i, index = 0;
	
	
	dist_min = FLT_MAX;
	p_dico = dico;
	
	for ( i = 0; i < dico_size; i++ )
	{
		temp = lsf_r1[0] - *p_dico++;
		temp *= wf1[0];
		dist = temp * temp;
		temp = lsf_r1[1] - *p_dico++;
		temp *= wf1[1];
		dist += temp * temp;
		temp = lsf_r1[2] - *p_dico++;
		temp *= wf1[2];
		dist += temp * temp;
		temp = lsf_r1[3] - *p_dico++;
		temp *= wf1[3];
		dist += temp * temp;
		
		if ( dist < dist_min ) 
		{
			dist_min = dist;
			index = i;
		}
	}
	
	/* Reading the selected vector */
	p_dico = &dico[index << 2];
	lsf_r1[0] = *p_dico++;
	lsf_r1[1] = *p_dico++;
	lsf_r1[2] = *p_dico++;
	lsf_r1[3] = *p_dico++;
	return( INT16 )index;
}


/*
* Q_plsf_3
*
*