#include "SamsungSolomonVoiceW_EQ.h"



void SolomonVoiceWFIRFilterInit(FIRStatus* FIR_buf, SamsungSolomonVoiceWTxFIRParam* FIR_param)

{

	short i;



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

	for(i=0;i< SF_FRAME_LENGTH_SWB + SF_FIRFILTERNUM - 1;i++)

		FIR_buf->FIRCoefBuff[i] = 0;



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

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

	{

		FIR_param->FIRCoef_out_WB[i] = 0;

		//FIR_param->FIRCoefCur_out_WB[i] = 0;



		FIR_param->FIRCoef_out_NB[i] = 0;

		//FIR_param->FIRCoefCur_out_NB[i] = 0;



		FIR_param->FIRCoef_out_RX_WB[i] = 0;

		FIR_param->FIRCoef_out_RX_NB[i] = 0;

	}

	FIR_param->FIRCoef_out_WB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;

	//FIR_param->FIRCoefCur_out_WB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;

	FIR_param->FIRCoef_out_NB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;

	//FIR_param->FIRCoefCur_out_NB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;



	FIR_param->FIRCoef_out_RX_WB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;

	FIR_param->FIRCoef_out_RX_NB[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;



	/*for (i = 0; i < FIRFILTERTEMP; i++)

	{

		FIR_param->FIRCoefDEST_out_WB[i] = 0;

		FIR_param->FIRCoefDEST_out_NB[i] = 0;

	}*/



	return;

}



void SolomonVoiceW_FIR_ParamConfig(SamsungSolomonVoiceWTxFIRParam* FIR_param, SamsungSolomonVoiceWTxFIRParam* param)

{

	FN_SOLOMONVOICEW_TX_FirCoefConfig(FIR_param->FIRCoef_out_WB, _FIRCoef_OUTPUT_WB);

	FN_SOLOMONVOICEW_TX_FirCoefConfig(FIR_param->FIRCoef_out_NB, _FIRCoef_OUTPUT_NB);



	FN_SOLOMONVOICEW_TX_FirCoefConfig(FIR_param->FIRCoef_out_RX_WB, _FIRCoef_OUTPUT_RX_WB);

	FN_SOLOMONVOICEW_TX_FirCoefConfig(FIR_param->FIRCoef_out_RX_NB, _FIRCoef_OUTPUT_RX_NB);



	return;

}



void FN_SOLOMONVOICEW_TX_FirCoefConfig(short *SF_FIRCoef, short *rxfircoef)

{

	short i, j, count = 0;



	for (i = 0, j = SF_FIRFILTERNUM - 1; i < SF_FIRFILTERNUM / 2; i++, j--)

	{

		SF_FIRCoef[i] = rxfircoef[i];

		SF_FIRCoef[j] = rxfircoef[i];

	}

	SF_FIRCoef[i] = rxfircoef[i];



	for (i = 0; i < SF_FIRFILTERNUM; i++) {

		if (SF_FIRCoef[i] != 0)

			count++;

	}



	if (count == 0)

		SF_FIRCoef[SF_FIRFILTER_CENTER] = SF_FIRFILTER_BYPASS;



	return;

}



int FN_SOLOMONVOICEW_TX_FilterUpdate(short* cur_coeff, short* dst_coeff, short flag)

{

	short i;

	short S_tmp, adp_time;

	long L_tmp;



	if (flag == 1)

		adp_time = SF_INC_ADTIME;

	else

		adp_time = SF_DEC_ADTMIE;



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

	{

		L_tmp = DVTXOP_L_mult(adp_time, cur_coeff[i]);

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

		L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, dst_coeff[i]);

		cur_coeff[i] = DVTXOP_extract_h(L_tmp);

	}



	return 0;

}

#ifndef FN_SOLOMONVOICEW_TX_FIRFilter_OPT_DSP

void FN_SOLOMONVOICEW_TX_FIRFilter(short* Input, short* FIRCoeff, short* FIRFiltBuff, short size)

{

	short i, j, k;

	long left_out;

#ifndef KHW_OPTI_20191216_HIFI

	for (i = SF_EQ_TAP - 1, j = 0; i < (size + SF_EQ_TAP - 1); i++, j++)

		FIRFiltBuff[i] = Input[j];

#else

	__vec_memcpy(&FIRFiltBuff[SF_EQ_TAP-1],Input,(size<<1));

#endif

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

		left_out = 0;

		for (j = 0, k = i; j < SF_EQ_TAP; j++)

			left_out = DVTXOP_L_mac(left_out, FIRFiltBuff[k++], FIRCoeff[j]);



		Input[i] = DVTXOP_round(DVTXOP_L_shl(left_out, 1));

	}

#ifndef KHW_OPTI_20191216_HIFI

	for (i = 0; i < SF_EQ_TAP - 1; i++)

		FIRFiltBuff[i] = FIRFiltBuff[size + i];

#else

	__vec_memcpy(FIRFiltBuff,&FIRFiltBuff[size],((SF_EQ_TAP - 1)<<1));

