/*
 * EQ.cpp
 * ------
 * Purpose: Mixing code for equalizer.
 * Notes  : Ugh... This should really be removed at some point.
 * Authors: Olivier Lapicque
 *          OpenMPT Devs
 * The OpenMPT source code is released under the BSD license. Read LICENSE for more details.
 */


#include "stdafx.h"
#include "../soundlib/MixerLoops.h"
#include "../sounddsp/EQ.h"

#include <cstddef>


OPENMPT_NAMESPACE_BEGIN


#ifndef NO_EQ


#define EQ_BANDWIDTH	2.0
#define EQ_ZERO			0.000001



static constexpr UINT gEqLinearToDB[33] =
{
	16, 19, 22, 25, 28, 31, 34, 37,
	40, 43, 46, 49, 52, 55, 58, 61,
	64, 76, 88, 100, 112, 124, 136, 148,
	160, 172, 184, 196, 208, 220, 232, 244, 256
};

static constexpr EQBANDSTRUCT gEQDefaults[MAX_EQ_BANDS*2] =
{
	// Default: Flat EQ
	{0,0,0,0,0, 0,0,0,0, 1,   120, false},
	{0,0,0,0,0, 0,0,0,0, 1,   600, false},
	{0,0,0,0,0, 0,0,0,0, 1,  1200, false},
	{0,0,0,0,0, 0,0,0,0, 1,  3000, false},
	{0,0,0,0,0, 0,0,0,0, 1,  6000, false},
	{0,0,0,0,0, 0,0,0,0, 1, 10000, false},
	{0,0,0,0,0, 0,0,0,0, 1,   120, false},
	{0,0,0,0,0, 0,0,0,0, 1,   600, false},
	{0,0,0,0,0, 0,0,0,0, 1,  1200, false},
	{0,0,0,0,0, 0,0,0,0, 1,  3000, false},
	{0,0,0,0,0, 0,0,0,0, 1,  6000, false},
	{0,0,0,0,0, 0,0,0,0, 1, 10000, false},
};

#ifdef ENABLE_X86

#if MPT_COMPILER_MSVC
#pragma warning(push)
#pragma warning(disable:4100)
#endif // MPT_COMPILER_MSVC

#define PBS_A0	DWORD PTR [eax + EQBANDSTRUCT.a0]
#define PBS_A1	DWORD PTR [eax + EQBANDSTRUCT.a1]
#define PBS_A2	DWORD PTR [eax + EQBANDSTRUCT.a2]
#define PBS_B1	DWORD PTR [eax + EQBANDSTRUCT.b1]
#define PBS_B2	DWORD PTR [eax + EQBANDSTRUCT.b2]
#define PBS_X1	DWORD PTR [eax + EQBANDSTRUCT.x1]
#define PBS_X2	DWORD PTR [eax + EQBANDSTRUCT.x2]
#define PBS_Y1	DWORD PTR [eax + EQBANDSTRUCT.y1]
#define PBS_Y2	DWORD PTR [eax + EQBANDSTRUCT.y2]

static void X86_EQFilter(EQBANDSTRUCT *pbs, float32 *pbuffer, UINT nCount)
{
	_asm {
	mov eax, pbs		// eax = pbs
	mov edx, nCount		// edx = nCount
	mov ecx, pbuffer	// ecx = pbuffer
	fld		PBS_Y2		// ST(3)=y2
	fld		PBS_Y1		// ST(2)=y1
	fld		PBS_X2		// ST(1)=x2
	fld		PBS_X1		// ST(0)=x1
EQ_Loop:
	fld		DWORD PTR [ecx]		// ST(0):x ST(1):x1 ST(2):x2 ST(3):y1 ST(4):y2
	fld		PBS_A0				// ST(0):a0 ST(1):x ST(2):x1 ST(3):x2 ST(4):y1 ST(5):y2
	fmul	ST(0), ST(1)		// ST(0):a0*x
	fld		PBS_A1				// ST(0):a1 ST(1):a0*x ST(2):x ST(3):x1 ST(4):x2 ST(5):y1 ST(6):y2
	fmul	ST(0), ST(3)		// ST(0):a1*x1
	add		ecx, 4

	faddp	ST(1), ST(0)
	fld		PBS_A2
	fmul	ST(0), ST(4)
	faddp	ST(1), ST(0)
	fld		PBS_B1
	fmul	ST(0), ST(5)
	faddp	ST(1), ST(0)
	fld		PBS_B2
	fmul	ST(0), ST(6)
	sub     edx, 1
	faddp	ST(1), ST(0)
	fst		DWORD PTR [ecx-4]		// *pbuffer = a0*x+a1*x1+a2*x2+b1*y1+b2*y2
	// Here, ST(0)=y ST(1)=x ST(2)=x1 ST(3)=x2 ST(4)=y1 ST(5)=y2
	fxch	ST(4)	// y1=y
	fstp	ST(5)	// y2=y1
	// Here, ST(0)=x ST(1)=x1 ST(2)=x2 ST(3)=y1 ST(4)=y2
	fxch	ST(1)	// x1=x
	fstp	ST(2)	// x2=x1
	jnz		EQ_Loop
	// Store x1,y1,x2,y2 and pop FPU stack
	fstp	PBS_X1
	fstp	PBS_X2
	fstp	PBS_Y1
	fstp	PBS_Y2
	}
}


