// CButterworthDesigner.cpp: implementation of the CButterworthDesigner class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "BtworthFilter.h"
#include "CButterworthDesigner.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CButterworthDesigner::CButterworthDesigner()
{

}

CButterworthDesigner::~CButterworthDesigner()
{

}

void CButterworthDesigner::m_IIRbcf(int band, int ns, int n, double f1, double f2, double f3, double f4,double *b, double *a)
{
	double fl,fh;
	double d[5],c[5];
	if((band==1)||(band==4))fl=f1;
	if((band==2)||(band==3))fl=f2;
	if(band<=3)fh=f3;
	if(band==4)fh=f4;
	for(int k=0;k<ns;k++)
	{
		m_Bwth(2*ns,k,4,d,c);
		m_Fblt(d,c,n,band,fl,fh,&b[k*(n+1)+0],&a[k*(n+1)+0]);
	}
}

void CButterworthDesigner::m_Bwth(int ln, int k, int n, double *d, double *c)
{
	int i;
	double pi,tmp;
	pi=4.0*atan(1.0);
	d[0]=1.0;
	c[0]=1.0;
	for(i=1;i<=n;i++)
	{
		d[i]=0.0;
		c[i]=0.0;
	}
	tmp=(k+1)-(ln+1.0)/2.0;
	if(tmp==0.0)
	{
		c[1]=1.0;
	}
	else
	{
		c[1]=-2.0*cos((2*(k+1)+ln-1)*pi/(2*ln));
		c[2]=1.0;
	}
}

void CButterworthDesigner::m_Fblt(double *d, double *c, int n, int band, double fln, double fhn, double *b, double *a)
{
	int i,k,m,n1,n2,ls;
	double pi,w,w0,w1,w2,tmp,tmpd,tmpc,*work;
	pi=4.0*atan(1.0);
	w1=tan(pi*fln);
	for(i=n;i>=0;i--)
	{
		if((c[i]!=0.0)||(d[i]!=0.0))
		{
			break;
		}
	}
	m=i;
	switch(band)
	{
	case 1:
	case 2:
		n2=m;
		n1=n2+1;
		if(band==2)
		{
			for(i=0;i<=m/2;i++)
			{
				tmp=d[i];
				d[i]=d[m-i];
				d[m-i]=tmp;
				tmp=c[i];
				c[i]=c[m-i];
				c[m-i]=tmp;
			}
		}
		for(i=0;i<=m;i++)
		{
			d[i]=d[i]/pow(w1,i);
			c[i]=c[i]/pow(w1,i);
		}
		break;
	case 3:
	case 4:
		n2=2*m;
		n1=n2+1;
		work=(double *)malloc(n1*n1*sizeof(double));
		w2=tan(pi*fhn);
		w=w2-w1;
		w0=w1*w2;
		if(band==4)
		{
			for(i=0;i<=m/2;i++)
			{
				tmp=d[i];
				d[i]=d[m-i];
				d[m-i]=tmp;
				tmp=c[i];
				c[i]=c[m-i];
				c[m-i]=tmp;
			}
		}
		for(i=0;i<=n2;i++)
		{
			work[0*n1+i]=0.0;
			work[1*n1+i]=0.0;
		}
		for(i=0;i<=m;i++)
		{
			tmpd=d[i]*pow(w,(m-i));
			tmpc=c[i]*pow(w,(m-i));
			for(k=0;k<=i;k++)
			{
				ls=m+i-2*k;
				tmp=m_Combin(i,i)/(m_Combin(k,k)*m_Combin(i-k,i-k));
				work[0*n1+ls]+=tmpd*pow(w0,k)*tmp;
				work[1*n1+ls]+=tmpc*pow(w0,k)*tmp;
			}
		}
		for(i=0;i<=n2;i++)
		{
			d[i]=work[0*n1+i];
			c[i]=work[1*n1+i];
		}
		free(work);
	}
	m_Bilinear(d,c,b,a,n);
}

double CButterworthDesigner::m_Combin(int i1, int i2)
{
	int i;
	double s;
	s=1.0;
	if(i2==0) return s;
	for(i=i1;i>(i1-i2);i--)
	{
		s*=i;
	}
	return s;
}