#endif

	return;

}

#else

void FN_SOLOMONVOICEW_TX_FIRFilter(short* Input, short* FIRCoeff, short* FIRFiltBuff, short size)

{

	short i, j, k;

#ifndef KHW_OPTI_20191216_HIFI

	for (i = SF_EQ_TAP - 1, j = 0; i < (size + SF_EQ_TAP - 1); i++, j++)

		FIRFiltBuff[i] = Input[j];

#else

	__vec_memcpy(&FIRFiltBuff[SF_EQ_TAP-1],Input,(size<<1));

#endif

	ae_int32x2 L_add11, L_add12, L_add21, L_add22;

	ae_int32x2 L_add31, L_add32, L_add41, L_add42;

	ae_int16x4 data1, data2, data3, data4, tmpdata;

	ae_int16x4 coffdata;

	ae_valign align_buf;

	ae_valign align_filter;

	ae_valign align_out;

	ae_int16x4 *pbuf;

	ae_int16x4 *pfilter;

	ae_int16x4 *pout;

	ae_int16x4 mask = AE_MOVF16_FROMF64(AE_MOV64(0xffffffffffff0000));

	ae_f32x2 out1,out2;

	pout = (ae_int16x4 *)&Input[0];

	align_out = AE_ZALIGN64();

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

	{

		pbuf = (ae_int16x4 *)&FIRFiltBuff[i];

		align_buf = AE_LA64_PP(pbuf);



		pfilter = (ae_int16x4 *)&FIRCoeff[0];

		align_filter = AE_LA64_PP(pfilter);



		AE_LA16X4_IP(tmpdata, align_buf, pbuf);

		L_add11 = L_add12 = L_add21 = L_add22 = 0;

		L_add31 = L_add32 = L_add41 = L_add42 = 0;

		for(j=0;j<15;j++)

		{

			data1=tmpdata;

			AE_LA16X4_IP(tmpdata, align_buf, pbuf);

			AE_LA16X4_IP(coffdata, align_filter, pfilter);



			data2 = AE_SEL16_6543(data1, tmpdata);

			data3 = AE_SEL16_5432(data1, tmpdata);

			data4 = AE_SEL16_4321(data1, tmpdata);



			AE_MULAF16X4SS(L_add11, L_add12, data1, coffdata);

			AE_MULAF16X4SS(L_add21, L_add22, data2, coffdata);

			AE_MULAF16X4SS(L_add31, L_add32, data3, coffdata);

			AE_MULAF16X4SS(L_add41, L_add42, data4, coffdata);

		}

		data1 = tmpdata;

		AE_LA16X4_IP(tmpdata, align_buf, pbuf);

		AE_LA16X4_IP(coffdata, align_filter, pfilter);

		data2 = AE_SEL16_6543(data1, tmpdata);

		data3 = AE_SEL16_5432(data1, tmpdata);

		data4 = AE_SEL16_4321(data1, tmpdata);



		coffdata = AE_AND16(coffdata, mask);

		AE_MULAF16X4SS(L_add11, L_add12, data1, coffdata);

		AE_MULAF16X4SS(L_add21, L_add22, data2, coffdata);

		AE_MULAF16X4SS(L_add31, L_add32, data3, coffdata);

		AE_MULAF16X4SS(L_add41, L_add42, data4, coffdata);



		L_add11 = AE_ADD32S(L_add11, L_add12);

		L_add12 = AE_SEL32_LH(L_add12, L_add11);

		L_add11 = AE_ADD32S(L_add11, L_add12);



		L_add21 = AE_ADD32S(L_add21, L_add22);

		L_add22 = AE_SEL32_LH(L_add22, L_add21);

		L_add21 = AE_ADD32S(L_add21, L_add22);

		out1 = AE_SEL32_LL(L_add11, L_add21);

		out1 = AE_SLAA32S(out1, 1);



    	L_add31 = AE_ADD32S(L_add31, L_add32);

		L_add32 = AE_SEL32_LH(L_add32, L_add31);

		L_add31 = AE_ADD32S(L_add31, L_add32);



		L_add41 = AE_ADD32S(L_add41, L_add42);

		L_add42 = AE_SEL32_LH(L_add42, L_add41);

		L_add41 = AE_ADD32S(L_add41, L_add42);



		out2 = AE_SEL32_LL(L_add31, L_add41);

		out2 = AE_SLAA32S(out2, 1);

		tmpdata = AE_ROUND16X4F32SASYM(out1, out2);

		AE_SA16X4_IP(tmpdata, align_out, pout);

	}

	AE_SA64POS_FP(align_out, pout);



#ifndef KHW_OPTI_20191216_HIFI

	for (i = 0; i < SF_EQ_TAP - 1; i++)

		FIRFiltBuff[i] = FIRFiltBuff[size + i];

#else

	__vec_memcpy(FIRFiltBuff,&FIRFiltBuff[size],((SF_EQ_TAP - 1)<<1));

#endif

	return;

}

#endif