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

static void G729AGbk_presel(Word16 best_gain[], Word16 *cand1, Word16 *cand2, Word16 gcode0);

Word16 G729AQua_gain(Word16 code[], Word16 g_coeff[], Word16 exp_coeff[], Word16 L_subfr, Word16 *gain_pit, Word16 *gain_cod, Word16 tameflag)
{
	Word16  i, j, index1, index2;
	Word16  cand1, cand2;
	Word16  exp, gcode0, exp_gcode0, gcode0_org, e_min ;
	Word16  nume, denom, inv_denom;
	Word16  exp1,exp2,exp_nume,exp_denom,exp_inv_denom,sft,tmp;
	Word16  g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
	Word16  coeff[5], coeff_lsf[5];
	Word16  exp_min[5];
	Word32  L_gbk12;
	Word32  L_tmp, L_dist_min, L_temp, L_tmp1, L_tmp2, L_acc, L_accb;
	Word16  best_gain[2];
	
	static Word16 past_qua_en[4] = { -14336, -14336, -14336, -14336 };
	
	G729AGain_predict( past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );
	
	L_tmp1 = _smpy( g_coeff[0], g_coeff[2] );
	exp1   = _sadd( _sadd( exp_coeff[0], exp_coeff[2] ), 1-2 );
	L_tmp2 = _smpy( g_coeff[4], g_coeff[4] );
	exp2   = _sadd( _sadd( exp_coeff[4], exp_coeff[4] ), 1 );
	L_tmp = (exp1 > exp2) ? (_ssub(_sshvr(L_tmp1, _ssub(exp1,exp2)), L_tmp2)) : (_ssub(L_tmp1, _sshvr(L_tmp2, _ssub(exp2,exp1))));
	exp = _min2(exp1, exp2);
	
	sft = G729Anorm_l( L_tmp );
	denom = G729Aextract_h( G729AL_shl(L_tmp, sft) );
	exp_denom = G729Asub_s( G729Aadd_s( exp, sft ), 16 );
	
	inv_denom = G729Adiv_s(16384,denom);
	inv_denom = G729Anegate( inv_denom );
	exp_inv_denom = G729Asub_s( 14+15, exp_denom );
	
	L_tmp1 = _smpy( g_coeff[2], g_coeff[1] );
	exp1   = _sadd( exp_coeff[2], exp_coeff[1] );
	L_tmp2 = _smpy( g_coeff[3], g_coeff[4] );
	exp2   = _sadd( _sadd( exp_coeff[3], exp_coeff[4] ), 1 );
	L_tmp = (exp1 > exp2) ? (_ssub(_sshvr(L_tmp1, _sadd(_ssub(exp1,exp2),1)), _sshvr(L_tmp2,1))) : (_ssub(_sshvr(L_tmp1,1 ), _sshvr(L_tmp2, _sadd(_ssub(exp2,exp1),1))));
	exp = (exp1 > exp2) ? _ssub(exp2,1) : _ssub(exp1,1);
	
	sft = G729Anorm_l( L_tmp );
	nume = G729Aextract_h( G729AL_shl(L_tmp, sft) );
	exp_nume = G729Asub_s( G729Aadd_s( exp, sft ), 16 );
	
	sft = G729Asub_s( G729Aadd_s( exp_nume, exp_inv_denom ), (9+16-1) );
	L_acc = G729AL_shr( G729AL_mult( nume,inv_denom ), sft );
	best_gain[0] = G729Aextract_h( L_acc );             
	best_gain[0] = (tameflag == 1) ? best_gain[0] : _min2(best_gain[0], G729A_GPCLIP2);
	
	L_tmp1 = G729AL_mult( g_coeff[0], g_coeff[3] );
	exp1   = G729Aadd_s( exp_coeff[0], exp_coeff[3] ) ;
	L_tmp2 = G729AL_mult( g_coeff[1], g_coeff[4] );
	exp2   = G729Aadd_s( G729Aadd_s( exp_coeff[1], exp_coeff[4] ), 1 );
	L_tmp = (exp1 > exp2) ? (_ssub(_sshvr(L_tmp1, _sadd(_ssub(exp1,exp2),1)), _sshvr(L_tmp2, 1))) : (_ssub(_sshvr(L_tmp1, 1), _sshvr(L_tmp2, _sadd(_ssub(exp2,exp1), 1))));
	exp = (exp1 > exp2) ? _ssub(exp2, 1) : _ssub(exp1, 1);
	
	sft = G729Anorm_l( L_tmp );
	nume = G729Aextract_h( G729AL_shl(L_tmp, sft) );
	exp_nume = G729Asub_s( G729Aadd_s( exp, sft ), 16 );
	
	sft = G729Asub_s( G729Aadd_s( exp_nume, exp_inv_denom ), (2+16-1) );
	L_acc = G729AL_shr( G729AL_mult( nume,inv_denom ), sft );
	best_gain[1] = G729Aextract_h( L_acc );            
	
	if( G729Asub_s(exp_gcode0,4) >= 0 )
	{
		gcode0_org = G729Ashr_s( gcode0, G729Asub_s(exp_gcode0,4) );
	}
	else
	{
		L_acc = G729AL_deposit_l( gcode0 );
		L_acc = G729AL_shl( L_acc, G729Asub_s( (4+16), exp_gcode0 ) );
		gcode0_org = G729Aextract_h( L_acc );              
	}
		
	G729AGbk_presel(best_gain, &cand1, &cand2, gcode0_org );
	
	exp_min[0] = G729Aadd_s( exp_coeff[0], 13 );
	exp_min[1] = G729Aadd_s( exp_coeff[1], 14 );
	exp_min[2] = G729Aadd_s( exp_coeff[2], G729Asub_s( G729Ashl_s( exp_gcode0, 1 ), 21 ) );
	exp_min[3] = G729Aadd_s( exp_coeff[3], G729Asub_s( exp_gcode0, 3 ) );
	exp_min[4] = G729Aadd_s( exp_coeff[4], G729Asub_s( exp_gcode0, 4 ) );
	
	e_min = exp_min[0];
	for(i=1; i<5; i++)
	{
		if( G729Asub_s(exp_min[i], e_min) < 0 )
		{
			e_min = exp_min[i];
		}
	}
	
	for(i=0; i<5; i++)
	{
		j = G729Asub_s( exp_min[i], e_min );
		L_tmp = G729AL_deposit_h( g_coeff[i] );
		L_tmp = G729AL_shr( L_tmp, j );          
		G729AL_Extract( L_tmp, &coeff[i], &coeff_lsf[i] );          
	}
	
	L_dist_min = G729AMAX_32;
	
	index1 = cand1;
	index2 = cand2;
	
	if(tameflag == 1)
	{
		for(i=0; i<G729A_NCAN1; i++)
		{
			for(j=0; j<G729A_NCAN2; j++)
			{
				g_pitch = G729Aadd_s( G729A_gbk1[cand1+i][0], G729A_gbk2[cand2+j][0] );     
				if(g_pitch < G729A_GP0999) 
				{
					L_acc = G729AL_deposit_l( G729A_gbk1[cand1+i][1] );
					L_accb = G729AL_deposit_l( G729A_gbk2[cand2+j][1] );               
					L_tmp = G729AL_add( L_acc,L_accb );
					tmp = G729Aextract_l( G729AL_shr( L_tmp,1 ) );                     
					
					g_code   = G729Amult( gcode0, tmp );          
					g2_pitch = G729Amult(g_pitch, g_pitch);                       
					g2_code  = G729Amult(g_code,  g_code);   
					g_pit_cod= G729Amult(g_code,  g_pitch);      
					
					L_tmp = G729AMpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
					L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
					L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
					L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[3], coeff_lsf[3], g_code) );
					L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
					
					L_temp = G729AL_sub(L_tmp, L_dist_min);
					
					if( L_temp < 0L ){
						L_dist_min = L_tmp;
						index1 = G729Aadd_s(cand1,i);
						index2 = G729Aadd_s(cand2,j);
					}
				}
			}
		}
		
	}
	else
	{
		for(i=0; i<G729A_NCAN1; i++)
		{
			for(j=0; j<G729A_NCAN2; j++)
			{
				g_pitch = G729Aadd_s( G729A_gbk1[cand1+i][0], G729A_gbk2[cand2+j][0] );    
				L_acc = G729AL_deposit_l( G729A_gbk1[cand1+i][1] );
				L_accb = G729AL_deposit_l( G729A_gbk2[cand2+j][1] );            
				L_tmp = G729AL_add( L_acc,L_accb );
				tmp = G729Aextract_l( G729AL_shr( L_tmp,1 ) );                   
				
				g_code   = G729Amult( gcode0, tmp );          
				g2_pitch = G729Amult(g_pitch, g_pitch);                       
				g2_code  = G729Amult(g_code,  g_code);       
				g_pit_cod= G729Amult(g_code,  g_pitch);      
				
				L_tmp = G729AMpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
				L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
				L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
				L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[3], coeff_lsf[3], g_code) );
				L_tmp = G729AL_add(L_tmp, G729AMpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
				
				L_temp = G729AL_sub(L_tmp, L_dist_min);
				
				if( L_temp < 0L ){
					L_dist_min = L_tmp;
					index1 = G729Aadd_s(cand1,i);
					index2 = G729Aadd_s(cand2,j);
				}
				
			}
		}
	}
	*gain_pit = G729Aadd_s( G729A_gbk1[index1][0], G729A_gbk2[index2][0] );   
	
	L_acc = G729AL_deposit_l( G729A_gbk1[index1][1] );
	L_accb = G729AL_deposit_l( G729A_gbk2[index2][1] );
	L_gbk12 = G729AL_add( L_acc, L_accb );                         
	tmp = G729Aextract_l( G729AL_shr( L_gbk12,1 ) );                    
	L_acc = G729AL_mult(tmp, gcode0);                 
	
	L_acc = G729AL_shl(L_acc, G729Aadd_s( G729Anegate(exp_gcode0),(-12-1+1+16) ));
	*gain_cod = G729Aextract_h( L_acc );                            
	
	G729AGain_update( past_qua_en, L_gbk12 );
	
	return( G729Aadd_s( G729A_map1[index1]*(Word16)G729A_NCODE2, G729A_map2[index2] ) );
	
}

