/* SPU2-X, A plugin for Emulating the Sound Processing Unit of the Playstation 2
 * Developed and maintained by the Pcsx2 Development Team.
 *
 * Original portions from SPU2ghz are (c) 2008 by David Quintana [gigaherz]
 *
 * This library is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your
 * option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along
 * with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
 */

#include "Spu2.h"
#include "DPLII.h"
#include <string.h>

static const u8 sLogTable[256] = {
	0x00,0x3C,0x60,0x78,0x8C,0x9C,0xA8,0xB4,0xBE,0xC8,0xD0,0xD8,0xDE,0xE4,0xEA,0xF0,
	0xF6,0xFA,0xFE,0x04,0x08,0x0C,0x10,0x14,0x16,0x1A,0x1E,0x20,0x24,0x26,0x2A,0x2C,
	0x2E,0x32,0x34,0x36,0x38,0x3A,0x3E,0x40,0x42,0x44,0x46,0x48,0x4A,0x4C,0x4E,0x50,
	0x50,0x52,0x54,0x56,0x58,0x5A,0x5A,0x5C,0x5E,0x60,0x60,0x62,0x64,0x66,0x66,0x68,
	0x6A,0x6A,0x6C,0x6E,0x6E,0x70,0x70,0x72,0x74,0x74,0x76,0x76,0x78,0x7A,0x7A,0x7C,
	0x7C,0x7E,0x7E,0x80,0x80,0x82,0x82,0x84,0x84,0x86,0x86,0x88,0x88,0x8A,0x8A,0x8C,
	0x8C,0x8C,0x8E,0x8E,0x90,0x90,0x92,0x92,0x92,0x94,0x94,0x96,0x96,0x96,0x98,0x98,
	0x9A,0x9A,0x9A,0x9C,0x9C,0x9C,0x9E,0x9E,0xA0,0xA0,0xA0,0xA2,0xA2,0xA2,0xA4,0xA4,
	0xA4,0xA6,0xA6,0xA6,0xA8,0xA8,0xA8,0xAA,0xAA,0xAA,0xAC,0xAC,0xAC,0xAC,0xAE,0xAE,
	0xAE,0xB0,0xB0,0xB0,0xB2,0xB2,0xB2,0xB2,0xB4,0xB4,0xB4,0xB6,0xB6,0xB6,0xB6,0xB8,
	0xB8,0xB8,0xB8,0xBA,0xBA,0xBA,0xBC,0xBC,0xBC,0xBC,0xBE,0xBE,0xBE,0xBE,0xC0,0xC0,
	0xC0,0xC0,0xC2,0xC2,0xC2,0xC2,0xC2,0xC4,0xC4,0xC4,0xC4,0xC6,0xC6,0xC6,0xC6,0xC8,
	0xC8,0xC8,0xC8,0xC8,0xCA,0xCA,0xCA,0xCA,0xCC,0xCC,0xCC,0xCC,0xCC,0xCE,0xCE,0xCE,
	0xCE,0xCE,0xD0,0xD0,0xD0,0xD0,0xD0,0xD2,0xD2,0xD2,0xD2,0xD2,0xD4,0xD4,0xD4,0xD4,
	0xD4,0xD6,0xD6,0xD6,0xD6,0xD6,0xD8,0xD8,0xD8,0xD8,0xD8,0xD8,0xDA,0xDA,0xDA,0xDA,
	0xDA,0xDC,0xDC,0xDC,0xDC,0xDC,0xDC,0xDE,0xDE,0xDE,0xDE,0xDE,0xDE,0xE0,0xE0,0xE0,
};

DPLII::DPLII( s32 lowpass_freq, s32 samplerate ) :
	LAccum( 0 ),
	RAccum( 0 ),
	ANum( 0 ),
	lpf_l( lowpass_freq, samplerate ),
	lpf_r( lowpass_freq, samplerate ),
	bufdone( 1 ),
	Gfl( 0 ),
	Gfr( 0 ),
	LMax( 0 ),
	RMax( 0 )
{
	memset( LBuff, 0, sizeof( LBuff ) );
	memset( RBuff, 0, sizeof( RBuff ) );
	memset( spdif_data, 0, sizeof( spdif_data ) );
}

