#include "SamsungSolomonVoiceW_AGC.h"



void SolomonVoiceWAGCInit(AGCStatus* AGC_buf, SamsungSolomonVoiceWTxAGCParam* AGC_param)

{

	short	*TxAGCAT, *TxAGCTH, *TxAGCSL, *TxAGCMB;



	/**** Buffer *****/

	AGC_buf->AGC_xd_level = TX_XD_LEVEL_V;

	AGC_buf->AGC_G_old = TX_G_OLD_V;

	

	/***** Param ******/

	AGC_param->denom = 0;

	AGC_param->size = 0;



	AGC_param->agc_comThd = 0;

	AGC_param->agc_comSlope = 0;

	AGC_param->agc_expThd = 0;

	AGC_param->agc_expSlope = 0;



	TxAGCAT = (short *)TxAGC_Adaptime;

	TxAGCTH = (short *)TxAGC_Threshold;

	TxAGCSL = (short *)TxAGC_Slope;

	TxAGCMB = (short *)TxAGC_MaxBoost;



	AGC_param->latt = *TxAGCAT++;

	AGC_param->lrlt = *TxAGCAT++;

	AGC_param->gat = *TxAGCAT++;

	AGC_param->grt = *TxAGCAT++;

	AGC_param->compthd = *TxAGCTH++;

	AGC_param->expthd = *TxAGCTH++;

	AGC_param->compslop = *TxAGCSL++;

	AGC_param->expslop = *TxAGCSL++;

	AGC_param->maxboost1 = *TxAGCMB++;

	AGC_param->maxboost2 = *TxAGCMB++;



	return;

}



void SolomonVoiceW_AGC_ParamConfig(SamsungSolomonVoiceWTxAGCParam* AGC_param, SamsungSolomonVoiceWTxAGCParam* param)

{

	short compthreshold, compSlope, expthreshold, expSlope;



	AGC_param->denom = param->denom;

	AGC_param->size = param->size;



	compthreshold = param->agc_comThd;

	compSlope = param->agc_comSlope;

	expthreshold = param->agc_expThd;

	expSlope = param->agc_expSlope;



	if (compthreshold < 0) {

		compthreshold = 0;

	}

	if (compthreshold > 54) {

		compthreshold = 54;

	}

	if (compSlope < 10) {

		compSlope = 10;

	}

	if (compSlope > 100) {

		compSlope = 100;

	}

	if (expthreshold < 55) {

		expthreshold = 55;

	}

	if (expthreshold > 90) {

		expthreshold = 90;

	}

	if (expSlope < 10) {

		expSlope = 10;

	}

	if (expSlope > 100) {

		expSlope = 100;

	}



	AGC_param->compthd = TxAGC_Comp_thrd[compthreshold];

	AGC_param->compslop = TxAGC_Comp_slope[compSlope / 5 - 2];

	AGC_param->expthd = TxAGC_Exp_thrd[expthreshold - 55];

	AGC_param->expslop = TxAGC_Exp_slope[expSlope / 5 - 2];



	return;

}



void FN_SOLOMONVOICEW_TX_AGC(short *In, AGCStatus* AGC_buf, SamsungSolomonVoiceWTxAGCParam* AGC_param)