#if defined(ENABLE_X86) && defined(ENABLE_SSE)

static void SSE_StereoEQ(EQBANDSTRUCT *pbl, EQBANDSTRUCT *pbr, float32 *pbuffer, UINT nCount)
{
	static constexpr float gk1 = 1.0f;
	_asm {
	mov eax, pbl
	mov edx, pbr
	mov ebx, pbuffer
	mov ecx, nCount
	movss xmm0, [eax+EQBANDSTRUCT.Gain]
	movss xmm1, gk1
	comiss xmm0, xmm1
	jne doeq
	movss xmm0, [edx+EQBANDSTRUCT.Gain]
	comiss xmm0, xmm1
	je done
doeq:
	test ecx, ecx
	jz done
	movss xmm6, [eax+EQBANDSTRUCT.a1]
	movss xmm7, [eax+EQBANDSTRUCT.a2]
	movss xmm4, [eax+EQBANDSTRUCT.x1]
	movss xmm5, [eax+EQBANDSTRUCT.x2]
	movlhps xmm6, xmm7		// xmm6 = [  0 | a2 |  0 | a1 ]
	movlhps xmm4, xmm5		// xmm4 = [  0 | x2 |  0 | x1 ]
	movss xmm2, [edx+EQBANDSTRUCT.a1]
	movss xmm3, [edx+EQBANDSTRUCT.a2]
	movss xmm0, [edx+EQBANDSTRUCT.x1]
	movss xmm1, [edx+EQBANDSTRUCT.x2]
	movlhps xmm2, xmm3		// xmm2 = [ 0 | a'2 | 0 | a'1 ]
	movlhps xmm0, xmm1		// xmm0 = [ 0 | x'2 | 0 | x'1 ]
	shufps xmm6, xmm2, 0x88	// xmm6 = [ a'2 | a'1 | a2 | a1 ]
	shufps xmm4, xmm0, 0x88 // xmm4 = [ x'2 | x'1 | x2 | x1 ]
	shufps xmm6, xmm6, 0xD8	// xmm6 = [ a'2 | a2 | a'1 | a1 ]
	shufps xmm4, xmm4, 0xD8	// xmm4 = [ x'2 | x2 | x'1 | x1 ]
	movss xmm7, [eax+EQBANDSTRUCT.b1]
	movss xmm0, [eax+EQBANDSTRUCT.b2]
	movss xmm2, [edx+EQBANDSTRUCT.b1]
	movss xmm1, [edx+EQBANDSTRUCT.b2]
	movlhps xmm7, xmm0		// xmm7 = [ 0 |  b2 | 0 |  b1 ]
	movlhps xmm2, xmm1		// xmm2 = [  0 | b'2 |  0 | b'1 ]
	shufps xmm7, xmm2, 0x88	// xmm7 = [ b'2 | b'1 | b2 | b1 ]
	shufps xmm7, xmm7, 0xD8 // xmm7 = [ b'2 | b2 | b'1 | b1 ]
	movss xmm5, [eax+EQBANDSTRUCT.y1]
	movss xmm1, [eax+EQBANDSTRUCT.y2]
	movss xmm3, [edx+EQBANDSTRUCT.y1]
	movss xmm0, [edx+EQBANDSTRUCT.y2]
	movlhps xmm5, xmm1		// xmm5 = [  0 |  y2 |  0 |  y1 ]
	movlhps xmm3, xmm0		// xmm3 = [  0 | y'2 |  0 | y'1 ]
	shufps xmm5, xmm3, 0x88 // xmm5 = [ y'2 | y'1 | y2 | y1 ]
	shufps xmm5, xmm5, 0xD8 // xmm5 = [ y'2 |  y2 | y'1 |  y1 ]
	movss xmm3, [eax+EQBANDSTRUCT.a0]
	movss xmm2, [edx+EQBANDSTRUCT.a0]
	shufps xmm3, xmm2, 0x88
	shufps xmm3, xmm3, 0xD8	// xmm3 = [  0 |  0 | a'0 | a0 ]
mainloop:
	movlps xmm0, qword ptr [ebx]
	add ebx, 8
	movaps xmm1, xmm5		// xmm1 = [ y2r | y2l | y1r | y1l ]
	mulps xmm1, xmm7		// xmm1 = [b2r*y2r|b2l*y2l|b1r*y1r|b1l*y1l]
	movaps xmm2, xmm4		// xmm2 = [ x2r | x2l | x1r | x1l ]
	mulps xmm2, xmm6		// xmm6 = [a2r*x2r|a2l*x2l|a1r*x1r|a1l*x1l]
	shufps xmm4, xmm0, 0x44 // xmm4 = [  xr |  xl | x1r | x1l ]
	mulps xmm0, xmm3		// xmm0 = [   0 |   0 |a0r*xr|a0l*xl]
	shufps xmm4, xmm4, 0x4E // xmm4 = [ x1r | x1l |  xr |  xl ]
	addps xmm1, xmm2		// xmm1 = [b2r*y2r+a2r*x2r|b2l*y2l+a2l*x2l|b1r*y1r+a1r*x1r|b1l*y1l+a1l*x1l]
	addps xmm1, xmm0		// xmm1 = [b2r*y2r+a2r*x2r|b2l*y2l+a2l*x2l|b1r*y1r+a1r*x1r+a0r*xr|b1l*y1l+a1l*x1l+a0l*xl]
	sub ecx, 1
	movhlps xmm0, xmm1
	addps xmm0, xmm1		// xmm0 = [  ? |  ? | yr(n) | yl(n) ]
	movlps [ebx-8], xmm0
	shufps xmm0, xmm5, 0x44
	movaps xmm5, xmm0
	jnz mainloop
	movhlps xmm0, xmm4
	movhlps xmm1, xmm5
	movss [eax+EQBANDSTRUCT.x1], xmm4
	movss [eax+EQBANDSTRUCT.x2], xmm0
	movss [eax+EQBANDSTRUCT.y1], xmm5
	movss [eax+EQBANDSTRUCT.y2], xmm1
	shufps xmm4, xmm4, 0x01
	shufps xmm0, xmm0, 0x01
	shufps xmm5, xmm5, 0x01
	shufps xmm1, xmm1, 0x01
	movss [edx+EQBANDSTRUCT.x1], xmm4
	movss [edx+EQBANDSTRUCT.x2], xmm0
	movss [edx+EQBANDSTRUCT.y1], xmm5
	movss [edx+EQBANDSTRUCT.y2], xmm1
done:;
	}
}

