#include "SamsungSolomonVoiceW_MBDRC.h"



#if(FLAG_SELECT_C_DSP == 1)

#include <stdio.h>

#include <stdlib.h>

extern FILE		*fp_dbg_MBDRC;

#endif



void SolomonVoiceWMBDRCInit(MBDRCStatus* MBDRC_buf, SamsungSolomonVoiceWTxMBDRCParam* MBDRC_param)

{

	short i;

	

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

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

	{

		MBDRC_buf->gain_band[i] = 0;

		MBDRC_buf->ener_band[i] = 0;

		MBDRC_buf->pre_ener[i] = -11520;

	}

	

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

	MBDRC_param->band_num = 5;



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

	{

		MBDRC_param->limit_thd[i] = 0;

		MBDRC_param->limit_slp[i] = 0;

		MBDRC_param->band_bound[i][0] = 0;

		MBDRC_param->band_bound[i][1] = 0;

		MBDRC_param->band_div[i][0] = 0;

		MBDRC_param->band_div[i][1] = 0;

		MBDRC_param->boostgain[i] = 0;

		MBDRC_param->boostgain_dec[i] = 0;



		MBDRC_param->smfactor_pre_ener[i][0] = 16384;	//0.5

		MBDRC_param->smfactor_pre_ener[i][1] = 300;		//0.0092

		MBDRC_param->smfactor_ener_band[i] = 1181116006;//0.55

	}



	return;

}



void SolomonVoiceW_MBDRC_ParamConfig(SamsungSolomonVoiceWTxMBDRCParam* MBDRC_param, SamsungSolomonVoiceWTxMBDRCParam* param)

{

	short i;

	short bin_num, band_div;

	short max_bin, resol;

	short bandNum;



	if (param->mbdrcBandNum > 5 || param->mbdrcBandNum < 0)

	{

		bandNum = 0;

		MBDRC_param->band_num = 0;



		return -1;

	}

	else

	{

		bandNum = param->mbdrcBandNum;

		MBDRC_param->band_num = param->mbdrcBandNum;

	}



	if (bandNum != 0)

	{

		max_bin = DVTX_FFT_SIZE_256 - 1;

		resol = FFT_RESOL;

		param->mbdrcCrossFreq[bandNum - 1] = Fs;

	}





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

	{

		if (param->mbdrcBandLimit[i] < 0)

			param->mbdrcBandLimit[i] = 0;

		if (param->mbdrcBandLimit[i] > 70)

			param->mbdrcBandLimit[i] = 70;



		if (param->mbdrcBandsGain[i] > 24)

			param->mbdrcBandsGain[i] = 24;

		if (param->mbdrcBandsGain[i] < -24)

			param->mbdrcBandsGain[i] = -24;



		MBDRC_param->limit_thd[i] = MBDRC_THD_COM[param->mbdrcBandLimit[i]];

		MBDRC_param->limit_slp[i] = MBDRC_EXP_SLOP[MBDRC_LIMIT_GAIN_STEP - 1];



		if (param->mbdrcBandsGain[i] < 0)

		{

			MBDRC_param->boostgain[i] = MBDRC_MGAIN_COM[-param->mbdrcBandsGain[i]];

			MBDRC_param->boostgain_dec[i] = MBDRC_MGAIN_COM_DEC[-param->mbdrcBandsGain[i]];

		}

		else

		{

			MBDRC_param->boostgain[i] = MBDRC_GAIN_COM[param->mbdrcBandsGain[i]];

			MBDRC_param->boostgain_dec[i] = MBDRC_GAIN_COM_DEC[param->mbdrcBandsGain[i]];

		}

	}





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

	{

		if (i == 0)

			MBDRC_param->band_bound[0][0] = 1;

		else

			MBDRC_param->band_bound[i][0] = MBDRC_param->band_bound[i - 1][1] + 1;



		if (i == bandNum - 1)

			MBDRC_param->band_bound[bandNum - 1][1] = max_bin;

		else

			MBDRC_param->band_bound[i][1] = (short)(param->mbdrcCrossFreq[i] >> 5);



		bin_num = (MBDRC_param->band_bound[i][1] - MBDRC_param->band_bound[i][0]) + 1;



		band_div = MBDRC_DIV_TABLE[bin_num];

		MBDRC_param->band_div[i][0] = 0;

		MBDRC_param->band_div[i][1] = MBDRC_DIV_TABLE[bin_num];

		if (band_div == 0 || band_div == 1 || band_div == 2 || band_div == 3 || band_div == 4 || band_div == 5 || band_div == 6 || band_div == 7 || band_div == 8)

			MBDRC_param->band_div[i][0] = 1;

	}

	

	/*MBDRC_param->mbdrc_band_gain[0] = param->mbdrcBandsGain[0];

	MBDRC_param->mbdrc_band_gain[1] = param->mbdrcBandsGain[1];

	MBDRC_param->mbdrc_band_gain[2] = param->mbdrcBandsGain[2];

	MBDRC_param->mbdrc_band_gain[3] = param->mbdrcBandsGain[3];

	MBDRC_param->mbdrc_band_gain[4] = param->mbdrcBandsGain[4];



	MBDRC_param->mbdrc_band_limt[0] = param->mbdrcBandLimit[0];

	MBDRC_param->mbdrc_band_limt[1] = param->mbdrcBandLimit[1];

	MBDRC_param->mbdrc_band_limt[2] = param->mbdrcBandLimit[2];

	MBDRC_param->mbdrc_band_limt[3] = param->mbdrcBandLimit[3];

	MBDRC_param->mbdrc_band_limt[4] = param->mbdrcBandLimit[4];*/



	return;

}



