/* Noise Cancellation for In-ear mic */

#include "SV_InEarNC.h"

#include "SV_basic_op.h"





//#define SV_INEAR_OVERFLOW_CHECK

//#define BSH_DEBUG

#if defined (_WIN32) && defined(BSH_DEBUG)

#define DEBUG_INEAR

extern frmCnt;

#endif

#define TARGETBAND_LOWERBOUND 1

#define TARGETBAND_UPPERBOUND 200 // not included

#define TARGETBAND_SIZE	    (TARGETBAND_UPPERBOUND - TARGETBAND_LOWERBOUND)



typedef long long INT64;



#define SMULL( a,   b )\

    ((INT64)(a)*(INT64)(b))

#define ABS(a) (((a)>0)?(a):(-a))

#define COEF_Q 20

//#define UPDATE_BOUND (INT64)((INT64)1<<(COEF_Q*2)) // 1 << (COEF_Q*2)

#define UPDATE_BOUND (INT64)109951162778 // 0.1



typedef struct {

	short mu; //Q15

	short muSlow; //Q15

	short muFast; //Q15

	short powerClipping;



	int coef[TARGETBAND_SIZE * 2]; // COEF_Q

	int *inEarOutSpec;

	int ADFerror[TARGETBAND_SIZE * 2];

	int ADFin[TARGETBAND_SIZE * 2];



	int noiseUpdateFrameCnt;

	int RxVAD;

	int TxVAD;

	int UpdateFreezing;

} SV_InEarNC_Vars;

static SV_InEarNC_Vars inEarVars ;







#ifdef DEBUG_INEAR

#include<stdlib.h>

#include<stdio.h>

extern FILE *f_inEar[10];

#endif

void SV_InEarNC_Init()

{

	int i;



	inEarVars.mu = (short)3276; // 0.1 (Q15)

	inEarVars.muSlow = (short)3276 >> 3; //(Q15)

	inEarVars.muFast = (short)9830; //(Q15)



	memset(inEarVars.coef, 0, sizeof(int) * TARGETBAND_SIZE * 2);





	memset(inEarVars.ADFerror, 0, sizeof(int) * TARGETBAND_SIZE * 2);

	memset(inEarVars.ADFin, 0, sizeof(int) * TARGETBAND_SIZE * 2);



	inEarVars.powerClipping = 0; // cause speech distortion

	inEarVars.noiseUpdateFrameCnt = 0;



	inEarVars.RxVAD = 0;

	inEarVars.TxVAD = 0;

	inEarVars.UpdateFreezing = 0;



}



void SV_InEarNC_SetPar(int TxVAD, int RxVAD, int UpdateFreezing)

{

	inEarVars.TxVAD = TxVAD;

	inEarVars.RxVAD = RxVAD;

	inEarVars.UpdateFreezing = UpdateFreezing;

}



void SV_InEarNC_Proc(int *OutSpec, int *inEarSpec, int *refSpec)

{

	int i;



	int *inEarSpecPtr = inEarSpec + TARGETBAND_LOWERBOUND * 2;

	int *OutSpecPtr = OutSpec + TARGETBAND_LOWERBOUND * 2;

	int *refSpecPtr = refSpec + TARGETBAND_LOWERBOUND * 2;

	int *coefPtr = inEarVars.coef;



#ifdef DEBUG_INEAR

	float ftmp;

	inEarSpecPtr = inEarSpec + TARGETBAND_LOWERBOUND * 2;

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

	{

		int real = *inEarSpecPtr++;

		int imag = *inEarSpecPtr++;

		ftmp = ((float)real*real + (float)imag*imag) / (float)(1 << 30);

		fwrite(&ftmp, sizeof(float), 1, f_inEar[4]);

	}

#endif

#if 1

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

	{

		OutSpec[2 * i] = 0;

		OutSpec[2 * i + 1] = 0;

	}

	for (i = TARGETBAND_UPPERBOUND; i < 256; i++)

	{

		OutSpec[2 * i] = 0;

		OutSpec[2 * i + 1] = 0;

	}

#endif



	// noise cancellation

	inEarSpecPtr = inEarSpec + TARGETBAND_LOWERBOUND * 2;

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

	{

		int refSpecReal = *refSpecPtr++;

		int refSpecImag = *refSpecPtr++;

		int coefReal = *coefPtr++;

		int coefImag = *coefPtr++;



		*OutSpecPtr++ = *inEarSpecPtr++ - (int)((SMULL(refSpecReal, coefReal) - SMULL(refSpecImag, coefImag)) >> COEF_Q);

		*OutSpecPtr++ = *inEarSpecPtr++ - (int)((SMULL(refSpecImag, coefReal) + SMULL(refSpecReal, coefImag)) >> COEF_Q);

	}



	inEarVars.inEarOutSpec = OutSpec;

	// buffering for filter udpate

	memcpy(inEarVars.ADFin, refSpec + TARGETBAND_LOWERBOUND * 2, sizeof(int) * TARGETBAND_SIZE * 2);

	memcpy(inEarVars.ADFerror, OutSpec + TARGETBAND_LOWERBOUND * 2, sizeof(int) * TARGETBAND_SIZE * 2);



#ifdef DEBUG_INEAR

	inEarSpecPtr = OutSpec + TARGETBAND_LOWERBOUND * 2;

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

	{

		int real = *inEarSpecPtr++;

		int imag = *inEarSpecPtr++;

		ftmp = ((float)real*real + (float)imag*imag) / (float)(1 << 30);

		fwrite(&ftmp, sizeof(float), 1, f_inEar[5]);

	}

#endif

}



void SV_InEarNC_Update(int TxVAD, int RxVAD, int UpdateFreezing)