{

	//AGC parameters

	short denom = AGC_param->denom;

	short size = AGC_param->size;



	short	i;

	short	S_tmp;

	long	L_tmp = 0;



	short	G;

	short	S_Tal;

	long	L_ener;



	short	lat, lrt, gat, grt;

	short	xd_level, G_old;

	short	thd1, thd2;

	short	slp1, slp2;

	short	maxboost1, maxboost2;

	short	exp = 0, fract = 0, out = 0;

	short	OverFcount = 0;

	short	mix_size = 0;



	mix_size = size >> 1;



	lat = AGC_param->latt;

	lrt = AGC_param->lrlt;

	gat = AGC_param->gat;

	grt = AGC_param->grt;



	thd1 = AGC_param->compthd;

	slp1 = AGC_param->compslop;

	thd2 = AGC_param->expthd;

	slp2 = AGC_param->expslop;



	maxboost1 = AGC_param->maxboost1;

	maxboost2 = AGC_param->maxboost2;



	

	xd_level = AGC_buf->AGC_xd_level;

	G_old = AGC_buf->AGC_G_old;

	

	// Kim RMS

	L_ener = 0;

	OverFcount = 1;

#ifndef	FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT1

	for (i = 0; i < size; i++)

	{

		S_tmp = In[i];

		L_tmp = DVTXOP_L_mult(S_tmp, S_tmp);

		L_tmp = DVTXOP_L_shr(L_tmp, OverFcount);

		L_ener = DVTXOP_L_add(L_ener, L_tmp);



		if (L_ener > 0x40000000)

		{

			L_ener = DVTXOP_L_shr(L_ener, 1);

			OverFcount++;

		}

	}

#else

	L_ener = Fx_vector_sum_squares(In, size, &OverFcount);

#endif

	OverFcount--;

	L_ener = DVTXOP_L_shl(L_ener, 1);

	L_ener = DVTXOP_L_mpy_ls(L_ener, denom);



	DVTXOP_Log2(L_ener, &exp, &fract);

	exp = DVTXOP_add(exp, OverFcount);

	out = DVTXOP_shl(DVTXOP_sub(exp, 30), 9);

	out = DVTXOP_add(out, DVTXOP_shr(fract, 6));	//Q9		

	S_tmp = DVTXOP_mult(out, C10LOG2); 				//Q7		//  10log10(x) = 10log10(2) * log2(x) = 3.0103  * log2(x)

	S_tmp = DVTXOP_add(S_tmp, -384);				//Q7



	if (S_tmp > xd_level)

		S_Tal = lat;

	else

		S_Tal = lrt;



	L_tmp = DVTXOP_L_mult(S_Tal, S_tmp);			// Q16.15 * Q16.

	S_tmp = DVTXOP_sub(0x7fff, S_Tal);				// Q16.15

	L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, xd_level);

	S_tmp = DVTXOP_round(L_tmp);

	xd_level = S_tmp;



	// Apply Static Curve

	if (S_tmp > thd1)								// Q7

	{

		// G = -(slop_com1)*(x_level-thd1)

		S_tmp = DVTXOP_sub(thd1, S_tmp);			// Q16.7 = Q16.7 - Q16.7

		L_tmp = DVTXOP_L_mult(S_tmp, slp1);		// Q32.23 = Q16.7 * Q16.15

		L_tmp = DVTXOP_L_shl(L_tmp, 4);			// Q32.23

		S_tmp = DVTXOP_round(L_tmp);			// Q16.7

		G = DVTXOP_add(S_tmp, maxboost1);

	}

	else if (S_tmp > thd2)

	{

		G = maxboost1;

	}

	else

	{

		//G = -(slop_com2)*(x_level-thd2)+MAXGAIN;							// Q16.7

		S_tmp = DVTXOP_sub(thd2, S_tmp);			// Q16.7 = Q16.7 - Q16.7

		L_tmp = DVTXOP_L_mult(S_tmp, slp2);		// Q32.19 = Q16.7 * Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);			// Q32.23

		S_tmp = DVTXOP_round(L_tmp);			// Q16.7

		G = DVTXOP_add(S_tmp, maxboost1);

	}



	G = G - maxboost1;



	if (G == 0)

	{

		G = 32767;

	}

	else

	{

		out = DVTXOP_mult(Gain20_2Q15, G); //Gain20_2Q15=1638

		L_tmp = DVTXOP_L_deposit_h(out);

		L_tmp = DVTXOP_L_shl(L_tmp, 3);

		L_tmp = DVTXOP_fnExp10(L_tmp);

		G = DVTXOP_round(L_tmp);			//Q15

	}



	if (G < G_old)

		S_Tal = gat;

	else

		S_Tal = grt;



	L_tmp = DVTXOP_L_mult(S_Tal, G);					// Q16.15 * Q16.

	S_tmp = DVTXOP_sub(0x7fff, S_Tal);				// Q16.15

	L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old);

	S_tmp = DVTXOP_round(L_tmp);

	G = S_tmp;

