/*****************************************************************************
 *
 * This program is free software ; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
 *
 * $Id: equalizer.c 271 2005-08-09 08:31:35Z picard $
 *
 * The Core Pocket Media Player
 * Copyright (c) 2004-2005 Gabor Kovacs
 *
 ****************************************************************************/

#include "common.h"
#include "fixed.h"
#include <math.h>

#define MAXFILTER		10
#define ACCFAST_ADJUST	1

typedef struct eqfilter
{
	fix_t alpha;
	fix_t beta;
	fix_t gamma;
	fix_t alpha0;

} eqfilter;

typedef struct equalizer
{
	codec Codec;
	bool_t Enabled;
	bool_t Attenuate;
	int Amplify;	//-20..20
	int Eq[10];		//-20..20
	buffer Buffer;
	int Channels;
	void* PCM;
	fix_t Scale;
	fix_t ScalePreamp;
	fix_t ScaleFinal;
	eqfilter Filter[MAXFILTER];
	fix_t State[MAXPLANES][3+MAXFILTER*2];
	planes Tmp;

} equalizer;

static const datatable Params[] = 
{
	{ EQUALIZER_ENABLED,	TYPE_BOOL,DF_SETUP|DF_CHECKLIST },
	{ EQUALIZER_ATTENUATE,	TYPE_BOOL,DF_SETUP|DF_CHECKLIST },
	{ EQUALIZER_PREAMP,		TYPE_INT, DF_SETUP|DF_MINMAX|DF_SHORTLABEL, -20, 20 },

	{ EQUALIZER_1,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL|DF_GAP, -20, 20 },
	{ EQUALIZER_2,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_3,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_4,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_5,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_6,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_7,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_8,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_9,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },
	{ EQUALIZER_10,			TYPE_INT, DF_SETUP|DF_MINMAX|DF_NOWRAP|DF_SHORTLABEL, -20, 20 },

	{ EQUALIZER_RESET,		TYPE_RESET,DF_SETUP|DF_NOSAVE|DF_GAP },

	DATATABLE_END(EQUALIZER_ID)
};

static int Enum( equalizer* p, int* No, datadef* Param)
{
	if (CodecEnum(&p->Codec,No,Param)==ERR_NONE)
		return ERR_NONE;
	return NodeEnumTable(No,Param,Params);
}

static int Get( equalizer* p, int No, void* Data, int Size )
{
	int Result = CodecGet(&p->Codec,No,Data,Size);
	switch (No)
	{
	case EQUALIZER_RESET: Result = ERR_NONE; break;
	case EQUALIZER_ENABLED: GETVALUE(p->Enabled,bool_t); break;
	case EQUALIZER_ATTENUATE: GETVALUE(p->Attenuate,bool_t); break;
	case EQUALIZER_PREAMP: GETVALUE(p->Amplify,int); break;
	case EQUALIZER_1:
	case EQUALIZER_2:
	case EQUALIZER_3:
	case EQUALIZER_4:
	case EQUALIZER_5:
	case EQUALIZER_6:
	case EQUALIZER_7:
	case EQUALIZER_8:
	case EQUALIZER_9:
	case EQUALIZER_10:GETVALUE(p->Eq[No-EQUALIZER_1],int); break;
	}
	return Result;
}

static NOINLINE void UpdateScale(equalizer* p)
{
	fix_t Scale;
	int n;

	if (!p->Attenuate)
		p->Scale=FIXC(1.);
	if (p->Scale<FIXC(1./65556))
		p->Scale=FIXC(1./65556);
	Scale = fix_mul(p->Scale,p->ScalePreamp);
	p->ScaleFinal = FIXFAST_BSHIFT(Scale);
	for (n=0;n<MAXFILTER;++n)
		p->Filter[n].alpha = ACCFAST_BSHIFT(fix_mul(p->Filter[n].alpha0,Scale));
}

static const eqfilter Band44100[MAXFILTER] =
{
	{ FIXC(0.003013),FIXC(-0.993973),FIXC( 1.993901) },
	{ FIXC(0.008490),FIXC(-0.983019),FIXC( 1.982437) },
	{ FIXC(0.015374),FIXC(-0.969252),FIXC( 1.967331) },
	{ FIXC(0.029328),FIXC(-0.941343),FIXC( 1.934254) },
	{ FIXC(0.047918),FIXC(-0.904163),FIXC( 1.884869) },
	{ FIXC(0.130408),FIXC(-0.739184),FIXC( 1.582718) },
	{ FIXC(0.226555),FIXC(-0.546889),FIXC( 1.015267) },
	{ FIXC(0.344937),FIXC(-0.310127),FIXC(-0.181410) },
	{ FIXC(0.366438),FIXC(-0.267123),FIXC(-0.521151) },
	{ FIXC(0.379009),FIXC(-0.241981),FIXC(-0.808451) },
};

