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

static Word16 G729Ares2_buf[G729A_PIT_MAX+G729A_L_SUBFR];
static Word16 *G729Ares2;
static Word16 G729Ascal_res2_buf[G729A_PIT_MAX+G729A_L_SUBFR];
static Word16 *G729Ascal_res2;

static Word16 G729Amem_syn_pst[G729A_M];

void G729AInit_Post_Filter(void)
{
	G729Ares2  = G729Ares2_buf + G729A_PIT_MAX;
	G729Ascal_res2  = G729Ascal_res2_buf + G729A_PIT_MAX;
	
	G729ASet_zero(G729Amem_syn_pst, G729A_M);
	G729ASet_zero(G729Ares2_buf, G729A_PIT_MAX+G729A_L_SUBFR);
	G729ASet_zero(G729Ascal_res2_buf, G729A_PIT_MAX+G729A_L_SUBFR);
	
	return;
}

void G729APost_Filter(Word16 *syn, Word16 *Az_4, Word16 *T)
{
	
	Word16 res2_pst[G729A_L_SUBFR]; 
	Word16 syn_pst[G729A_L_FRAME];  
	
	Word16 Ap3[G729A_MP1], Ap4[G729A_MP1];  
	
	Word16 *Az;                          
	Word16   t0_max, t0_min;   
	Word16   i_subfr;            
	
	Word16 h[G729A_L_H];
	
	Word16  i, j;
	Word16  temp1, temp2;
	Word32  L_tmp;
	Word32  *Temp0, *Temp1;
	
    Az = Az_4;
	
    for (i_subfr = 0; i_subfr < G729A_L_FRAME; i_subfr += G729A_L_SUBFR)
    {
		t0_min = G729Asub_s(*T++, 3);
		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);
		}
		
		G729AWeight_Az(Az, G729A_GAMMA2_PST, G729A_M, Ap3);
		G729AWeight_Az(Az, G729A_GAMMA1_PST, G729A_M, Ap4);
		
		G729AResidu(Ap3, &syn[i_subfr], G729Ares2, G729A_L_SUBFR);
		
		Temp0 = (Word32 *)&G729Ares2[0];
		Temp1 = (Word32 *)&G729Ascal_res2[0];
		for (j = G729A_L_SUBFR>>1; j > 0; j--)
		{
			*Temp1++ = _shr2(*Temp0++, 2);
		}
		
		G729Apit_pst_filt(G729Ares2, G729Ascal_res2, t0_min, t0_max, G729A_L_SUBFR, res2_pst);
		
		G729ACopy(Ap3, h, G729A_M+1);
		G729ASet_zero(&h[G729A_M+1], G729A_L_H-G729A_M-1);
		G729ASyn_filt(Ap4, h, h, G729A_L_H, &h[G729A_M+1], 0);
		
		L_tmp = G729AL_mult(h[0], h[0]);
		for (i=1; i<G729A_L_H; i++) L_tmp = G729AL_mac(L_tmp, h[i], h[i]);
		temp1 = G729Aextract_h(L_tmp);
		
		L_tmp = G729AL_mult(h[0], h[1]);
		for (i=1; i<G729A_L_H-1; i++) L_tmp = G729AL_mac(L_tmp, h[i], h[i+1]);
		temp2 = G729Aextract_h(L_tmp);
		
		if(temp2 <= 0) {
			temp2 = 0;
		}
		else {
			temp2 = G729Amult(temp2, G729A_MU);
			temp2 = G729Adiv_s(temp2, temp1);
		}
		
		G729Apreemphasis(res2_pst, temp2, G729A_L_SUBFR);
		
		G729ASyn_filt(Ap4, res2_pst, &syn_pst[i_subfr], G729A_L_SUBFR, G729Amem_syn_pst, 1);
		
		G729Aagc(&syn[i_subfr], &syn_pst[i_subfr], G729A_L_SUBFR);
		
		G729ACopy(&G729Ares2[G729A_L_SUBFR-G729A_PIT_MAX], &G729Ares2[-G729A_PIT_MAX], G729A_PIT_MAX);
		G729ACopy(&G729Ascal_res2[G729A_L_SUBFR-G729A_PIT_MAX], &G729Ascal_res2[-G729A_PIT_MAX], G729A_PIT_MAX);
		
		Az += G729A_MP1;
    }
	
    G729ACopy(&syn[G729A_L_FRAME-G729A_M], &syn[-G729A_M], G729A_M);
	
    G729ACopy(syn_pst, syn, G729A_L_FRAME);
	
    return;
}