#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2

	for (i = 0; i < mix_size; i++)

	{

		L_tmp = DVTXOP_L_mult(TXAGCSm_WB[i], G);					// Q16.15 * Q16.

		S_tmp = DVTXOP_sub(0x7fff, TXAGCSm_WB[i]);				// Q16.15

		L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(In[i], S_tmp);

		S_tmp = DVTXOP_round(L_tmp);								// Q16.15



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In[i] = DVTXOP_round(L_tmp);

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT2(TXAGCSm_WB,In,G,G_old,maxboost2,mix_size);

#endif



#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2

	for (i = mix_size; i < size; i++)

	{

		L_tmp = DVTXOP_L_mult(In[i], G);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In[i] = DVTXOP_round(L_tmp);								// Q16.15

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT3(&In[mix_size],G,maxboost2, size - mix_size);

#endif

	G_old = G;



	AGC_buf->AGC_xd_level = xd_level;

	AGC_buf->AGC_G_old = G_old;				// 0x800 = Q11(1)

	

	return;

}



void FN_SOLOMONVOICEW_TX_AGC_2ch(short *In1, short *In2, AGCStatus* AGC_buf, SamsungSolomonVoiceWTxAGCParam* AGC_param)

{

	//AGC parameters

	short denom = AGC_param->denom;

	short size = AGC_param->size;



	short	i;

	short	S_tmp;

	long	L_tmp = 0;



	short	G;

	short	S_Tal;

	long	L_ener, L_ener_ch2;



	short	lat, lrt, gat, grt;

	short	xd_level, G_old;

	short	thd1, thd2;

	short	slp1, slp2;

	short	maxboost1, maxboost2;

	short	exp = 0, fract = 0, out = 0;

#ifndef KHW_OPTI_20191216

	short	OverFcount = 0;

	short	OverFcount_ch2 = 0;	

#else

	short	OverFcount;

	short	OverFcount_ch2;	

#endif

	short	mix_size = 0;



	mix_size = size >> 1;



	lat = AGC_param->latt;

	lrt = AGC_param->lrlt;

	gat = AGC_param->gat;

	grt = AGC_param->grt;



	thd1 = AGC_param->compthd;

	slp1 = AGC_param->compslop;

	thd2 = AGC_param->expthd;

	slp2 = AGC_param->expslop;



	maxboost1 = AGC_param->maxboost1;

	maxboost2 = AGC_param->maxboost2;





	xd_level = AGC_buf->AGC_xd_level;

	G_old = AGC_buf->AGC_G_old;



	// RMS (ch1)

	L_ener = 0;

	OverFcount = 1;

#ifndef	FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT1

	for (i = 0; i < size; i++)

	{

		S_tmp = In1[i];

		L_tmp = DVTXOP_L_mult(S_tmp, S_tmp);

		L_tmp = DVTXOP_L_shr(L_tmp, OverFcount);

		L_ener = DVTXOP_L_add(L_ener, L_tmp);



		if (L_ener > 0x40000000)

		{

			L_ener = DVTXOP_L_shr(L_ener, 1);

			OverFcount++;

		}

	}

#else

	L_ener = Fx_vector_sum_squares(In1, size, &OverFcount);

#endif

	OverFcount--;

	L_ener = DVTXOP_L_shl(L_ener, 1);

	L_ener = DVTXOP_L_mpy_ls(L_ener, denom);



	// RMS (ch2)

	L_ener_ch2 = 0;

	OverFcount_ch2 = 1;

#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT1

	for (i = 0; i < size; i++)

	{

		S_tmp = In1[i];

		L_tmp = DVTXOP_L_mult(S_tmp, S_tmp);

		L_tmp = DVTXOP_L_shr(L_tmp, OverFcount_ch2);

		L_ener_ch2 = DVTXOP_L_add(L_ener_ch2, L_tmp);



		if (L_ener_ch2 > 0x40000000)

		{

			L_ener_ch2 = DVTXOP_L_shr(L_ener_ch2, 1);

			OverFcount_ch2++;

		}

	}

#else

	L_ener_ch2 = Fx_vector_sum_squares(In1, size, &OverFcount_ch2);

#endif

	OverFcount_ch2--;

	L_ener_ch2 = DVTXOP_L_shl(L_ener_ch2, 1);

	L_ener_ch2 = DVTXOP_L_mpy_ls(L_ener_ch2, denom);



	// RMS channel select

	if (OverFcount > OverFcount_ch2)

	{

		S_tmp = OverFcount - OverFcount_ch2;

		L_tmp = DVTXOP_L_shr(L_ener_ch2, S_tmp);

		if (L_ener < L_tmp)

		{

			L_ener = L_ener_ch2;

			OverFcount = OverFcount_ch2;

		}

	}

	else

	{

		S_tmp = OverFcount_ch2 - OverFcount;

		L_tmp = DVTXOP_L_shr(L_ener, S_tmp);

		if (L_tmp < L_ener_ch2)

		{

			L_ener = L_ener_ch2;

			OverFcount = OverFcount_ch2;

		}

	}



	// gain

	DVTXOP_Log2(L_ener, &exp, &fract);

	exp = DVTXOP_add(exp, OverFcount);

	out = DVTXOP_shl(DVTXOP_sub(exp, 30), 9);

	out = DVTXOP_add(out, DVTXOP_shr(fract, 6));	//Q9		

	S_tmp = DVTXOP_mult(out, C10LOG2); 				//Q7		//  10log10(x) = 10log10(2) * log2(x) = 3.0103  * log2(x)

	S_tmp = DVTXOP_add(S_tmp, -384);				//Q7



	if (S_tmp > xd_level)

		S_Tal = lat;

	else

		S_Tal = lrt;



	L_tmp = DVTXOP_L_mult(S_Tal, S_tmp);			// Q16.15 * Q16.

	S_tmp = DVTXOP_sub(0x7fff, S_Tal);				// Q16.15

	L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, xd_level);

	S_tmp = DVTXOP_round(L_tmp);

	xd_level = S_tmp;



	// Apply Static Curve

	if (S_tmp > thd1)								// Q7

	{

		// G = -(slop_com1)*(x_level-thd1)

		S_tmp = DVTXOP_sub(thd1, S_tmp);			// Q16.7 = Q16.7 - Q16.7

		L_tmp = DVTXOP_L_mult(S_tmp, slp1);		// Q32.23 = Q16.7 * Q16.15

		L_tmp = DVTXOP_L_shl(L_tmp, 4);			// Q32.23

		S_tmp = DVTXOP_round(L_tmp);			// Q16.7

		G = DVTXOP_add(S_tmp, maxboost1);

	}

	else if (S_tmp > thd2)

	{

		G = maxboost1;

	}

	else

	{

		//G = -(slop_com2)*(x_level-thd2)+MAXGAIN;							// Q16.7

		S_tmp = DVTXOP_sub(thd2, S_tmp);			// Q16.7 = Q16.7 - Q16.7

		L_tmp = DVTXOP_L_mult(S_tmp, slp2);		// Q32.19 = Q16.7 * Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);			// Q32.23

		S_tmp = DVTXOP_round(L_tmp);			// Q16.7

		G = DVTXOP_add(S_tmp, maxboost1);

	}



	G = G - maxboost1;



	if (G == 0)

	{

		G = 32767;

	}

	else

	{

		out = DVTXOP_mult(Gain20_2Q15, G); //Gain20_2Q15=1638

		L_tmp = DVTXOP_L_deposit_h(out);

		L_tmp = DVTXOP_L_shl(L_tmp, 3);

		L_tmp = DVTXOP_fnExp10(L_tmp);

		G = DVTXOP_round(L_tmp);			//Q15

	}



	if (G < G_old)

		S_Tal = gat;

	else

		S_Tal = grt;



	L_tmp = DVTXOP_L_mult(S_Tal, G);					// Q16.15 * Q16.

	S_tmp = DVTXOP_sub(0x7fff, S_Tal);				// Q16.15

	L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old);

	S_tmp = DVTXOP_round(L_tmp);

	G = S_tmp;



	// apply gain (ch1)

