#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 G729AGet_lsp_pol(Word16 *lsp, Word32 *f);

void G729ALsp_Az(Word16 lsp[], Word16 a[])
{
	Word16 i, j;
	Word32 f1[6], f2[6];
	Word32 t0;
	
	G729AGet_lsp_pol(&lsp[0],f1);
	G729AGet_lsp_pol(&lsp[1],f2);
	for (i = 5; i > 0; i--)
		f1[i] = _sadd(f1[i], f1[i-1]);
		
    for (i = 5; i > 0; i--)         
		f2[i] = G729AL_sub(f2[i], f2[i-1]);  
	
	a[0] = 4096;
	for (i = 1, j = 10; i <= 5; i++, j--)
	{
		t0   = G729AL_add(f1[i], f2[i]);                           
		a[i] = G729Aextract_l( G729AL_shr_r(t0, 13) );        
	
		t0   = G729AL_sub(f1[i], f2[i]);                         
		a[j] = G729Aextract_l( G729AL_shr_r(t0, 13) );        
		
	}
	
	return;
}

static void G729AGet_lsp_pol(Word16 *lsp, Word32 *f)
{
	Word16 i,j, hi, lo;
	Word32 t0;
	
	*f = _smpy(4096, 2048);
	f++;
	*f = _ssub((Word32)0, _smpy(*lsp, 512));  
	
	f++;
	lsp += 2;                                     
	for(i=2; i<=5; i++)
	{
		*f = f[-2];		
		for(j=1; j<i; j++, f--)
		{
			hi = f[-1] >> 16;
	        lo = (Word16)_ssub(_sshvr(f[-1], 1), _smpy(hi, 16384));
			t0 = _sadd(_smpy(hi, *lsp), _smpy(_smpy(lo, *lsp)>>16, 1));         
			t0 = _sshl(t0, 1);
			*f = _sadd(*f, f[-2]);                        
			*f = _ssub(*f, t0);                              
		}
		*f   = _ssub(*f, _smpy(*lsp, 512));                   
		f   += i;                                 
		lsp += 2;                                
	}
	return;
}

void G729ALsf_lsp(Word16 lsf[], Word16 lsp[], Word16 m)
{
	Word16 i, ind, offset;
	Word32 L_tmp;
	for(i=0; i<m; i++)
	{
		ind    = _sshvr(lsf[i], 8);                
		offset = lsf[i] & (Word16)0x00ff;       
		
		L_tmp   = _smpy(_ssub(G729A_table[ind+1], G729A_table[ind]), offset);
		lsp[i] = _sadd(G729A_table[ind], G729Aextract_l(_sshvr(L_tmp, 9)));
	}
	return;
}

void G729ALsp_lsf(Word16 lsp[], Word16 lsf[], Word16 m)
{
	Word16 i, ind, tmp;
	Word32 L_tmp;
	
	ind = 63;    
	for(i= m-(Word16)1; i >= 0; i--)
	{
		while( _ssub(G729A_table[ind], lsp[i]) < 0 )
		{
			ind = _ssub(ind,1);
		}		
		L_tmp  = _smpy( _ssub(lsp[i], G729A_table[ind]) , G729A_slope[ind] );
		tmp = (Word16)(_sadd(_sshl(L_tmp, 3), (Word32)0x00008000L)>>16);
		lsf[i] = _sadd(tmp, _sshl(ind, 8));
	}
	return;
}

void G729ALsf_lsp2(Word16 lsf[], Word16 lsp[], Word16 m)
{
	Word16 i, ind;
	Word16 offset; 
	Word16 freq;    
	Word32 L_tmp;
	for(i=0; i<m; i++)
	{
		freq = _smpy(lsf[i], 20861)>>16;          
		ind    = _sshvr(freq, 8);              
		offset = freq & (Word16)0x00ff;     
		
		ind = _min2(ind, 63);                 
		
		L_tmp   = _smpy(G729A_slope_cos[ind], offset);   
		lsp[i] = _sadd(G729A_table2[ind], (Word16)_sshvr(L_tmp, 13));
		
	}
	return;
}

void G729ALsp_lsf2(Word16 lsp[], Word16 lsf[], Word16 m)
{
	Word16 i, ind;
	Word16 offset;   
	Word16 freq;     
	Word32 L_tmp, L_tmp1;
	
	ind = 63;          
	for(i= m-(Word16)1; i >= 0; i--)
	{
		while( _ssub(G729A_table2[ind], lsp[i]) < 0 )
		{
			ind = _ssub(ind,1);
			if ( ind <= 0 )
				break;
		}		
		offset = _ssub(lsp[i], G729A_table2[ind]);		
		L_tmp  = _smpy(G729A_slope_acos[ind], offset );   
		 
		L_tmp1 = _sshvl(ind, 9);
		freq = _sadd2(_spack2(L_tmp1, L_tmp1), (Word16)(_sshvr(L_tmp, 12)));
		
		lsf[i] = _smpy(freq, 25736)>>16;          	
	}
	return;
}

void G729AWeight_Az(Word16 a[], Word16 gamma, Word16 m, Word16 ap[])
{
	Word16 i, fac;
	
	ap[0] = a[0];
	fac   = gamma;
	for(i=1; i<m; i++)
	{
	     ap[i] = (Word16)(_sadd(_smpy(a[i], fac),  (Word32)0x00008000L)>>16);
	     fac   = (Word16)(_sadd(_smpy(fac, gamma), (Word32)0x00008000L)>>16);
	}
	ap[m] = (Word16)(_sadd(_smpy(a[m], fac), (Word32)0x00008000L)>>16);
	return;
}

void G729AInt_qlpc(Word16 lsp_old[], Word16 lsp_new[], Word16 Az[])
{
	Word16 i;
	Word16 lsp[G729A_M];
	Word32 *tmp;
	Word32 Temp0, Temp1, Temp2, Temp3;
	tmp = (Word32 *)&lsp[0];
	for (i = 0; i < G729A_M; i+=2) 
	{
	    Temp0 = _sshvr(lsp_new[i],   1);
	    Temp2 = _sshvr(lsp_new[i+1], 1);
	    Temp1 = _sshvr(lsp_old[i],   1);
	    Temp3 = _sshvr(lsp_old[i+1], 1);
		*tmp++ = _sadd2(_spack2(Temp2, Temp0), _spack2(Temp3, Temp1));
	}
	G729ALsp_Az(lsp, Az);              
	
	G729ALsp_Az(lsp_new, &Az[G729A_MP1]);    
	
	return;
}