//---------------------------------------------------------------------------
#include "Hrtf_nCycle.h"

#include <stdio.h>
#include <math.h>

void Rms_nCycle_cal(short *InData, short *OutData, short nLEN, informationRMS *rmsInfo)
{
	short	i;

	for(i=0;i<nLEN;i++){
		RmsMainCycle(
			&InData[i*2],
			&OutData[i*2],
			rmsInfo->dLLGain,
			rmsInfo->dLRGain,
			rmsInfo->dRLGain,
			rmsInfo->dRRGain,
			rmsInfo->dLLDelay,
			rmsInfo->dLRDelay,
			rmsInfo->dRLDelay,
			rmsInfo->dRRDelay,

			rmsInfo->dPeak1b10,
			rmsInfo->dPeak1b11,
			rmsInfo->dPeak1b12,
			rmsInfo->dPeak1a11,
			rmsInfo->dPeak1a12,
			rmsInfo->dNotch1b10,
			rmsInfo->dNotch1b11,
			rmsInfo->dNotch1b12,
			rmsInfo->dNotch1a11,
			rmsInfo->dNotch1a12,
			rmsInfo->dNotch2b10,
			rmsInfo->dNotch2b11,
			rmsInfo->dNotch2b12,
			rmsInfo->dNotch2a11,
			rmsInfo->dNotch2a12,

			rmsInfo->dNTFType);
	}
}
//-----------------------------------------------------------------------
//---------------------------------------------------------------------------

int RmsMainCycle(
	short *InData, 
	short *OutData,
	double LLGain,
	double LRGain,
	double RLGain,
	double RRGain,
	double LLDelay,
	double LRDelay,
	double RLDelay,
	double RRDelay,

	double p1_b10,
	double p1_b11,
	double p1_b12,
	double p1_a11,
	double p1_a12,
	double n1_b10,
	double n1_b11,
	double n1_b12,
	double n1_a11,
	double n1_a12,
	double n2_b10,
	double n2_b11,
	double n2_b12,
	double n2_a11,
	double n2_a12,

	int	NTFType
);

	double NTFLch(long,double,double,double,double,double,double,double,double,double,double,double,double,double,double,double);
	double NTFRch(long,double,double,double,double,double,double,double,double,double,double,double,double,double,double,double);

/*-------------------------------------------------------------------
		Remaster Main Calculation
-------------------------------------------------------------------*/
int RmsMainCycle(
	short *InData, 
	short *OutData,
	double LLGain,
	double LRGain,
	double RLGain,
	double RRGain,
	double LLDelay,
	double LRDelay,
	double RLDelay,
	double RRDelay,

	double p1_b10,
	double p1_b11,
	double p1_b12,
	double p1_a11,
	double p1_a12,
	double n1_b10,
	double n1_b11,
	double n1_b12,
	double n1_a11,
	double n1_a12,
	double n2_b10,
	double n2_b11,
	double n2_b12,
	double n2_a11,
	double n2_a12,

	int	NTFType

)
{
	long	lidata, ridata ;
	long	dspl=0, dspr=0 ;

	static long	BufferLch[5120] = {0};
	static long	BufferRch[5120] = {0};
	static int	delaycount = 5119;
	static int	delaycountll = 0;
	static int	delaycountlr = 0;
	static int	delaycountrl = 0;
	static int	delaycountrr = 0;

	lidata = (long)InData[0];
	ridata = (long)InData[1];

	delaycount += 1;
	if(delaycount > 5119)	delaycount = 0;
	delaycountll = delaycount - (int)(LLDelay * 0.0441);
	if(delaycountll < 0)	delaycountll += 5120;
	delaycountlr = delaycount - (int)(LRDelay * 0.0441);
	if(delaycountlr < 0)	delaycountlr += 5120;
	delaycountrl = delaycount - (int)(RLDelay * 0.0441);
	if(delaycountrl < 0)	delaycountrl += 5120;
	delaycountrr = delaycount - (int)(RRDelay * 0.0441);
	if(delaycountrr < 0)	delaycountrr += 5120;

	/*	入力信号　16bit⇒24bit	*/
	lidata = lidata * 256 ;
	ridata = ridata * 256 ;

	BufferLch[delaycount] = lidata;
	BufferRch[delaycount] = ridata;

	/*	Ｌｃｈ	*/
	dspl = (long)(BufferLch[delaycountll] * LLGain) + (long)((BufferRch[delaycountrl]) * RLGain);	//20100520 add
	if(NTFType == 1)	dspl = (long)(NTFLch(dspl,p1_b10,p1_b11,p1_b12,p1_a11,p1_a12,
						n1_b10,n1_b11,n1_b12,n1_a11,n1_a12,
						n2_b10,n2_b11,n2_b12,n2_a11,n2_a12));	//20100531 chang
	dspl = dspl / 256;

	/*	Ｒｃｈ	*/
	dspr = (long)(BufferRch[delaycountrr] * RRGain) + (long)((BufferLch[delaycountlr]) * LRGain);	//20100520 add
	if(NTFType == 1)	dspr = (long)(NTFRch(dspr,p1_b10,p1_b11,p1_b12,p1_a11,p1_a12,
						n1_b10,n1_b11,n1_b12,n1_a11,n1_a12,
						n2_b10,n2_b11,n2_b12,n2_a11,n2_a12));	//20100531 chang
	dspr = dspr / 256;

	/* 16bit ROUND */
    if (dspl>32767L) dspl = 32767L;
    else if (dspl<-32768L) dspl = -32768L;
    if (dspr>32767L) dspr = 32767L;
    else if (dspr<-32768L) dspr = -32768L;

	/* Store into Output Buffer */
	OutData[0] = (short)dspl;
	OutData[1] = (short)dspr;

	return 0 ;
}