static void G729AGbk_presel(Word16 best_gain[], Word16 *cand1, Word16 *cand2, Word16 gcode0)
{
	Word16    acc_h;
	Word16    sft_x,sft_y;
	Word32    L_acc,L_preg,L_cfbg,L_tmp,L_tmp_x,L_tmp_y;
	Word32 L_temp;
	
	L_cfbg = _smpy( G729A_coef[0][0], best_gain[0] );        
	L_acc = _sshvr( G729A_L_coef[1][1], 15 );                     
	L_acc = _sadd( L_cfbg , L_acc );
	acc_h = L_acc >> 16;                            
	L_preg = _smpy(acc_h, gcode0 );                      
	L_acc = _sshl( (Word32)best_gain[1], 7 );        
	L_acc = _ssub( L_acc, L_preg );
	acc_h = _sshl( L_acc,2 ) >> 16;              
	L_tmp_x = _smpy( acc_h, G729A_INV_COEF );
	
	L_acc = _sshvr( G729A_L_coef[0][1], 10 );                   
	L_acc = _ssub( L_cfbg, L_acc );                     
	acc_h = L_acc >> 16;                            
	acc_h = G729Amult( acc_h, gcode0 );                       
	L_tmp = _smpy( acc_h, G729A_coef[1][0] );                   
	
	L_preg = _smpy( G729A_coef[0][0], best_gain[1] );          
	L_acc = _ssub( L_tmp, _sshvr(L_preg,3) );             
	
	acc_h = _sshl( L_acc,2 ) >> 16;              
	L_tmp_y = _smpy( acc_h, G729A_INV_COEF );
	
	sft_y = (14+4+1)-16;          
	sft_x = (15+4+1)-15;       
	
	if(gcode0>0)
	{
		*cand1 = 0 ;
		
		do
		{
			L_temp = G729AL_sub( L_tmp_y, G729AL_shr(G729AL_mult(G729A_thr1[*cand1],gcode0),sft_y));
			if(L_temp >0L  )
			{
				(*cand1) =G729Aadd_s(*cand1,1);
			}
			else               break ;			
		} while(G729Asub_s((*cand1),(G729A_NCODE1-G729A_NCAN1))<0) ;
		*cand2 = 0 ;
		
		do
		{
			L_temp = G729AL_sub( L_tmp_x , G729AL_shr(G729AL_mult(G729A_thr2[*cand2],gcode0),sft_x));
			if( L_temp >0L) 
			{
				(*cand2) =G729Aadd_s(*cand2,1);
			}
			else               break ;
		} while(G729Asub_s((*cand2),(G729A_NCODE2-G729A_NCAN2))<0) ;
	}
	else
	{
		*cand1 = 0 ;
		
		do
		{
			L_temp = G729AL_sub(L_tmp_y ,G729AL_shr(G729AL_mult(G729A_thr1[*cand1],gcode0),sft_y));
			if( L_temp <0L)
			{
				(*cand1) =G729Aadd_s(*cand1,1);
			}
			else               break ;
		} while(G729Asub_s((*cand1),(G729A_NCODE1-G729A_NCAN1))) ;
		*cand2 = 0 ;
		
		do
		{
			L_temp =G729AL_sub(L_tmp_x ,G729AL_shr(G729AL_mult(G729A_thr2[*cand2],gcode0),sft_x));
			if( L_temp <0L)
			{
				(*cand2) =G729Aadd_s(*cand2,1);
			}
			else               break ;
		} while(G729Asub_s( (*cand2),(G729A_NCODE2-G729A_NCAN2))) ;
	}
	
	return ;
}