static const fix_t PowTab[37] = //pow(10,(n-18)/20.f))
{
	FIXC(0.125892541),FIXC(0.141253754),FIXC(0.158489319),FIXC(0.177827941),
	FIXC(0.199526231),FIXC(0.223872114),FIXC(0.251188643),FIXC(0.281838293),
	FIXC(0.316227766),FIXC(0.354813389),FIXC(0.398107171),FIXC(0.446683592),
	FIXC(0.501187234),FIXC(0.562341325),FIXC(0.630957344),FIXC(0.707945784),
	FIXC(0.794328235),FIXC(0.891250938),FIXC(1.000000000),FIXC(1.122018454),
	FIXC(1.258925412),FIXC(1.412537545),FIXC(1.584893192),FIXC(1.778279410),
	FIXC(1.995262315),FIXC(2.238721139),FIXC(2.511886432),FIXC(2.818382931),
	FIXC(3.162277660),FIXC(3.548133892),FIXC(3.981071706),FIXC(4.466835922),
	FIXC(5.011872336),FIXC(5.623413252),FIXC(6.309573445),FIXC(7.079457844),
	FIXC(7.943282347),
};

static INLINE fix_t Pow(int n)
{
	if (n<-18) n=-18;
	if (n>18) n=18;
	return PowTab[n+18];
}

static NOINLINE int UpdateParam(equalizer* p)
{
	if (p->Codec.In.Format.Type == PACKET_AUDIO)
	{
		int n;
		const eqfilter *src;
		eqfilter *dst;

		src = Band44100;
		dst = p->Filter;
		for (n=0;n<MAXFILTER;++n,++src,++dst)
		{
			dst->alpha0 = fix_mul(src->alpha,Pow(p->Eq[n])-FIXC(1.));
			dst->beta = ACCFAST_BSHIFT(src->beta);
			dst->gamma = ACCFAST_BSHIFT(src->gamma);
		}

		p->ScalePreamp = Pow(p->Amplify);
		UpdateScale(p);
	}
	return ERR_NONE;
}

static NOINLINE int UpdateEqualizer()
{
	context* p = Context();
	if (!p->Player)
		return ERR_NONE;
	return p->Player->Set(p->Player,PLAYER_UPDATEEQUALIZER,NULL,0);
}

static int Set( equalizer* p, int No, const void* Data, int Size )
{
	int Result = CodecSet(&p->Codec,No,Data,Size);
	switch (No)
	{
	case EQUALIZER_ENABLED: SETVALUE(p->Enabled,bool_t,UpdateEqualizer()); break;
	case EQUALIZER_ATTENUATE: SETVALUE(p->Attenuate,int,UpdateParam(p)); break;
	case EQUALIZER_PREAMP: SETVALUE(p->Amplify,int,UpdateParam(p)); break;
	case EQUALIZER_1: 
	case EQUALIZER_2: 
	case EQUALIZER_3: 
	case EQUALIZER_4: 
	case EQUALIZER_5: 
	case EQUALIZER_6: 
	case EQUALIZER_7: 
	case EQUALIZER_8: 
	case EQUALIZER_9: 
	case EQUALIZER_10:SETVALUE(p->Eq[No-EQUALIZER_1],int,UpdateParam(p)); break;
	}
	return Result;
}

static int Flush(equalizer* p)
{
	memset(p->State,0,sizeof(p->State));
	return ERR_NONE;
}

static int UpdateInput(equalizer* p)
{
	PCMRelease(p->PCM);
	p->PCM = NULL;
	BufferClear(&p->Buffer);

	if (p->Codec.In.Format.Type == PACKET_AUDIO)
	{
		if (!p->Enabled || p->Codec.In.Format.Format.Audio.Channels>MAXPLANES)
			return ERR_INVALID_PARAM;

		PacketFormatPCM(&p->Codec.Out.Format,&p->Codec.In.Format,FIX_FRACBITS+1);
		p->Codec.Out.Format.Format.Audio.Bits = sizeof(fix_t)*8;
		p->Codec.Out.Format.Format.Audio.Flags = PCM_PLANES;
#ifndef FIXED_POINT
		p->Codec.Out.Format.Format.Audio.Flags |= PCM_FLOAT;
#endif

		p->PCM = PCMCreate(&p->Codec.Out.Format.Format.Audio,&p->Codec.In.Format.Format.Audio,0,0);
		if (!p->PCM)
			return ERR_OUT_OF_MEMORY;
		p->Scale = FIXC(1);
		UpdateParam(p);
		Flush(p);
	}
	return ERR_NONE;
}