#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2

	for (i = 0; i < mix_size; i++)

	{

		L_tmp = DVTXOP_L_mult(TXAGCSm_WB[i], G);					// Q16.15 * Q16.

		S_tmp = DVTXOP_sub(0x7fff, TXAGCSm_WB[i]);				// Q16.15

		L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(In1[i], S_tmp);

		S_tmp = DVTXOP_round(L_tmp);								// Q16.15



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In1[i] = DVTXOP_round(L_tmp);

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT2(TXAGCSm_WB,In1,G,G_old,maxboost2,mix_size);

#endif



#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT3

	for (i = mix_size; i < size; i++)

	{

		L_tmp = DVTXOP_L_mult(In1[i], G);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In1[i] = DVTXOP_round(L_tmp);								// Q16.15

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT3(&In1[mix_size],G,maxboost2, size - mix_size);

#endif

	// apply gain (ch2)

#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2

	for (i = 0; i < mix_size; i++)

	{

		L_tmp = DVTXOP_L_mult(TXAGCSm_WB[i], G);					// Q16.15 * Q16.

		S_tmp = DVTXOP_sub(0x7fff, TXAGCSm_WB[i]);				// Q16.15

		L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(In2[i], S_tmp);

		S_tmp = DVTXOP_round(L_tmp);								// Q16.15



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In2[i] = DVTXOP_round(L_tmp);

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT2(TXAGCSm_WB,In2,G,G_old,maxboost2,mix_size);

#endif



#ifndef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2

	for (i = mix_size; i < size; i++)

	{

		L_tmp = DVTXOP_L_mult(In2[i], G);

		S_tmp = DVTXOP_round(L_tmp);



		L_tmp = DVTXOP_L_mult(S_tmp, maxboost2);				// Q32.27 = Q16.15*Q16.11

		L_tmp = DVTXOP_L_shl(L_tmp, 4);							// Q32.31

		In2[i] = DVTXOP_round(L_tmp);								// Q16.15

	}

#else

	FN_SOLOMONVOICEW_TX_AGC_2ch_OPT3(&In2[mix_size],G,maxboost2, size - mix_size);

#endif

	G_old = G;



	AGC_buf->AGC_xd_level = xd_level;

	AGC_buf->AGC_G_old = G_old;				// 0x800 = Q11(1)



	return;

}