void fn_MBDRC_Noise_Param(SamsungSolomonVoiceWTxMBDRCParam* MBDRC_param, MBDRC_Noise_Param* param)

{

	short i;

	short bandNum = MBDRC_param->band_num;



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

	{

		MBDRC_param->smfactor_pre_ener[i][0] = param->smfactor_pre_ener[i][0];

		MBDRC_param->smfactor_pre_ener[i][1] = param->smfactor_pre_ener[i][1];

		MBDRC_param->smfactor_ener_band[i] = param->smfactor_ener_band[i];

	}



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

	{

		if (param->mbdrcBandLimit[i] < 0)

			param->mbdrcBandLimit[i] = 0;

		if (param->mbdrcBandLimit[i] > 70)

			param->mbdrcBandLimit[i] = 70;



		MBDRC_param->limit_thd[i] = MBDRC_THD_COM[param->mbdrcBandLimit[i]];

	}





	return;

}

#ifdef FN_SOLOMONVOICEW_TX_MBDRC_energy_OPT_DSP

int FN_SOLOMONVOICEW_TX_MBDRC_energy_vector(short *xo1, short *xo2, short st, short end )

{

	int i,num, enrg, enrg1, enrg2;

	ae_int16x4 *px1, *px2;

	ae_valign align_x1, align_x2;

	ae_int16x4 tmpx1,tmpx2;

	ae_int32x2 add11, add12, add21, add22;

	px1 = (ae_int16x4 *)&xo1[st];

	px2 = (ae_int16x4 *)&xo2[st];

	align_x1 = AE_LA64_PP(px1);

	align_x2 = AE_LA64_PP(px2);

	add11 = add12 = add21 = add22 = 0;

	num = (end - st)/4;

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

	{

		AE_LA16X4_IP(tmpx1, align_x1, px1);

		AE_LA16X4_IP(tmpx2, align_x2, px2);

		AE_MULAF16X4SS(add11, add12, tmpx1, tmpx1);

		AE_MULAF16X4SS(add21, add22, tmpx2, tmpx2);

	}

	add11 = AE_ADD32S(add11, add12);

	add12 = AE_SEL32_LH(add12, add11);

	add11 = AE_ADD32S(add11, add12);

	enrg1 = AE_MOVAD32_L(add11);



	add21 = AE_ADD32S(add21, add22);

	add22 = AE_SEL32_LH(add22, add21);

	add21 = AE_ADD32S(add21, add22);

	enrg2 = AE_MOVAD32_L(add21);

	for(i = st + num*4; i < end; i++ )

	{

		enrg1 = DVTXOP_L_mac(enrg1, xo1[i], xo1[i]);

		enrg2 = DVTXOP_L_mac(enrg2, xo2[i], xo2[i]);

	}

	enrg = (enrg2 > enrg1) ? enrg2 : enrg1;

	return enrg;

}