static NOINLINE void Rescale(planes data, fix_t v, int ramp, size_t length, int channels)
{
	fix_t dv;
	int n;

	if (length < 4*sizeof(fix_t)) return;
	while ((sizeof(fix_t)<<ramp) > length) --ramp;

#ifdef FIXED_POINT
	dv = (v-FIXC(1)) >> ramp;
#else
	dv = (fix_t)ldexp(v-FIXC(1),-ramp);
#endif

	for (n=0;n<channels;++n)
	{
		fix_t* i=data[n];
		fix_t* ie=i+(1<<ramp);

		v = FIXC(1);
		for (;i!=ie;++i,v+=dv)
			i[0] = fix_mul(i[0],v);

		ie = (fix_t*)((uint8_t*)ie+length)-(1<<ramp);
		for (;i!=ie;i+=4)
		{
			i[0] = fix_mul(i[0],v);
			i[1] = fix_mul(i[1],v);
			i[2] = fix_mul(i[2],v);
			i[3] = fix_mul(i[3],v);
		}
	}
}

static int Process(equalizer* p, const packet* Packet, const flowstate* State)
{
	fix_t* s;
	int n,DstLength,SrcLength,Channels;
	if (!Packet)
		return ERR_NEED_MORE_DATA;

	Channels = p->Codec.In.Format.Format.Audio.Channels;
	SrcLength = Packet->Length;
	DstLength = PCMDstLength(p->PCM,SrcLength);
	if (DstLength*Channels*2 > p->Buffer.Allocated)
	{
		uint8_t* Data;
		if (!BufferAlloc(&p->Buffer,DstLength*Channels*2,1024))
			return ERR_OUT_OF_MEMORY;

		Data = p->Buffer.Data;
		for (n=0;n<Channels;++n)
		{
			p->Codec.Packet.Data[n] = Data; 
			Data += DstLength;
			p->Tmp[n] = Data; 
			Data += DstLength;
		}
	}

	PCMConvert(p->PCM,*(planes*)&p->Codec.Packet.Data,Packet->Data,&DstLength,&SrcLength,SPEED_ONE,256);
	p->Codec.Packet.RefTime = Packet->RefTime;
	p->Codec.Packet.Length = DstLength;

	s = p->State[0];
	for (n=0;n<Channels;++n)
	{
		eqfilter* f;
		fix_t* i=(fix_t*)p->Codec.Packet.Data[n];
		fix_t* ie=(fix_t*)((uint8_t*)i+DstLength);
		fix_t* j=(fix_t*)p->Tmp[n];
		fix_t a,b,c,d,e;
		e = p->ScaleFinal;
		a = s[2];
		b = s[1];
		c = s[0];
		for (ie-=3;i<ie;i+=4,j+=4)
		{
			d=i[0];
			i[0]=fixfast_mul(c,e);
			j[0]=ACCFAST_ASHIFT(c-a,ACCFAST_ADJUST);
			a=i[1];
			i[1]=fixfast_mul(d,e);
			j[1]=ACCFAST_ASHIFT(d-b,ACCFAST_ADJUST);
			b=i[2];
			i[2]=fixfast_mul(a,e);
			j[2]=ACCFAST_ASHIFT(a-c,ACCFAST_ADJUST);
			c=i[3];
			i[3]=fixfast_mul(b,e);
			j[3]=ACCFAST_ASHIFT(b-d,ACCFAST_ADJUST);
		}
		for (ie+=3;i!=ie;++i,++j)
		{
			j[0]=ACCFAST_ASHIFT(c-a,ACCFAST_ADJUST);
			a=b;
			b=c;
			c=i[0];
			i[0]=fixfast_mul(b,e);
		}
		s[2] = a;
		s[1] = b;
		s[0] = c;
		s += 3;

		for (f=p->Filter;f!=p->Filter+MAXFILTER;f+=2,s+=2*2)
			if (f[0].alpha!=0 || f[1].alpha!=0)
			{
				accfast_var(v0);
				accfast_var(v1);

				i=(fix_t*)p->Codec.Packet.Data[n];
				ie=(fix_t*)((uint8_t*)i+DstLength);
				j=(fix_t*)p->Tmp[n];
				a = s[0];
				c = s[1];
				b = s[2];
				d = s[3];
				for (--ie;i<ie;i+=2,j+=2)
				{
					accfast_mul(v0,a,f[0].beta);
					accfast_mul(v1,b,f[1].beta);
					accfast_mla(v0,j[0],f[0].alpha);
					accfast_mla(v1,j[0],f[1].alpha);
					accfast_mla(v0,c,f[0].gamma);
					accfast_mla(v1,d,f[1].gamma);
					a = accfast_get(v0,ACCFAST_ADJUST);
					b = accfast_get(v1,ACCFAST_ADJUST);
					i[0] += a+b;
					a = ACCFAST_ASHIFT(a,ACCFAST_ADJUST);
					b = ACCFAST_ASHIFT(b,ACCFAST_ADJUST);
					accfast_mul(v0,c,f[0].beta);
					accfast_mul(v1,d,f[1].beta);
					accfast_mla(v0,j[1],f[0].alpha);
					accfast_mla(v1,j[1],f[1].alpha);
					accfast_mla(v0,a,f[0].gamma);
					accfast_mla(v1,b,f[1].gamma);
					c = accfast_get(v0,ACCFAST_ADJUST);
					d = accfast_get(v1,ACCFAST_ADJUST);
					i[1] += c+d;
					c = ACCFAST_ASHIFT(c,ACCFAST_ADJUST);
					d = ACCFAST_ASHIFT(d,ACCFAST_ADJUST);
				}
				if (i==ie)
				{
					accfast_mul(v0,a,f[0].beta);
					accfast_mul(v1,b,f[1].beta);
					accfast_mla(v0,j[0],f[0].alpha);
					accfast_mla(v1,j[0],f[1].alpha);
					accfast_mla(v0,c,f[0].gamma);
					accfast_mla(v1,d,f[1].gamma);
					a = c; c = accfast_get(v0,ACCFAST_ADJUST);
					b = d; d = accfast_get(v1,ACCFAST_ADJUST);
					i[0] += c+d;
					c = ACCFAST_ASHIFT(c,ACCFAST_ADJUST);
					d = ACCFAST_ASHIFT(d,ACCFAST_ADJUST);
				}
				s[0] = a;
				s[1] = c;
				s[2] = b;
				s[3] = d;
			}
	}

	if (p->Attenuate)
	{
		fix_t Max = 0;
		for (n=0;n<Channels;++n)
		{
			fix_t* i=(fix_t*)p->Codec.Packet.Data[n];
			fix_t* ie=(fix_t*)((uint8_t*)i+DstLength);
			for (;i!=ie;++i)
			{
				fix_t v = *i;
				if (v<0) v=-v;
				if (v>Max) Max=v;
			}
		}

		if (Max > FIXC(1.))
		{
			Max = fix_mul(fix_1div(Max),FIXC(15./16));
			Rescale(*(planes*)&p->Codec.Packet.Data,Max,5,DstLength,Channels);
			
			p->Scale = fix_mul(p->Scale,Max);
			UpdateScale(p);
		}
		else
		if (Max < FIXC(6./8) && p->Scale < FIXC(1-1./256))
		{
			Max = FIXC(32./31);
			if (fix_mul(p->Scale,Max) > FIXC(1))
				Max = fix_1div(p->Scale);
			Rescale(*(planes*)&p->Codec.Packet.Data,Max,10,DstLength,Channels);
		
			p->Scale = fix_mul(p->Scale,Max);
			UpdateScale(p);
		}
	}
	else
	if (p->Scale != FIXC(1.))
		UpdateScale(p);

	return ERR_NONE;
}

static int Create(equalizer* p)
{
	p->Codec.Node.Enum = (nodeenum)Enum;
	p->Codec.Node.Set = (nodeset)Set;
	p->Codec.Node.Get = (nodeget)Get;
	p->Codec.Process = (packetprocess)Process;
	p->Codec.UpdateInput = (nodefunc)UpdateInput;
	p->Codec.Flush = (nodefunc)Flush;
	return ERR_NONE;
}

static const nodedef Equalizer =
{
	sizeof(equalizer)|CF_GLOBAL|CF_SETTINGS,
	EQUALIZER_ID,
	CODEC_CLASS,
	PRI_MAXIMUM+580,
	(nodecreate)Create,
};

void Equalizer_Init()
{
	NodeRegisterClass(&Equalizer);
}

void Equalizer_Done()
{
	NodeUnRegisterClass(EQUALIZER_ID);
}