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

static Word16 G729A_old_speech[G729A_L_TOTAL];
static Word16 *G729A_speech, *G729A_p_window;

Word16 *G729A_new_speech;  //全局變量

static Word16 G729A_old_wsp[G729A_L_FRAME+G729A_PIT_MAX];
static Word16 *G729A_wsp;

static Word16 G729A_old_exc[G729A_L_FRAME+G729A_PIT_MAX+G729A_L_INTERPOL];
static Word16 *G729A_exc;

static Word16 G729A_lsp_old[G729A_M]={30000, 26000, 21000, 15000, 8000, 0, -8000,-15000,-21000,-26000};
static Word16 G729A_lsp_old_q[G729A_M];
	
static Word16  G729A_mem_w0[G729A_M], G729A_mem_w[G729A_M], G729A_mem_zero[G729A_M];
static Word16  G729A_sharp;
	
void G729AInit_Coder_ld8a(void)
{
		
	G729A_new_speech = G729A_old_speech + G729A_L_TOTAL - G729A_L_FRAME;         
	G729A_speech     = G729A_new_speech - G729A_L_NEXT;                    
	G729A_p_window   = G729A_old_speech + G729A_L_TOTAL - G729A_L_WINDOW;       
		
	G729A_wsp    = G729A_old_wsp + G729A_PIT_MAX;
	G729A_exc    = G729A_old_exc + G729A_PIT_MAX + G729A_L_INTERPOL;

	G729ASet_zero4(G729A_old_speech, G729A_L_TOTAL);			
	G729ASet_zero2(G729A_old_exc, G729A_PIT_MAX + G729A_L_INTERPOL); 
	G729ASet_zero(G729A_old_wsp, G729A_PIT_MAX);				
	G729ASet_zero2(G729A_mem_w,   G729A_M);						
	G729ASet_zero2(G729A_mem_w0,  G729A_M);
	G729ASet_zero2(G729A_mem_zero,G729A_M);
	G729A_sharp = G729A_SHARPMIN;
				
	G729ACopy2(G729A_lsp_old, G729A_lsp_old_q, G729A_M);
	G729ALsp_encw_reset();
	G729AInit_exc_err();
		
	return;
}
	