void G729Apit_pst_filt(Word16 *signal, Word16 *scal_sig, Word16 t0_min, Word16 t0_max, Word16 L_subfr, Word16 *signal_pst)
{
	Word16 i, j, t0;
	Word16 g0, gain, cmax, en, en0;
	Word16 *p, *p1, *deb_sig;
	Word32 corr, cor_max, ener, ener0, temp;
	Word32 L_temp;
	
	deb_sig = &scal_sig[-t0_min];
	cor_max = G729AMIN_32;
	t0 = t0_min;              
	for (i=t0_min; i<=t0_max; i++)
	{
		corr = 0;
		p    = scal_sig;
		p1   = deb_sig;
		for (j=0; j<L_subfr; j++)
			corr = G729AL_mac(corr, *p++, *p1++);
		
		L_temp = G729AL_sub(corr, cor_max);
		if (L_temp > (Word32)0)
		{
			cor_max = corr;
			t0 = i;
		}
		deb_sig--;
	}
	
	ener = 1;
	p = scal_sig - t0;
	for ( i=0; i<L_subfr ;i++, p++)
		ener = G729AL_mac(ener, *p, *p);
	
	ener0 = 1;
	p = scal_sig;
	for ( i=0; i<L_subfr; i++, p++)
		ener0 = G729AL_mac(ener0, *p, *p);
	
	if (cor_max < 0)
	{
		cor_max = 0;
	}
	
	temp = cor_max;
	if (ener > temp)
	{
		temp = ener;
	}
	if (ener0 > temp)
	{
		temp = ener0;
	}
	j = G729Anorm_l(temp);
	cmax = G729Around(G729AL_shl(cor_max, j));
	en = G729Around(G729AL_shl(ener, j));
	en0 = G729Around(G729AL_shl(ener0, j));

	temp = G729AL_mult(cmax, cmax);
	temp = G729AL_sub(temp, G729AL_shr(G729AL_mult(en, en0), 1));
	
	if (temp < (Word32)0)            
	{                              
		for (i = 0; i < L_subfr; i++)
			signal_pst[i] = signal[i];
		return;
	}
	
	if (G729Asub_s(cmax, en) > 0)     
	{
		g0 = G729A_INV_GAMMAP;
		gain = G729A_GAMMAP_2;
	}
	else {
		cmax = G729Ashr_s(G729Amult(cmax, G729A_GAMMAP), 1); 
		en = G729Ashr_s(en, 1);        
		i = G729Aadd_s(cmax, en);
		if(i > 0)
		{
			gain = G729Adiv_s(cmax, i);    
			g0 = G729Asub_s(32767, gain);  
		}
		else
		{
			g0 =  32767;
			gain = 0;
		}
	}
		
	for (i = 0; i < L_subfr; i++)
	{
		signal_pst[i] = G729Aadd_s(G729Amult(g0, signal[i]), G729Amult(gain, signal[i-t0]));
		
	}
	
	return;
}

void G729Apreemphasis(Word16 *signal, Word16 g, Word16 L)
{
	static Word16 G729A_mem_pre = 0;
	Word16 *p1, *p2, temp, i;
	
	p1 = signal + L - 1;
	p2 = p1 - 1;
	temp = *p1;
	
	for (i = 0; i <= L-2; i++)
	{
		*p1-- = G729Asub_s(*p1, G729Amult(g, *p2--));
	}
	
	*p1 = G729Asub_s(*p1, G729Amult(g, G729A_mem_pre));
	
	G729A_mem_pre = temp;
	
	return;
}

void G729Aagc(Word16 *sig_in, Word16 *sig_out, Word16 l_trm)
{
	static Word16 G729A_past_gain=4096;         
	Word16 i, exp;
	Word16 gain_in, gain_out, g0, gain;                    
	Word32 s;
	
	Word16 signal[G729A_L_SUBFR];
	
	for(i=0; i<l_trm; i++)
		signal[i] = G729Ashr_s(sig_out[i], 2);
	
	s = 0;
	for(i=0; i<l_trm; i++)
		s = G729AL_mac(s, signal[i], signal[i]);
	
	if (s == 0) {
		G729A_past_gain = 0;
		return;
	}
	exp = G729Asub_s(G729Anorm_l(s), 1);
	gain_out = G729Around(G729AL_shl(s, exp));
	
	for(i=0; i<l_trm; i++)
		signal[i] = G729Ashr_s(sig_in[i], 2);
	
	s = 0;
	for(i=0; i<l_trm; i++)
		s = G729AL_mac(s, signal[i], signal[i]);
	
	if (s == 0) {
		g0 = 0;
	}
	else {
		i = G729Anorm_l(s);
		gain_in = G729Around(G729AL_shl(s, i));
		exp = G729Asub_s(exp, i);
		
		s = G729AL_deposit_l(G729Adiv_s(gain_out,gain_in));  
		s = G729AL_shl(s, 7);          
		s = G729AL_shr(s, exp);       
		
		s = G729AInv_sqrt(s);         
		i = G729Around(G729AL_shl(s,9));   
		
		g0 = G729Amult(i, G729A_AGC_FAC1);       
	}
		
	gain = G729A_past_gain;
	for(i=0; i<l_trm; i++) {
		gain = G729Amult(gain, G729A_AGC_FAC);
		gain = G729Aadd_s(gain, g0);
		sig_out[i] = G729Aextract_h(G729AL_shl(G729AL_mult(sig_out[i], gain), 3));
	}
	G729A_past_gain = gain;
	
	return;
}