{

	int i;



	int *scratchMEM1 = inEarVars.ADFin; // available after calculating gradient & power

	int *scratchMEM2 = inEarVars.ADFerror;



	if (TxVAD == 0 && \

		RxVAD == 0 && \

		UpdateFreezing == 0)

	{

		int *inPtr = inEarVars.ADFin;

		int *erPtr = inEarVars.ADFerror;

		int *gradPtr = scratchMEM1;

		int *inPowerPtr = scratchMEM2;

		int *coefPtr;

		const int normPower = 11;

		const int PowerThd = \

			//524; //-30dB Q(30-normPower) update gating by input power

			2654; // -30 + 7.0437 (input gain)

			//4719; // -30 + 9.5424 (input gain)

			//8389; // -30 + 12.0412 (input gain)



		short mu;



		// calculating gradient & power

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

		{

			int inReal = *inPtr++; int inImag = *inPtr++;

			int erReal = *erPtr++; int erImag = *erPtr++;



			*gradPtr++ = (int)((SMULL(inReal, erReal) + SMULL(inImag, erImag)) >> normPower); //Q30 - normPower -> Q(N)

			*gradPtr++ = (int)((SMULL(-inImag, erReal) + SMULL(inReal, erImag)) >> normPower);

			*inPowerPtr++ = (int)((SMULL(inReal, inReal) + SMULL(inImag, inImag)) >> normPower);



#ifdef SV_INEAR_OVERFLOW_CHECK

			{

				if (inPowerPtr[-1] < 0)

				{

					printf("Overflow!! [SV_InEarNC_Update->inPowerPtr] %ld %ld %ld\n", inReal, inImag, inPowerPtr[-1]);

				}

				INT64 LLtmp = (SMULL(erReal, erReal) + SMULL(erImag, erImag) >> normPower);

				if (LLtmp > 2147483647 /* 2^31 - 1*/)

				{

					printf("Overflow!! [SV_InEarNC_Update->error] %ld %ld %ld\n", erReal, erImag, (int)LLtmp);

				}

			}

#endif

		}



		gradPtr = scratchMEM1;

		inPowerPtr = scratchMEM2;

		coefPtr = inEarVars.coef;



		if (inEarVars.noiseUpdateFrameCnt < 10)

		{

			mu = inEarVars.muFast;

		}

		else if (inEarVars.noiseUpdateFrameCnt < 200)

		{

			mu = inEarVars.mu;

		}

		else

		{

			mu = inEarVars.muSlow;

			//mu = inEarVars.mu;

		}

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

		{

			int inPower = *inPowerPtr++;

			int gradReal = *gradPtr++;

			int gradImag = *gradPtr++;



			int updateReal, updateImag;

			short inPowerNormSize;

			short SnormedInPower;

			short normedStepSize;

			short shiftSize;



			// update gating by input power

			if (inPower < PowerThd)

			{

				coefPtr += 2;

#ifdef DEBUG_INEAR

				short stmp = 0;

				fwrite(&stmp, sizeof(short), 1, f_inEar[9]);

#endif

				continue;

			}



			// caculating mu * gradient / input_power

			inPowerNormSize = SV_L_norm(inPower);

			SnormedInPower = SV_extract_h(inPower << inPowerNormSize); // Q(N-16+inPowerNorm)

			normedStepSize = SV_div_s(mu, SnormedInPower); // Q15 / Q(N-16 + inPowerNorm) = Q(31-N - inPowerNorm + 15) = Q(46 - N - inPowerNorm)



			shiftSize = 46 - inPowerNormSize - COEF_Q; //  (for Q(N) * Q(46 - N - inPowerNorm))

			updateReal = (int)(SMULL(gradReal, normedStepSize) >> shiftSize); //COEF_Q

			updateImag = (int)(SMULL(gradImag, normedStepSize) >> shiftSize);



			// update gating by update amount

			//20*log10(abs(Update))<0;

			if (SMULL(updateReal, updateReal) + SMULL(updateImag, updateImag) > UPDATE_BOUND)

			{

				coefPtr += 2;

#ifdef DEBUG_INEAR

				short stmp = 0;

				fwrite(&stmp, sizeof(short), 1, f_inEar[9]);

#endif

				continue;

			}



			// fiter update

			*coefPtr++ += updateReal;

			*coefPtr++ += updateImag;



#ifdef DEBUG_INEAR

			short stmp = 1;

			fwrite(&stmp, sizeof(short), 1, f_inEar[9]);

#endif



		}



		if (inEarVars.noiseUpdateFrameCnt < 500) inEarVars.noiseUpdateFrameCnt++;

	}

#ifdef DEBUG_INEAR

	else

	{

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

		{

			short stmp = 0;

			fwrite(&stmp, sizeof(short), 1, f_inEar[9]);

		}

	}

#endif

}



void SV_InEarNC_Deinit()

{



}



int *SV_InEarNC_GetWeight() { return inEarVars.coef; }

int SV_InEarNC_GetWeightQ() { return COEF_Q; }

int SV_InEarNC_GetLowerBound() { return TARGETBAND_LOWERBOUND; }

int* SV_InEarNC_GetInEarOut() { return inEarVars.inEarOutSpec; }

void SV_InEarNC_ResetNoiseFrameCnt() { inEarVars.noiseUpdateFrameCnt = 0; }



extern void SV_InEarNC_Exe(ComplexInt* OutSpec, ComplexInt* InEarMicSpec, ComplexInt* OuterMicSpec)

{

	SV_InEarNC_Proc((int*)OutSpec, (int*)InEarMicSpec, (int*)OuterMicSpec);

	SV_InEarNC_Update(inEarVars.TxVAD, inEarVars.RxVAD, inEarVars.UpdateFreezing);



	return;

}