void G729ACoder_ld8a(Word16 ana[])
{
	Word16 Aq_t[(G729A_MP1)*2];          
	Word16 Ap_t[(G729A_MP1)*2];         
	Word16 *Aq, *Ap;                          

	Word16 h1[G729A_L_SUBFR];             
	Word16 xn[G729A_L_SUBFR];           
	Word16 xn2[G729A_L_SUBFR];            
	Word16 code[G729A_L_SUBFR];                
	Word16 y1[G729A_L_SUBFR];           
	Word16 y2[G729A_L_SUBFR];       
	Word16 g_coeff[4];                   
		
	Word16 g_coeff_cs[5];
	Word16 exp_g_coeff_cs[5];     
								   		
	Word16 i, j, k, i_subfr;
	Word16 T_op, T0, T0_min, T0_max, T0_frac;
	Word16 gain_pit, gain_code, index;
	Word16 temp, taming;
	Word32 L_temp;

	{
		Word16 r_l[G729A_MP1], r_h[G729A_MP1];               
		Word16 rc[G729A_M];                                
		Word16 lsp_new[G729A_M], lsp_new_q[G729A_M];                
			
		G729AAutocorr(G729A_p_window, G729A_M, r_h, r_l);               
		G729ALag_window(G729A_M, r_h, r_l);                          
		G729ALevinson(r_h, r_l, Ap_t, rc);                 
	
		G729AAz_lsp(Ap_t, lsp_new, G729A_lsp_old);           
		
		G729AQua_lsp(lsp_new, lsp_new_q, ana);
		ana += 2;                         

		G729AInt_qlpc(G729A_lsp_old_q, lsp_new_q, Aq_t);
			
		G729AWeight_Az(&Aq_t[0],   G729A_GAMMA1, G729A_M, &Ap_t[0]);
		G729AWeight_Az(&Aq_t[G729A_MP1], G729A_GAMMA1, G729A_M, &Ap_t[G729A_MP1]);
			
		G729ACopy2(lsp_new,   G729A_lsp_old,   G729A_M);
		G729ACopy2(lsp_new_q, G729A_lsp_old_q, G729A_M);
	}
		
	G729AResidu(&Aq_t[0], &G729A_speech[0], &G729A_exc[0], G729A_L_SUBFR);
	G729AResidu(&Aq_t[G729A_MP1], &G729A_speech[G729A_L_SUBFR], &G729A_exc[G729A_L_SUBFR], G729A_L_SUBFR);
		
	{
		Word16 Ap1[G729A_MP1];
			
		Ap = Ap_t;
		Ap1[0] = 4096;

		for(i=1; i<=G729A_M; i++)    
			Ap1[i] = G729Asub_s(Ap[i], G729Amult(Ap[i-1], 22938));
		
		G729ASyn_filt(Ap1, &G729A_exc[0], &G729A_wsp[0], G729A_L_SUBFR, G729A_mem_w, 1);
			
		Ap += G729A_MP1;

		for(i=1; i<=G729A_M; i++)    
			Ap1[i] = G729Asub_s(Ap[i], G729Amult(Ap[i-1], 22938));
		
		G729ASyn_filt(Ap1, &G729A_exc[G729A_L_SUBFR], &G729A_wsp[G729A_L_SUBFR], G729A_L_SUBFR, G729A_mem_w, 1);
	}
		
	T_op = G729APitch_ol_fast(G729A_wsp, G729A_PIT_MAX, G729A_L_FRAME);
		
	T0_min = G729Asub_s(T_op, 3);
	if (G729Asub_s(T0_min,G729A_PIT_MIN)<0) 
	{
		T0_min = G729A_PIT_MIN;
	}
		
	T0_max = G729Aadd_s(T0_min, 6);
	if (G729Asub_s(T0_max ,G729A_PIT_MAX)>0)
	{
		T0_max = G729A_PIT_MAX;
		T0_min = G729Asub_s(T0_max, 6);
	}
			
	Aq = Aq_t;     
	Ap = Ap_t;              
		
	for (i_subfr = 0;  i_subfr < G729A_L_FRAME; i_subfr += G729A_L_SUBFR)
	{
		h1[0] = 4096;
		G729ASet_zero(&h1[1], G729A_L_SUBFR-1);
		G729ASyn_filt(Ap, h1, h1, G729A_L_SUBFR, &h1[1], 0);
		G729ASyn_filt(Ap, &G729A_exc[i_subfr], xn, G729A_L_SUBFR, G729A_mem_w0, 0);
			
		T0 = G729APitch_fr3_fast(&G729A_exc[i_subfr], xn, h1, G729A_L_SUBFR, T0_min, T0_max,
		                	i_subfr, &T0_frac);
			
		index = G729AEnc_lag3(T0, T0_frac, &T0_min, &T0_max,G729A_PIT_MIN,G729A_PIT_MAX,i_subfr);
			
		*ana++ = index;
			
		if (i_subfr == 0) 
		{
			*ana++ = G729AParity_Pitch(index);
		}

		G729ASyn_filt(Ap, &G729A_exc[i_subfr], y1, G729A_L_SUBFR, G729A_mem_zero, 0);
			
		gain_pit = G729AG_pitch(xn, y1, g_coeff, G729A_L_SUBFR);
			
		taming = G729Atest_err(T0, T0_frac);
			
		if( taming == 1)
		{
			if (G729Asub_s(gain_pit, G729A_GPCLIP) > 0) 
			{
				gain_pit = G729A_GPCLIP;
			}
		}
		for (i = 0; i < G729A_L_SUBFR; i++)
		{
			L_temp = _smpy(y1[i], gain_pit);
			L_temp = _sshl(L_temp, 1);                
			xn2[i] = _ssub(xn[i], L_temp>>16);
		}	
			
		index = G729AACELP_Code_A(xn2, h1, T0, G729A_sharp, code, y2, &i);
			
		*ana++ = index;         
		*ana++ = i;              
			
		g_coeff_cs[0]     = g_coeff[0];            
		exp_g_coeff_cs[0] = G729Anegate(g_coeff[1]);    
		g_coeff_cs[1]     = G729Anegate(g_coeff[2]);    
		exp_g_coeff_cs[1] = G729Anegate(G729Aadd_s(g_coeff[3], 1));  

		G729ACorr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs );   
			
		*ana++ = G729AQua_gain(code, g_coeff_cs, exp_g_coeff_cs,
	                      G729A_L_SUBFR, &gain_pit, &gain_code, taming);
						
		G729A_sharp = gain_pit;
		G729A_sharp = _min2(G729A_SHARPMAX, G729A_sharp);
		G729A_sharp = _max2(G729A_SHARPMIN, G729A_sharp);	
		for (i = 0; i < G729A_L_SUBFR;  i++)
		{              
			L_temp = _smpy(G729A_exc[i+i_subfr], gain_pit);
			L_temp = _sadd(L_temp, _smpy(code[i], gain_code));
			L_temp = _sshl(L_temp, 1);
			G729A_exc[i+i_subfr] = (Word16)(_sadd(L_temp, (Word32)0x00008000L)>>16);
		}
	
		G729Aupdate_exc_err(gain_pit, T0);
		for (i = G729A_L_SUBFR-G729A_M, j = 0; i < G729A_L_SUBFR; i++, j++)
		{
			temp       = _sshl(_smpy(y1[i], gain_pit),  1) >> 16;
			k          = _sshl(_smpy(y2[i], gain_code), 2) >> 16;
			G729A_mem_w0[j]  = _ssub(xn[i], _sadd(temp, k));
		}	
		Aq += G729A_MP1;           
		Ap += G729A_MP1;
			
}
  
    G729ACopy4(&G729A_old_speech[G729A_L_FRAME], &G729A_old_speech[0], G729A_L_TOTAL-G729A_L_FRAME);
    G729ACopy(&G729A_old_wsp[G729A_L_FRAME], &G729A_old_wsp[0], G729A_PIT_MAX);
    G729ACopy2(&G729A_old_exc[G729A_L_FRAME], &G729A_old_exc[0], G729A_PIT_MAX+G729A_L_INTERPOL);
  
    return;
}