void CButterworthDesigner::m_Bilinear(double *d, double *c, double *b, double *a, int n)
{
	int i,j,n1;
	double sum,atmp,scale, * temp;
	n1=n+1;
	temp=(double*)malloc(n1*n1*sizeof(double));
	for(j=0;j<=n;j++)
	{
		temp[j*n1+0]=1.0;
	}		
	sum=1.0;
	for(i=1;i<=n;i++)
	{
		sum=sum*double(n-i+1)/(double) i;
		temp[0*n1+i]=sum;
	}
	for(i=1;i<=n;i++)
		for(j=1;j<=n;j++)
		{
			temp[j*n1+i]=temp[(j-1)*n1+i]-temp[j*n1+i-1] -temp[(j-1)*n1+i-1];
		}
	for(i=n;i>=0;i--)
	{
		b[i]=0.0;
		atmp=0.0;
		for(j=0;j<=n;j++)
		{
			b[i]=b[i]+temp[j*n1+i]*d[j];
			atmp=atmp+temp[j*n1+i]*c[j];
		}
		scale=atmp;
		if(i!=0) a[i]=atmp;
	}
	for(i=0;i<=n;i++)
	{
		b[i]=b[i]/scale;
		a[i]=a[i]/scale;
	}
	a[0]=1.0;
	free(temp);
}
void CButterworthDesigner::m_Gainc(double *b, double *a, int n, int ns, double *x, double *y, int len, int sign)
{
	int i,j,k,nl;
	double ar,ai,br,bi,zr,zi,im,re,den,numr,numi,freq,temp;
	double hr,hi,tr,ti;
	nl=	n+1;
	for(k=0;k<len;k++)
	{
		freq=k*0.5/(len-1);
		zr=cos(-8.0*atan(1.0)*freq);
		zi=sin(-8.0*atan(1.0)*freq);
		x[k]=1.0;
		y[k]=0.0;
		for(j=0;j<ns;j++)
		{
			br=0.0;
			bi=0.0;
			for(i=n;i>0;i--)
			{
				re=br;
				im=bi;
				br=(re+b[j*nl+i])*zr-im*zi;
				bi=(re+b[j*nl+i])*zi+im*zr;
			}//for(i=n;i>0;i--)
			ar=0.0;
			ai=0.0;
			for(i=n;i>0;i--)
			{
				re=ar;
				im=ai;
				ar=(re+a[j*nl+i])*zr-im*zi;
				ai=(re+a[j*nl+i])*zi+im*zr;
			}//for(i=n;i>0;i--)
			br=br+b[j*nl+0];
			ar=ar+1.0;
			numr=ar*br+ai*bi;
			numi=ar*bi-ai*br;
			den=ar*ar+ai*ai;
			hr=numr/den;
			hi=numi/den;
			tr=x[k]*hr-y[k]*hi;
			ti=x[k]*hi+y[k]*hr;
			x[k]=tr;
			y[k]=ti;
		}//for(j=0;j<ns;j++)
		////////////////////////////////////
		//sign==1;

		temp=sqrt(fabs(x[k]*x[k]-y[k]*y[k]));
		if(temp!=0.0)
		{
			y[k]=atan2(y[k],x[k]);
		}
		else
		{
			y[k]=0.0;
		}
		x[k]=temp;

	}//for(k=0;k<len;k++)
}

void CButterworthDesigner::ButterworthLowPassDesign(double fl, double *b, double *a)
{
	int band=1;
	int n=2;
	int ns=20;
	m_IIRbcf(band,ns,n,fl/8000.0,0.0,0.0,0.0,b,a);
}
void CButterworthDesigner::ButterworthBandPassDesign(double fl, double fh, double *b, double *a)
{
	int band=3;
	int n=4;
	int ns=20;
	m_IIRbcf(band,ns,n,0.0,fl/8000.0,fh/8000.0,0.0,b,a);
}
void CButterworthDesigner::ButterworthGainC(double *b, double *a, int n,double *x, double *y)
{
	m_Gainc(b,a,n,20,x,y,300,2); 
}