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

void G729AQua_lsp(Word16 lsp[], Word16 lsp_q[], Word16 ana[])
{
	Word16 lsf[G729A_M], lsf_q[G729A_M];  
	
	G729ALsp_lsf2(lsp, lsf, G729A_M);
	
	G729ALsp_qua_cs(lsf, lsf_q, ana );
	
	G729ALsf_lsp2(lsf_q, lsp_q, G729A_M);
	
	return;
}

static Word16 G729Afreq_prev[G729A_MA_NP][G729A_M];          
static Word16 G729Afreq_prev_reset[G729A_M] = {  
	2339, 4679, 7018, 9358, 11698, 14037, 16377, 18717, 21056, 23396
};     


void G729ALsp_encw_reset(void)
{
	Word16 i;
	
	for(i=0; i<G729A_MA_NP; i++)
		G729ACopy2( &G729Afreq_prev_reset[0], &G729Afreq_prev[i][0], G729A_M );
}

void G729ALsp_qua_cs(Word16 flsp_in[G729A_M], Word16 lspq_out[G729A_M], Word16 *code)
{
	Word16 wegt[G729A_M];       
	
	G729AGet_wegt( flsp_in, wegt );
	
	G729ARelspwed( flsp_in, wegt, lspq_out, G729A_lspcb1, G729A_lspcb2, G729A_fg,
		G729Afreq_prev, G729A_fg_sum, G729A_fg_sum_inv, code);
}

void G729ARelspwed(Word16 lsp[], Word16 wegt[], Word16 lspq[], Word16 lspcb1[][G729A_M], Word16 lspcb2[][G729A_M], Word16 fg[G729A_MODE][G729A_MA_NP][G729A_M], Word16 G729Afreq_prev[G729A_MA_NP][G729A_M], Word16 fg_sum[G729A_MODE][G729A_M], Word16 fg_sum_inv[G729A_MODE][G729A_M], Word16 code_ana[])
{
	Word16 mode, j;
	Word16 index, mode_index;
	Word16 cand[G729A_MODE], cand_cur;
	Word16 tindex1[G729A_MODE], tindex2[G729A_MODE];
	Word32 L_tdist[G729A_MODE];        
	Word16 rbuf[G729A_M];              
	Word16 buf[G729A_M];                
	
	for(mode = 0; mode<G729A_MODE; mode++) 
	{
		G729ALsp_prev_extract(lsp, rbuf, fg[mode], G729Afreq_prev, fg_sum_inv[mode]);
		
		G729ALsp_pre_select(rbuf, lspcb1, &cand_cur );
		cand[mode] = cand_cur;
		
		G729ALsp_select_1(rbuf, lspcb1[cand_cur], wegt, lspcb2, &index);
		
		tindex1[mode] = index;
		for( j = 0 ; j < G729A_NC ; j++ )
			buf[j] = _sadd( lspcb1[cand_cur][j], lspcb2[index][j] );
			
		G729ALsp_expand_1(buf, G729A_GAP1);
		
		G729ALsp_select_2(rbuf, lspcb1[cand_cur], wegt, lspcb2, &index);
		
		tindex2[mode] = index;
		for( j = G729A_NC ; j < G729A_M ; j++ )
			buf[j] = _sadd( lspcb1[cand_cur][j], lspcb2[index][j] );
		G729ALsp_expand_2(buf, G729A_GAP1);
		
		G729ALsp_expand_1_2(buf, G729A_GAP2);
		
		G729ALsp_get_tdist(wegt, buf, &L_tdist[mode], rbuf, fg_sum[mode]);
	}
	
	G729ALsp_last_select(L_tdist, &mode_index);
	code_ana[0] = _sshl( mode_index,G729A_NC0_B ) | cand[mode_index];
	code_ana[1] = _sshl( tindex1[mode_index],G729A_NC1_B ) | tindex2[mode_index];
	
	G729ALsp_get_quant(lspcb1, lspcb2, cand[mode_index],
		tindex1[mode_index], tindex2[mode_index],
		fg[mode_index], G729Afreq_prev, lspq, fg_sum[mode_index]) ;
	
	return;
}

void G729ALsp_pre_select(Word16 rbuf[], Word16 lspcb1[][G729A_M], Word16 *cand)
{
	Word16 i, j;
	Word16 tmp;                 
	Word32 L_dmin;             
	Word32 L_tmp;              
	Word32 L_temp;
	
	*cand = 0;
	L_dmin = G729AMAX_32;
	for ( i = 0 ; i < G729A_NC0 ; i++ ) 
	{
		L_tmp = 0;
		for ( j = 0 ; j < G729A_M ; j++ ) 
		{
			tmp = _ssub(rbuf[j], lspcb1[i][j]);
			L_tmp = _sadd(L_tmp, _smpy(tmp, tmp));
		}
		
		L_temp = _ssub(L_tmp,L_dmin);
		if (  L_temp< 0L) 
		{
			L_dmin = L_tmp;
			*cand = i;
		}
	}     
	return;
}