/*-------------------------------------------------------------------
		Notch Filter　Lch
-------------------------------------------------------------------*/
double NTFLch(long indata,double p1_b10,double p1_b11,double p1_b12,double p1_a11,double p1_a12,
		double n1_b10,double n1_b11,double n1_b12,double n1_a11,double n1_a12,
		double n2_b10,double n2_b11,double n2_b12,double n2_a11,double n2_a12)
{
	double			x;//, xah, b, bm, bp ;

	static double	xd1 = 0 ;
	static double	xd2 = 0 ;
	static double	u1 = 0 ;
	static double	u1d1 = 0 ;
	static double	u1d2 = 0 ;
	static double	u2 = 0 ;
	static double	u2d1 = 0 ;
	static double	u2d2 = 0 ;

	static double	uu1 = 0 ;
	static double	uu1d1 = 0 ;
	static double	uu1d2 = 0 ;
	static double	u3 = 0;
	static double	u3d1 = 0 ;
	static double	u3d2 = 0 ;

	/*	Lch信号	*/
	x = indata ;

	/*	2th order HPF1	*/
	u1 = p1_b10*x + p1_b11*xd1 + p1_b12*xd2  + p1_a11*u1d1 + p1_a12*u1d2 ;
	uu1 = 2.0 * u1 + x;
	u2 = n1_b10*uu1+ n1_b11*uu1d1+ n1_b12*uu1d2 + n1_a11*u2d1 + n1_a12*u2d2 ;
	u3 = n2_b10*u2+ n2_b11*u2d1+ n2_b12*u2d2 + n2_a11*u3d1 + n2_a12*u3d2 ;

	xd2 = xd1 ;
	xd1 = x ;
	u1d2 = u1d1 ;
	u1d1 = u1 ;
	uu1d2 = uu1d1;
	uu1d1 = uu1;
	u2d2 = u2d1 ;
	u2d1 = u2 ;
	u3d2 = u3d1 ;
	u3d1 = u3 ;

	return	u3 ;
}

/*-------------------------------------------------------------------
		Notch Filter　Rch
-------------------------------------------------------------------*/
double NTFRch(long indata,double p1_b10,double p1_b11,double p1_b12,double p1_a11,double p1_a12,
		double n1_b10,double n1_b11,double n1_b12,double n1_a11,double n1_a12,
		double n2_b10,double n2_b11,double n2_b12,double n2_a11,double n2_a12)
{
	double			x ;//, xah, b, bm, bp ;

	static double	xd1 = 0 ;
	static double	xd2 = 0 ;
	static double	u1 = 0 ;
	static double	u1d1 = 0 ;
	static double	u1d2 = 0 ;
	static double	u2 = 0 ;
	static double	u2d1 = 0 ;
	static double	u2d2 = 0 ;

	static double	uu1 = 0 ;
	static double	uu1d1 = 0 ;
	static double	uu1d2 = 0 ;
	static double	u3 = 0;
	static double	u3d1 = 0 ;
	static double	u3d2 = 0 ;

	/*	Rch信号	*/
	x = indata ;

	/*	2th order HPF1	*/
	u1 = p1_b10*x + p1_b11*xd1 + p1_b12*xd2  + p1_a11*u1d1 + p1_a12*u1d2 ;
	uu1 = 2.0 * u1 + x;
	u2 = n1_b10*uu1+ n1_b11*uu1d1+ n1_b12*uu1d2 + n1_a11*u2d1 + n1_a12*u2d2 ;
	u3 = n2_b10*u2+ n2_b11*u2d1+ n2_b12*u2d2 + n2_a11*u3d1 + n2_a12*u3d2 ;

	xd2 = xd1 ;
	xd1 = x ;
	u1d2 = u1d1 ;
	u1d1 = u1 ;
	uu1d2 = uu1d1;
	uu1d1 = uu1;
	u2d2 = u2d1 ;
	u2d1 = u2 ;
	u3d2 = u3d1 ;
	u3d1 = u3 ;

	return	u3 ;
}


//---------------------------------------------------------------------------
