#include "../Common/typedef.h"
#include "../Include/G729A_basic_op.h"
#include "../Include/G729A_ld8a.h"

void G729ALsp_get_quant(Word16 lspcb1[][G729A_M], Word16 lspcb2[][G729A_M], Word16 code0, Word16 code1, Word16 code2, Word16 fg[][G729A_M], Word16 freq_prev[][G729A_M], Word16 lspq[], Word16 fg_sum[])
{
	Word16 j;
	Word16 buf[G729A_M];           
	
	for ( j = 0 ; j < G729A_NC ; j++ )
		buf[j] = _sadd( lspcb1[code0][j], lspcb2[code1][j] );
	
	for ( j = G729A_NC ; j < G729A_M ; j++ )
		buf[j] = _sadd( lspcb1[code0][j], lspcb2[code2][j] );
		
	G729ALsp_expand_1_2(buf, G729A_GAP1);
	G729ALsp_expand_1_2(buf, G729A_GAP2);
	
	G729ALsp_prev_compose(buf, lspq, fg, freq_prev, fg_sum);
	
	G729ALsp_prev_update(buf, freq_prev);
	
	G729ALsp_stability( lspq );
	
	return;
}

void G729ALsp_expand_1(Word16 buf[], Word16 gap)
{
	Word16 j, tmp;
	Word16 diff;       
	for ( j = 1 ; j < G729A_NC ; j++ ) 
	{
		diff = _ssub( buf[j-1], buf[j] );
		tmp = _sshvr( _sadd( diff, gap), 1 );
		
		tmp = (tmp > 0) ? tmp : 0;
		buf[j-1] = _ssub( buf[j-1], tmp );
		buf[j]   = _sadd( buf[j], tmp );
	}
	return;
}

void G729ALsp_expand_2(Word16 buf[], Word16 gap)
{
	Word16 j, tmp;
	Word16 diff;       
	Word32 temp;
	for ( j = G729A_NC ; j < G729A_M ; j++ ) 
	{
		diff = _ssub( buf[j-1], buf[j] );
		temp = _sshvr( _sadd2( diff, gap), 1 );
		tmp = _spack2(temp, temp);
		
		tmp = (tmp > 0) ? tmp : 0;
		buf[j-1] = _ssub( buf[j-1], tmp );
		buf[j]   = _sadd2( buf[j], tmp );
	}
	return;
}

void G729ALsp_expand_1_2(Word16 buf[], Word16 gap)
{
	Word16 j, tmp;
	Word16 diff;        
	for ( j = 1 ; j < G729A_M ; j++ ) 
	{
		diff = _ssub( buf[j-1], buf[j] );
		tmp = _sshvr( _sadd2(diff, gap),1);
		
		tmp = (tmp > 0)? tmp : 0;
		buf[j-1] = _ssub( buf[j-1], tmp );
		buf[j]   = _sadd2( buf[j], tmp );
	}
	return;
}

void G729ALsp_prev_compose(Word16 lsp_ele[], Word16 lsp[], Word16 fg[][G729A_M], Word16 freq_prev[][G729A_M], Word16 fg_sum[])
{
	Word16 j, k;
	Word32 L_acc;                 
	for ( j = 0 ; j < G729A_M ; j++ ) 
	{
	    L_acc = _smpy(lsp_ele[j], fg_sum[j]);
		for ( k = 0 ; k < G729A_MA_NP ; k++ )
			L_acc = _sadd( L_acc, _smpy(freq_prev[k][j], fg[k][j]));
		
		lsp[j] = L_acc>>16;
	}
	return;
}

void G729ALsp_prev_extract(Word16 lsp[G729A_M], Word16 lsp_ele[G729A_M], Word16 fg[G729A_MA_NP][G729A_M], Word16 freq_prev[G729A_MA_NP][G729A_M], Word16 fg_sum_inv[G729A_M])
{
	Word16 j, k;
	Word32 L_temp0, L_temp1;               
	Word16 temp0, temp1;                 	
	for ( j = 0 ; j < G729A_M ; j+=2 ) 
	{
		L_temp0 = (Word32)lsp[j]<<16;
		L_temp1 = (Word32)lsp[j+1]<<16;
		
		for ( k = 0 ; k < G729A_MA_NP ; k++ )
		{
		    L_temp0 = _ssub(L_temp0, _smpy(freq_prev[k][j],   fg[k][j]));
		    L_temp1 = _ssub(L_temp1, _smpy(freq_prev[k][j+1], fg[k][j+1]));
		}
		temp0 = L_temp0>>16;
		temp1 = L_temp1>>16;
		L_temp0 = _smpy( temp0, fg_sum_inv[j] );
		L_temp1 = _smpy( temp1, fg_sum_inv[j+1] );
		lsp_ele[j] = _sshl(L_temp0, 3)>>16;	
		lsp_ele[j+1] = _sshl(L_temp1, 3)>>16;	
	}
	return;
}

void G729ALsp_prev_update(Word16 lsp_ele[G729A_M], Word16 freq_prev[G729A_MA_NP][G729A_M])
{
	Word16 k;
	
	for ( k = G729A_MA_NP-1 ; k > 0 ; k-- )
		G729ACopy2(freq_prev[k-1], freq_prev[k], G729A_M);
	
	G729ACopy2(lsp_ele, freq_prev[0], G729A_M);
	return;
}

void G729ALsp_stability(Word16 buf[])
{
	Word16 j;
	Word16 tmp;
	Word32 L_diff;
	Word32 L_acc, L_accb;
	
	for(j=0; j<G729A_M-1; j++) {
		L_acc = G729AL_deposit_l( buf[j+1] );
		L_accb = G729AL_deposit_l( buf[j] );
		L_diff = G729AL_sub( L_acc, L_accb );
		
		if( L_diff < 0L ) 
		{
			tmp      = buf[j+1];
			buf[j+1] = buf[j];
			buf[j]   = tmp;
		}
	}
	
	if( G729Asub_s(buf[0], G729A_L_LIMIT) <0 ) {
		buf[0] = G729A_L_LIMIT;
		printf("lsp_stability warning Low \n");
	}
	for(j=0; j<G729A_M-1; j++) {
		L_acc = G729AL_deposit_l( buf[j+1] );
		L_accb = G729AL_deposit_l( buf[j] );
		L_diff = G729AL_sub( L_acc, L_accb );
		
		if( G729AL_sub(L_diff, G729A_GAP3)<0L ) {
			buf[j+1] = G729Aadd_s( buf[j], G729A_GAP3 );
		}
	}
	
	if( G729Asub_s(buf[G729A_M-1],G729A_M_LIMIT)>0 ) {
		buf[G729A_M-1] = G729A_M_LIMIT;
		printf("lsp_stability warning High \n");
	}
	return;
}