void G729ALsp_select_1(Word16 rbuf[], Word16 lspcb1[], Word16 wegt[], Word16 lspcb2[][G729A_M], Word16 *index)
{
	Word16 j, k1;
	Word16 buf[G729A_M];             
	Word32 L_dist;              
	Word32 L_dmin;             
	Word16 tmp,tmp2;            
	Word32 L_temp;
	for ( j = 0 ; j < G729A_NC ; j++ )
		buf[j] = _ssub(rbuf[j], lspcb1[j]);
	*index = 0;
	L_dmin = G729AMAX_32;
	for ( k1 = 0 ; k1 < G729A_NC1 ; k1++ ) 
	{
		L_dist = 0;
		for ( j = 0 ; j < G729A_NC ; j++ ) 
		{
			tmp = _ssub(buf[j], lspcb2[k1][j]);
		    tmp2 = _smpy(wegt[j], tmp)>>16;  
			L_dist = _sadd(L_dist, _smpy(tmp2, tmp));
		}
		
		L_temp =_ssub(L_dist,L_dmin);
		if ( L_temp <0L ) 
		{
			L_dmin = L_dist;
			*index = k1;
		}
	}
	
	return;
}

void G729ALsp_select_2(Word16 rbuf[], Word16 lspcb1[], Word16 wegt[], Word16 lspcb2[][G729A_M], Word16 *index)
{
	Word16 j, k1;
	Word16 buf[G729A_M];              
	Word32 L_dist;              
	Word32 L_dmin;             
	Word16 tmp,tmp2;           
	Word32 L_temp;
	for ( j = G729A_NC ; j < G729A_M ; j++ )
		buf[j] = _ssub(rbuf[j], lspcb1[j]);
	

	*index = 0;
	L_dmin = G729AMAX_32;
	for ( k1 = 0 ; k1 < G729A_NC1 ; k1++ ) 
	{
		L_dist = 0;
		for ( j = G729A_NC ; j < G729A_M ; j++ ) 
		{
			tmp = _ssub(buf[j], lspcb2[k1][j]);
			tmp2 = _smpy( wegt[j], tmp )>>16;
			L_dist = _sadd( L_dist, _smpy(tmp2, tmp) );
		}
		
		L_temp = _ssub(L_dist, L_dmin);
		if ( L_temp <0L ) 
		{
			L_dmin = L_dist;
			*index = k1;
		}
	}
	return;
}

void G729ALsp_get_tdist(Word16 wegt[], Word16 buf[], Word32 *L_tdist, Word16 rbuf[], Word16 fg_sum[])
{
	Word16 j;
	Word16 tmp, tmp2;     
	Word32 L_acc;         
	
	*L_tdist = 0;
	for ( j = 0 ; j < G729A_M ; j++ ) 
	{
		tmp = _ssub( buf[j], rbuf[j] );
		tmp = _smpy(tmp, fg_sum[j])>>16;
		
		L_acc = _smpy( wegt[j], tmp );
		tmp2 = _sshl( L_acc, 4 )>>16;
		*L_tdist = _sadd(*L_tdist, _smpy(tmp2, tmp));
	}
	
	return;
}

void G729ALsp_last_select(Word32 L_tdist[], Word16 *mode_index)
{
    Word32 L_temp;
	*mode_index = 0;

	L_temp =G729AL_sub(L_tdist[1] ,L_tdist[0]);
	if (  L_temp<0L)
	{
		*mode_index = 1;
	}
	return;
}

void G729AGet_wegt(Word16 flsp[], Word16 wegt[])
{
	Word16 i;
	Word16 tmp;
	Word32 L_acc;
	Word16 sft;
	Word16 buf[G729A_M]; 
	
	buf[0] = _ext(_ssub( flsp[1], (G729A_PI04+8192)), 16, 16);
	for ( i = 1 ; i < G729A_M-1 ; i++ ) 
	{
		tmp = _ssub( flsp[i+1], flsp[i-1] );
		buf[i] = _ssub( tmp, 8192 );
	}	
	buf[G729A_M-1] = _ext(_ssub( (G729A_PI92-8192), flsp[G729A_M-2]), 16, 16);
	
	for ( i = 0 ; i < G729A_M ; i++ ) 
	{
		if ( buf[i] > 0 )
		{
			wegt[i] = 2048;                    
		}
		else 
		{
			L_acc = _smpy(buf[i], buf[i]);          
			tmp = _sshl(L_acc, 2)>>16;       
			
			L_acc = _smpy(tmp, G729A_CONST10);             
			tmp = _sshl(L_acc, 2)>>16;       
			
			wegt[i] = _sadd2(tmp, 2048);  
		}
	}
	L_acc = _smpy(wegt[4], G729A_CONST12);             
	wegt[4] = _sshl(L_acc, 1)>>16;      
	
	L_acc = _smpy(wegt[5], G729A_CONST12);            
	wegt[5] = _sshl(L_acc, 1)>>16;
	tmp = 0;
	for ( i = 0; i < G729A_M; i++ ) 
	{
	    tmp = _max2(wegt[i], tmp);
	}
	
	sft = G729Anorm_s(tmp);
	for ( i = 0; i < G729A_M; i++ ) 
	{	
        wegt[i] = _sshl(wegt[i], sft);
    }
	return;
}