#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_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;



	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);



	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;

}



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;

}