// Takes a single stereo input sample and translates it into six output samples
// for 5.1 audio support on non-DPL2 hardware.
void DPLII::Convert( s16 *obuffer, s32 ValL, s32 ValR )
{
	ValL >>= 2;
	ValR >>= 2;
	if(PlayMode&4)
	{
		spdif_get_samples(spdif_data);
	}
	else
	{
		spdif_data[0]=0;
		spdif_data[1]=0;
		spdif_data[2]=0;
		spdif_data[3]=0;
		spdif_data[4]=0;
		spdif_data[5]=0;
	}

	//const u8 shift = SndOutVolumeShift;

	s32 XL = abs(ValL>>8);
	s32 XR = abs(ValR>>8);

	if(XL>LMax) LMax = XL;
	if(XR>RMax) RMax = XR;

	ANum++;
	if(ANum>=128)
	{
		ANum=0;
		LAccum = 1+((LAccum * 224 + LMax * 31)>>8);
		RAccum = 1+((RAccum * 224 + RMax * 31)>>8);

		LMax = 0;
		RMax = 0;

		s32 Tfl=(RAccum)*255/(LAccum);
		s32 Tfr=(LAccum)*255/(RAccum);

		int gMax = max(Tfl,Tfr);
		Tfl=Tfl*255/gMax;
		Tfr=Tfr*255/gMax;

		if(Tfl>255) Tfl=255;
		if(Tfr>255) Tfr=255;
		if(Tfl<1) Tfl=1;
		if(Tfr<1) Tfr=1;

		Gfl = (Gfl * 200 + Tfl * 56)>>8;
		Gfr = (Gfr * 200 + Tfr * 56)>>8;

	}

	s32 L,R,C,LFE,SL,SR,LL,LR;

	extern double pow_2_31;
	LL = (s32)(lpf_l.sample((ValL>>4)/pow_2_31)*pow_2_31);
	LR = (s32)(lpf_r.sample((ValR>>4)/pow_2_31)*pow_2_31);
	LFE = (LL + LR)>>4;

	C=(ValL+ValR)>>1; //16.8

	ValL-=C;//16.8
	ValR-=C;//16.8

	L=ValL>>8; //16.0
	R=ValR>>8; //16.0
	C=C>>8;    //16.0

	const s32 Cfl = 1 + sLogTable[Gfl];
	const s32 Cfr = 1 + sLogTable[Gfr];

	const s32 VL = (ValL>>4) * Cfl; //16.12
	const s32 VR = (ValR>>4) * Cfr;

	const s32 SC = (VL-VR)>>15;

	SL = (((VR/148 - VL/209)>>4)*Cfr)>>8;
	SR = (((VR/209 - VL/148)>>4)*Cfl)>>8;

	int AddCX  = (C * Config_DSound51.AddCLR)>>8;

	obuffer[0]=spdif_data[0] + (((L   * Config_DSound51.GainL  ))>>8) + AddCX;
	obuffer[1]=spdif_data[1] + (((R   * Config_DSound51.GainR  ))>>8) + AddCX;
	obuffer[2]=spdif_data[2] + (((C   * Config_DSound51.GainC  ))>>8); // - AddCX;
	obuffer[3]=spdif_data[3] + (((LFE * Config_DSound51.GainLFE))>>8);
	obuffer[4]=spdif_data[4] + (((SL  * Config_DSound51.GainSL ))>>8);
	obuffer[5]=spdif_data[5] + (((SR  * Config_DSound51.GainSR ))>>8);

#if 0
	if( UseAveraging )
	{
		LAccum+=abs(ValL);
		RAccum+=abs(ValR);
		ANum++;

		if(ANum>=512)
		{
			LMax=0;RMax=0;

			LAccum/=ANum;
			RAccum/=ANum;
			ANum=0;

			for(int i=0;i<127;i++)
			{
				LMax+=LBuff[i];
				RMax+=RBuff[i];
				LBuff[i]=LBuff[i+1];
				RBuff[i]=RBuff[i+1];
			}
			LBuff[127]=LAccum;
			RBuff[127]=RAccum;
			LMax+=LAccum;
			RMax+=RAccum;

			s32 TL = (LMax>>15)+1;
			s32 TR = (RMax>>15)+1;

			Gfl=(RMax)/(TL);
			Gfr=(LMax)/(TR);

			if(Gfl>255) Gfl=255;
			if(Gfr>255) Gfr=255;
		}
	}
	else
	{
		if(ValL>LMax) LMax = ValL;
		if(-ValL>LMax) LMax = -ValL;
		if(ValR>RMax) RMax = ValR;
		if(-ValR>RMax) RMax = -ValR;

		ANum++;
		if(ANum>=128)
		{
			// shift into a 21 bit value
			const u8 shift = SndOutVolumeShift-5;

			ANum=0;
			LAccum = ((LAccum * 224) + (LMax>>shift))>>8;
			RAccum = ((RAccum * 224) + (RMax>>shift))>>8;

			LMax=0;
			RMax=0;

			if(LAccum<1) LAccum=1;
			if(RAccum<1) RAccum=1;

			Gfl=(RAccum*256)/LAccum;
			Gfr=(LAccum*256)/RAccum;

			int gMax = max(Gfl,Gfr);

			Gfl=(Gfl*256)/gMax;
			Gfr=(Gfr*256)/gMax;

			if(Gfl>255) Gfl=255;
			if(Gfr>255) Gfr=255;
			if(Gfl<1) Gfl=1;
			if(Gfr<1) Gfr=1;
		}
	}

	Gfr = 1; Gfl = 1;

	extern double pow_2_31;

	// shift Values into 12 bits:
	u8 shift2 = SndOutVolumeShift + 4;

	const s32 LL = (s32)(lpf_l.sample((ValL>>shift2)/pow_2_31)*pow_2_31);
	const s32 LR = (s32)(lpf_r.sample((ValR>>shift2)/pow_2_31)*pow_2_31);
	const s32 LFE = (LL + LR)>>4;

	s32 C = (ValL+ValR)>>1; //16.8

	ValL -= C;//16.8
	ValR -= C;//16.8

	const s32 L = ValL>>SndOutVolumeShift; //16.0
	const s32 R = ValR>>SndOutVolumeShift; //16.0
	C >>= SndOutVolumeShift;    //16.0

	const s32 VL = (ValL>>4) * Gfl; //16.12
	const s32 VR = (ValR>>4) * Gfr;

	s32 SL = (VL/209 - VR/148)>>4; //16.0 (?)
	s32 SR = (VL/148 - VR/209)>>4; //16.0 (?)

	// increase surround stereo separation
	const int SC = (SL+SR)>>1; //16.0
	const int SLd = SL - SC;   //16.0
	const int SRd = SL - SC;   //16.0

	SL = (SLd * 209 + SC * 148)>>8; //16.0
	SR = (SRd * 209 + SC * 148)>>8; //16.0

	obuffer[0]=spdif_data[0] + (((L   * Config_DSound51.GainL  ) + (C * Config_DSound51.AddCLR))>>8);
	obuffer[1]=spdif_data[1] + (((R   * Config_DSound51.GainR  ) + (C * Config_DSound51.AddCLR))>>8);
	obuffer[2]=spdif_data[2] + (((C   * Config_DSound51.GainC  ))>>8);
	obuffer[3]=spdif_data[3] + (((LFE * Config_DSound51.GainLFE))>>8);
	obuffer[4]=spdif_data[4] + (((SL  * Config_DSound51.GainSL ))>>8);
	obuffer[5]=spdif_data[5] + (((SR  * Config_DSound51.GainSR ))>>8);
#endif
}