#endif // ENABLE_X86 && ENABLE_SSE

#if MPT_COMPILER_MSVC
#pragma warning(pop)
#endif // MPT_COMPILER_MSVC

#endif

static void EQFilter(EQBANDSTRUCT *pbs, float32 *pbuffer, UINT nCount)
{
	for (UINT i=0; i<nCount; i++)
	{
		float32 x = pbuffer[i];
		float32 y = pbs->a1 * pbs->x1 + pbs->a2 * pbs->x2 + pbs->a0 * x + pbs->b1 * pbs->y1 + pbs->b2 * pbs->y2;
		pbs->x2 = pbs->x1;
		pbs->y2 = pbs->y1;
		pbs->x1 = x;
		pbuffer[i] = y;
		pbs->y1 = y;
	}
}


void CEQ::ProcessMono(int *pbuffer, float *MixFloatBuffer, UINT nCount)
{
	MonoMixToFloat(pbuffer, MixFloatBuffer, nCount, 1.0f/MIXING_SCALEF);
	for (UINT b=0; b<MAX_EQ_BANDS; b++)
	{
		#ifdef ENABLE_X86
			if(GetProcSupport() & PROCSUPPORT_ASM_INTRIN)
			{
				if ((gEQ[b].bEnable) && (gEQ[b].Gain != 1.0f)) X86_EQFilter(&gEQ[b], MixFloatBuffer, nCount);
			} else
		#endif // ENABLE_X86
			{
				if ((gEQ[b].bEnable) && (gEQ[b].Gain != 1.0f)) EQFilter(&gEQ[b], MixFloatBuffer, nCount);
			}
	}
	FloatToMonoMix(MixFloatBuffer, pbuffer, nCount, MIXING_SCALEF);
}