#endif

#ifdef FN_SOLOMONVOICEW_TX_MBDRC_shift_OPT_DSP

void Right_Shift_vecter1(short * in, short * out, int shift, int len) // in(i ), out(out)

{

	ae_valign align1, align2;

	ae_int16x4 *pt1, *pt2;

	ae_int16x4 A;

	int i;

	pt1=(ae_int16x4 *)in;

	pt2=(ae_int16x4 *)out;

	align1 = AE_LA64_PP(pt1);

	align2 = AE_ZALIGN64();

	for (i = 0; i < len; i+=4)

	{

		AE_LA16X4_IP(A, align1, pt1);

		A = AE_SRAA16S(A, shift);

		AE_SA16X4_IP(A, align2, pt2);

	}

	AE_SA64POS_FP(align2, pt2);

}

#endif

short ch_gain_bin[MBDRC_FFT_BIN_NUM] = {0,};

short ch_temp_bin[MBDRC_FFT_BIN_NUM] = { 0, };

short ch_energy_db[MBDRC_SUBBAND_NUM] = { 0, };

void FN_SOLOMONVOICEW_TX_MBDRC_2ch(DVTX_ECNS_Cfg_t* DVTX_ECNS_vars)

{

	//static short fmrcnt = 0;

	FRAMEStatus* FRAME_buf = &DVTX_ECNS_vars->FRAME_buf;

	MBDRCStatus* MBDRC_buf = &DVTX_ECNS_vars->MBDRC_buf;



	SamsungSolomonVoiceWTxMBDRCParam* MBDRC_param = &DVTX_ECNS_vars->MBDRC_param;



	short q_diff;

	short FFT_LEN = DVTX_FFT_LEN_WB;

	short fftbin = DVTX_FFT_HALFLEN_WB;



	short BLK_NORM_Outer1, BLK_NORM_Outer2;

	short *Xo1, *Xo2, *MBDRCout_Xo1, *MBDRCout_Xo2;



	short normb_shift;



	short	i, j, j1, j2;

	

	short	exp, fract;

	short	tmp, norm_shift;

	

	short	band_num = MBDRC_param->band_num;

	long	enrg, enrg0, enrg1, L_tmp, Ltmp1, Ltmp2, Ltmp3;

	short	alpha;



	/* 2ch Input setting */

	Xo1 = FRAME_buf->fftbuf_Outer_1.data_buffer;

	Xo2 = FRAME_buf->fftbuf_Outer_2.data_buffer;

	BLK_NORM_Outer1 = FRAME_buf->fftbuf_Outer_1.BLK_NORM;

	BLK_NORM_Outer2 = FRAME_buf->fftbuf_Outer_2.BLK_NORM;



	if (BLK_NORM_Outer1 > BLK_NORM_Outer2)

	{

		q_diff = BLK_NORM_Outer1 - BLK_NORM_Outer2;

#ifndef FN_SOLOMONVOICEW_TX_MBDRC_shift_OPT_DSP

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

			Xo1[i] = DVTXOP_shr(Xo1[i], q_diff);

#else

		Right_Shift_vecter1(Xo1,Xo1,q_diff,FFT_LEN);

#endif



		BLK_NORM_Outer1 = BLK_NORM_Outer2;

		FRAME_buf->fftbuf_Outer_1.BLK_NORM = BLK_NORM_Outer2;

	}

	else if (BLK_NORM_Outer2 > BLK_NORM_Outer1)

	{

		q_diff = BLK_NORM_Outer2 - BLK_NORM_Outer1;

#ifndef FN_SOLOMONVOICEW_TX_MBDRC_shift_OPT_DSP

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

			Xo2[i] = DVTXOP_shr(Xo2[i], q_diff);

#else

		Right_Shift_vecter1(Xo2,Xo2,q_diff,FFT_LEN);

#endif

		BLK_NORM_Outer2 = BLK_NORM_Outer1;

		FRAME_buf->fftbuf_Outer_2.BLK_NORM = BLK_NORM_Outer1;

	}

	

	normb_shift = BLK_NORM_Outer1;



	/* Output setting */

	MBDRCout_Xo1 = FRAME_buf->fftbuf_Outer_1.data_buffer;

	MBDRCout_Xo2 = FRAME_buf->fftbuf_Outer_2.data_buffer;



	for (i = 0; i < MBDRC_FFT_BIN_NUM; i++)	// sbk 1001 for modifying MBDRC when num = 0

		ch_temp_bin[i] = 2048;



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

	{

		enrg = 0;

		enrg0 = 0;

		enrg1 = 0;

		j1 = MBDRC_param->band_bound[i][0];

		j2 = MBDRC_param->band_bound[i][1];

#ifndef FN_SOLOMONVOICEW_TX_MBDRC_energy_OPT_DSP

		for (j = j1; j <= j2; j++)

		{

			enrg0 = DVTXOP_L_mac(enrg0, Xo1[2 * j], Xo1[2 * j]);

			enrg0 = DVTXOP_L_mac(enrg0, Xo1[2 * j + 1], Xo1[2 * j + 1]);



			enrg1 = DVTXOP_L_mac(enrg1, Xo2[2 * j], Xo2[2 * j]);

			enrg1 = DVTXOP_L_mac(enrg1, Xo2[2 * j + 1], Xo2[2 * j + 1]);

		}



		enrg = (enrg1 > enrg0) ? enrg1 : enrg0;

#else

		enrg = FN_SOLOMONVOICEW_TX_MBDRC_energy_vector(Xo1, Xo2, 2*j1, 2*(j2+1));

#endif

		if (MBDRC_param->band_div[i][0] == TURE)

			enrg = DVTXOP_L_shr(enrg, MBDRC_param->band_div[i][1]);

		else

		{

			norm_shift = DVTXOP_norm_l(enrg);

			tmp = (short)((DVTXOP_L_shl(enrg, norm_shift)) >> 16);



			enrg = DVTXOP_L_mult(tmp, MBDRC_param->band_div[i][1]);

			enrg = DVTXOP_L_shr(enrg, norm_shift);

		}



		norm_shift = DVTXOP_norm_l(enrg);

		Ltmp1 = DVTXOP_L_shl(enrg, norm_shift);

		Ltmp1 = DVTXOP_L_mpy_ls(MBDRC_param->smfactor_ener_band[i], (short)(Ltmp1 >> 16));

		Ltmp1 = DVTXOP_L_shr(Ltmp1, norm_shift);

		Ltmp2 = DVTXOP_L_shl(Ltmp1, (short)(7 - (2 * normb_shift)));		// rescaled from 30,1 to 23,8 (w/block denorm)



		norm_shift = DVTXOP_norm_l(MBDRC_buf->ener_band[i]);

		Ltmp1 = DVTXOP_L_shl(MBDRC_buf->ener_band[i], norm_shift);

		Ltmp3 = DVTXOP_L_mpy_ls(DVTXOP_L_sub(2147483647, MBDRC_param->smfactor_ener_band[i]), (short)(Ltmp1 >> 16));

		Ltmp3 = DVTXOP_L_shr(Ltmp3, norm_shift);



		MBDRC_buf->ener_band[i] = DVTXOP_L_add(Ltmp3, Ltmp2);





		// dB scale

		DVTXOP_Log2(MBDRC_buf->ener_band[i], &exp, &fract);

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

		tmp = DVTXOP_add(tmp, (fract >> 6));					//Q9	



		// smoothing

		if (tmp > MBDRC_buf->pre_ener[i])

			alpha = MBDRC_param->smfactor_pre_ener[i][0];

		else

			alpha = MBDRC_param->smfactor_pre_ener[i][1];



		L_tmp = DVTXOP_L_mult(tmp, alpha);

		L_tmp = DVTXOP_L_mac(L_tmp, MBDRC_buf->pre_ener[i], 0x7FFF - alpha);

		tmp = (short)(L_tmp >> 16);



		MBDRC_buf->pre_ener[i] = tmp;



		ch_energy_db[i] = DVTXOP_mult(tmp, C10LOG2);			//Q7		//  10log10(x) = 10log10(2) * log2(x) = 3.0103  * log2(x)

	}



#if(FLAG_SELECT_C_DSP == 1)

#if(Debug_File_Write_C == 1)

	/******** dbg files ********/

	fprintf(fp_dbg_MBDRC, "%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",

		ch_energy_db[0],

		ch_energy_db[1],

		ch_energy_db[2],

		ch_energy_db[3],

		ch_energy_db[4],

		MBDRC_param->limit_thd[0],

		MBDRC_param->limit_thd[1],

		MBDRC_param->limit_thd[2],

		MBDRC_param->limit_thd[3],

		MBDRC_param->limit_thd[4]);

#endif

#endif



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

	{

		tmp = MBDRC_param->limit_thd[i] - MBDRC_param->boostgain[i];



		if (ch_energy_db[i] > tmp)

		{

			Ltmp1 = DVTXOP_L_mult(DVTXOP_sub(tmp, ch_energy_db[i]), MBDRC_param->limit_slp[i]);

			Ltmp1 = DVTXOP_L_mpy_ls(Ltmp1, 0x666);

			Ltmp1 = DVTXOP_L_shl(Ltmp1, 3);	/* rescale Ltmp1 to 5,26 */

			if (Ltmp1 == 0)

				Ltmp1 = -1;

			Ltmp2 = DVTXOP_fnExp10(Ltmp1);

			MBDRC_buf->gain_band[i] = (short)(Ltmp2 >> 16);

		}

		else

		{

			MBDRC_buf->gain_band[i] = 0x7FFF;

		}



		// G*boostgain

		Ltmp1 = DVTXOP_L_mult(MBDRC_buf->gain_band[i], MBDRC_param->boostgain_dec[i]);

		MBDRC_buf->gain_band[i] = DVTXOP_round(Ltmp1);

	}





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

	{

		j1 = MBDRC_param->band_bound[i][0], j2 = MBDRC_param->band_bound[i][1];

		for (j = j1; j <= j2; j++)

			ch_temp_bin[j] = MBDRC_buf->gain_band[i];

	}



#ifndef KHW_OPTI_20191216_HIFI

	for (i = 0; i < MBDRC_FFT_BIN_NUM; i++) ch_gain_bin[i] = ch_temp_bin[i];

#else

	__vec_memcpy(ch_gain_bin,ch_temp_bin,(MBDRC_FFT_BIN_NUM<<1));

#endif



#ifndef  FN_SOLOMONVOICEW_TX_MBDRC_2ch_OPT

	for (i = 2; i < fftbin - 3; i++)

	{

		L_tmp = (long)(ch_temp_bin[i - 1] + ch_temp_bin[i] + ch_temp_bin[i + 1] + ch_temp_bin[i + 2]);

		L_tmp = (L_tmp >> 2);

		ch_gain_bin[i] = (short)(L_tmp);

	}

#else

	{

		int save;

		save = (long)(ch_temp_bin[2 - 1] + ch_temp_bin[2] + ch_temp_bin[2 + 1] );

	    for (i = 2; i < fftbin - 3; i++)

	    {

	    	 save =  save + ch_temp_bin[i + 2];

		     ch_gain_bin[i] = (short)(save >> 2);

		     save = save - ch_temp_bin[i - 1];

	    }

	}

#endif



	ch_gain_bin[0] = 2048;

	ch_gain_bin[fftbin - 1] = 2048;



#ifndef FN_SOLOMONVOICEW_TX_MBDRC_2ch_DSP_OPT1

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

	{

		MBDRCout_Xo1[2 * i] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo1[2 * i], ch_gain_bin[i]), 4) >> 16);

		MBDRCout_Xo1[2 * i + 1] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo1[2 * i + 1], ch_gain_bin[i]), 4) >> 16);



		MBDRCout_Xo2[2 * i] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo2[2 * i], ch_gain_bin[i]), 4) >> 16);

		MBDRCout_Xo2[2 * i + 1] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo2[2 * i + 1], ch_gain_bin[i]), 4) >> 16);

	}

#else

	FN_SOLOMONVOICEW_TX_MBDRC_2ch_OPT1(Xo1,MBDRCout_Xo1, Xo2, MBDRCout_Xo2, ch_gain_bin, fftbin);

#endif

	return;

}