void CEQ::ProcessStereo(int *pbuffer, float *MixFloatBuffer, UINT nCount)
{

#if defined(ENABLE_X86) && defined(ENABLE_SSE)

	if(GetProcSupport() & PROCSUPPORT_SSE)
	{
		int sse_state, sse_eqstate;
		MonoMixToFloat(pbuffer, MixFloatBuffer, nCount*2, 1.0f/MIXING_SCALEF);

		_asm stmxcsr sse_state;
		sse_eqstate = sse_state | 0xFF80; // set flush-to-zero, denormals-are-zero, round-to-zero, mask all exception, leave flags alone
		_asm ldmxcsr sse_eqstate;
		for (UINT b=0; b<MAX_EQ_BANDS; b++)
		{
			if ((gEQ[b].bEnable) || (gEQ[b+MAX_EQ_BANDS].bEnable))
				SSE_StereoEQ(&gEQ[b], &gEQ[b+MAX_EQ_BANDS], MixFloatBuffer, nCount);
		}
		_asm ldmxcsr sse_state;

		FloatToMonoMix(MixFloatBuffer, pbuffer, nCount*2, MIXING_SCALEF);

	} else

#endif // ENABLE_X86 && ENABLE_SSE

#ifdef ENABLE_X86
	if(GetProcSupport() & PROCSUPPORT_ASM_INTRIN)
	{

		StereoMixToFloat(pbuffer, MixFloatBuffer, MixFloatBuffer+MIXBUFFERSIZE, nCount, 1.0f/MIXING_SCALEF);
		
		for (UINT bl=0; bl<MAX_EQ_BANDS; bl++)
		{
			if ((gEQ[bl].bEnable) && (gEQ[bl].Gain != 1.0f)) X86_EQFilter(&gEQ[bl], MixFloatBuffer, nCount);
		}
		for (UINT br=MAX_EQ_BANDS; br<MAX_EQ_BANDS*2; br++)
		{
			if ((gEQ[br].bEnable) && (gEQ[br].Gain != 1.0f)) X86_EQFilter(&gEQ[br], MixFloatBuffer+MIXBUFFERSIZE, nCount);
		}

		FloatToStereoMix(MixFloatBuffer, MixFloatBuffer+MIXBUFFERSIZE, pbuffer, nCount, MIXING_SCALEF);

	} else
#endif // ENABLE_X86

	{

		StereoMixToFloat(pbuffer, MixFloatBuffer, MixFloatBuffer+MIXBUFFERSIZE, nCount, 1.0f/MIXING_SCALEF);
		
		for (UINT bl=0; bl<MAX_EQ_BANDS; bl++)
		{
			if ((gEQ[bl].bEnable) && (gEQ[bl].Gain != 1.0f)) EQFilter(&gEQ[bl], MixFloatBuffer, nCount);
		}
		for (UINT br=MAX_EQ_BANDS; br<MAX_EQ_BANDS*2; br++)
		{
			if ((gEQ[br].bEnable) && (gEQ[br].Gain != 1.0f)) EQFilter(&gEQ[br], MixFloatBuffer+MIXBUFFERSIZE, nCount);
		}

		FloatToStereoMix(MixFloatBuffer, MixFloatBuffer+MIXBUFFERSIZE, pbuffer, nCount, MIXING_SCALEF);

	}
}


CEQ::CEQ()
{
	memcpy(gEQ, gEQDefaults, sizeof(gEQ));
}


void CEQ::Initialize(bool bReset, DWORD MixingFreq)
{
	float32 fMixingFreq = (float32)MixingFreq;
	// Gain = 0.5 (-6dB) .. 2 (+6dB)
	for (UINT band=0; band<MAX_EQ_BANDS*2; band++) if (gEQ[band].bEnable)
	{
		float32 k, k2, r, f;
		float32 v0, v1;
		bool b = bReset;

		f = gEQ[band].CenterFrequency / fMixingFreq;
		if (f > 0.45f) gEQ[band].Gain = 1;
		// if (f > 0.25) f = 0.25;
		// k = tan(PI*f);
		k = f * 3.141592654f;
		k = k + k*f;
//		if (k > (float32)0.707) k = (float32)0.707;
		k2 = k*k;
		v0 = gEQ[band].Gain;
		v1 = 1;
		if (gEQ[band].Gain < 1.0)
		{
			v0 *= (0.5f/EQ_BANDWIDTH);
			v1 *= (0.5f/EQ_BANDWIDTH);
		} else
		{
			v0 *= (1.0f/EQ_BANDWIDTH);
			v1 *= (1.0f/EQ_BANDWIDTH);
		}
		r = (1 + v0*k + k2) / (1 + v1*k + k2);
		if (r != gEQ[band].a0)
		{
			gEQ[band].a0 = r;
			b = true;
		}
		r = 2 * (k2 - 1) / (1 + v1*k + k2);
		if (r != gEQ[band].a1)
		{
			gEQ[band].a1 = r;
			b = true;
		}
		r = (1 - v0*k + k2) / (1 + v1*k + k2);
		if (r != gEQ[band].a2)
		{
			gEQ[band].a2 = r;
			b = true;
		}
		r = - 2 * (k2 - 1) / (1 + v1*k + k2);
		if (r != gEQ[band].b1)
		{
			gEQ[band].b1 = r;
			b = true;
		}
		r = - (1 - v1*k + k2) / (1 + v1*k + k2);
		if (r != gEQ[band].b2)
		{
			gEQ[band].b2 = r;
			b = true;
		}
		if (b)
		{
			gEQ[band].x1 = 0;
			gEQ[band].x2 = 0;
			gEQ[band].y1 = 0;
			gEQ[band].y2 = 0;
		}
	} else
	{
		gEQ[band].a0 = 0;
		gEQ[band].a1 = 0;
		gEQ[band].a2 = 0;
		gEQ[band].b1 = 0;
		gEQ[band].b2 = 0;
		gEQ[band].x1 = 0;
		gEQ[band].x2 = 0;
		gEQ[band].y1 = 0;
		gEQ[band].y2 = 0;
	}
}


void CEQ::SetEQGains(const UINT *pGains, UINT nGains, const UINT *pFreqs, bool bReset, DWORD MixingFreq)
{
	for (UINT i=0; i<MAX_EQ_BANDS; i++)
	{
		float32 g, f = 0;
		if (i < nGains)
		{
			UINT n = pGains[i];
			if (n > 32) n = 32;
			g = ((float32)gEqLinearToDB[n]) / 64.0f;
			if (pFreqs) f = (float32)(int)pFreqs[i];
		} else
		{
			g = 1;
		}
		gEQ[i].Gain = g;
		gEQ[i].CenterFrequency = f;
		gEQ[i+MAX_EQ_BANDS].Gain = g;
		gEQ[i+MAX_EQ_BANDS].CenterFrequency = f;
		if (f > 20.0f)
		{
			gEQ[i].bEnable = true;
			gEQ[i+MAX_EQ_BANDS].bEnable = true;
		} else
		{
			gEQ[i].bEnable = false;
			gEQ[i+MAX_EQ_BANDS].bEnable = false;
		}
	}
	Initialize(bReset, MixingFreq);
}


void CQuadEQ::Initialize(bool bReset, DWORD MixingFreq)
{
	front.Initialize(bReset, MixingFreq);
	rear.Initialize(bReset, MixingFreq);
}

void CQuadEQ::SetEQGains(const UINT *pGains, UINT nGains, const UINT *pFreqs, bool bReset, DWORD MixingFreq)
{
	front.SetEQGains(pGains, nGains, pFreqs, bReset, MixingFreq);
	rear.SetEQGains(pGains, nGains, pFreqs, bReset, MixingFreq);
}

void CQuadEQ::Process(int *frontBuffer, int *rearBuffer, UINT nCount, UINT nChannels)
{
	if(nChannels == 1)
	{
		front.ProcessMono(frontBuffer, EQTempFloatBuffer, nCount);
	} else if(nChannels == 2)
	{
		front.ProcessStereo(frontBuffer, EQTempFloatBuffer, nCount);
	} else if(nChannels == 4)
	{
		front.ProcessStereo(frontBuffer, EQTempFloatBuffer, nCount);
		rear.ProcessStereo(rearBuffer, EQTempFloatBuffer, nCount);
	}
}


#else


MPT_MSVC_WORKAROUND_LNK4221(EQ)


#endif // !NO_EQ


OPENMPT_NAMESPACE_END