Make your own free website on Tripod.com

To see the main program
To go back to homepage

/*----------------------------------------------------------------------*/
/*									*/
/* program name: litlib.c						*/
/*									*/
/* This program is a library of commonly used functions for various     */
/* programs in this and other chapters					*/
/*									*/
/*									*/
/* Manny Ifeachor, June 1992.						*/
/*									*/
/*----------------------------------------------------------------------*/

#include "dsp1.h"
#include "dsp21.h"

/* function to read coefficient values of iir or fir systems		*/

void read__coeffs()
{
	int i,j;
	extern int M,N,nstage,iir;
	extern double A[size], B[size], Ni[size], Di[size];

	if((in = fopen("coeff.dat","r"))==NULL)
	{
		printf("cannot open file coeff.dat\n");
		exit(1);
        }
      zero__arrays();	/* initialize all global arrays */

      if (iir==1)
      {
	printf("specify type of system \n");
	printf("0 for iir system \n");
	printf("1 for fir system \n");
	scanf("%d",&iir);
	}
	switch(iir)
	{
	case 0:			/* read iir filter coefficients for H(z) in cascade */
		fscanf(in,"%d",&nstage);
		j=0;
		for(i=0;i<nstage;++i)
		{
			fscanf(in,"%lf %lf %lf",&Di[j],&Di[j+1],&Di[j+2]);
			fscanf(in,"%lf %lf %lf",&Ni[j],&Ni[j+1],&Ni[j+2]);
			j=j+3;
		}
		M=nstage*2;
		N=M;
		break;
	case 1:
		fscanf(in,"%d",&N);
		for(i=0;i<N;++i)
		{
			fscanf(in,"%lf",&A[i]);
		}
		B[0]=1.0;
		M=N;
		break;
	default:
		printf("invalid option selected \n");
		printf("press enter to continue \n");
		getchar();
		break;
	}
	fclose(in);
}
/*------------------------------------------------------------------------*/

/* function performs complex additon */

complex cadd1(complex pa, complex pb, int nadd)
{
	long k;
	complex np;
	np.real=0;
	np.imag=0;
	if(nadd==0)
	{
		np.real=pa.real-pb.real;
		np.imag=pa.imag-pb.imag;
	}
	if(nadd==1)
	{
		np.real=pa.real+pb.real;
		np.imag=pa.imag+pb.imag;
	}
	np.modulus=sqrt(np.real*np.real+np.imag*np.imag);
	np=fixnp(np);
	return(np);
}
/*-----------------------------------------------------------------------*/

/* Function to adjust angles of complex numbers to their correct values */

complex fixnp(complex pa)
{
	long k;
	complex np;
	np=pa;
	if(fabs(pa.real)<0.0000000008)
	{
		np.real=0;
	}
	if(fabs(pa.imag)<0.0000000008)
	{
		np.imag=0;
	}
	if(np.real==0 && np.imag==0)
	{
		np.angle=0;
		return(np);
	}
	if(np.real==0 && np.imag!=0)
	{
		if(np.imag>0)
		 	np.angle=pi/2;
		if(np.imag<0)
			np.angle=1.5*pi;
		return(np);
	}
	if(np.real!=0 && np.imag==0)
	{
		if(np.real>0)
			np.angle=0;
		if(np.real<0)
			np.angle=pi;
		np.modulus=fabs(pa.real);
		return(np);
	}
	np.angle=atan(np.imag/np.real);
	if(np.real<0)
		np.angle=np.angle+pi;
	k=(long)(np.angle/2*pi);
	np.angle=np.angle-k*2*pi;
	np.modulus=sqrt(pa.real*pa.real+pa.imag*pa.imag);
	return(np);
}
/*--------------------------------------------------------------------------*/

/* function to multiply two complex numbers */

complex cmul1(complex pa, complex pb)
{
	complex np;
	np.real=0;
	np.imag=0;
	np.modulus=pa.modulus*pb.modulus;
	np.real=np.modulus*cos(pa.angle+pb.angle);
	np.imag=np.modulus*sin(pa.angle+pb.angle);
	np=fixnp(np);
	return(np);
}
/*-------------------------------------------------------------------------*/
/* function to divide two complex numbers */
complex cdiv1(complex pa, complex pb)
{
	complex np;
	np.real=0;
	np.imag=0;
	np.modulus=pa.modulus/pb.modulus;
	np.real=np.modulus*cos(pa.angle-pb.angle);
	np.imag=np.modulus*sin(pa.angle-pb.angle);
	np=fixnp(np);
	return(np);
}
/*-------------------------------------------------------------------------*/
/* Routine to compute and express the pole of a second order section in    */
/* polar and rectangular forms. The denominator polynomial has the form    */
/*									   */
/*				D(z)=1+b1z'-1+b2z'-2			   */
/*									   */
/*									   */
/*-------------------------------------------------------------------------*/

complex polar__pole()
{
	int i,k,j=0;
	complex px,ptemp;
	double temp;
	extern int nstage;
	extern double Di[size];
	extern complex pk[10];

	for(i=0;i<nstage;++i)		
	{
		temp=Di[1+j]*Di[1+j]-4*Di[2+j];
		if(temp>=0)	 /* poles are real */
		{
			px.imag=0;
			ptemp.imag=0;
			if(Di[2+j]==0)	/*simple poles */
			{
				px.real=-Di[1+j];
				ptemp.real=0;
			}
			else
			{
				px.real=(-Di[1+j]+sqrt(temp))/2;
				ptemp.real=(-Di[1+j]-sqrt(temp))/2;
			}
			px.modulus=fabs(px.real);
			ptemp.modulus=fabs(ptemp.real);
			px.angle=0;
			ptemp.angle=0;
			if(px.real<0)
				px.angle=acos(-1);
			if(ptemp.real<0)
				ptemp.angle=acos(-1);
		}
		else			/* complex conjugate poles */
		{
			px.modulus=sqrt(fabs(Di[2+j]));
			px.angle=acos(-Di[1+j]/(2*px.modulus));
			px.real=px.modulus*cos(px.angle);
			px.imag=px.modulus*sin(px.angle);
			ptemp.modulus=px.modulus;
			ptemp.angle=-px.angle;
			ptemp.real=px.real;
			ptemp.imag=-px.imag;
		}
		k=2*i+1;
		pk[k]=px;
		pk[k+1]=ptemp;
		j=j+3;
	}
}
/*-------------------------------------------------------------------------*/
/*      Routine to evaluate a given polynomial, of ordder n, at z=pi. The result is returned as a complex number in polar and rectangular forms            */

complex poly__polar(double P2[], complex pj, int n)
{
	complex np;
	int i,k;
	i=0;
	np.real=0;
	np.imag=0;

	for(i=0;i<n;++i)
	{
		k=n-i;
		np.real=np.real+P2[i]*pow(pj.modulus,k)*cos(k*pj.angle);
		np.imag=np.imag+P2[i]*pow(pj.modulus,k)*sin(k*pj.angle);
	}
	np.real=np.real+P2[n];
	np.modulus=sqrt(np.real*np.real+np.imag*np.imag);
	np.angle=atan(np.imag/np.real);
	if(np.real<0)
		np.angle=np.angle+pi;
	return(np);
}
/*----------------------------------------------------------------------------
Function to compute the absolute magnitude of a complex number
*/

double cabs1(complex z)
{
	double temp;
	temp=z.real*z.real+z.imag*z.imag;
	temp=sqrt(temp);
	return (temp);
}
/*--------------------------------------------------------------------------
Function to produce a polynomial in direct form */

void poly__product1(double P1[], double AB[], int nsect)
{
	double C[size], D1[size], D2[size];
	int i,NN;
		if(nsect<2)
		{
			for(i=0;i<3;++i)
			{
				AB[i]=P1[i];
			}
		return;
		}
		D1[0]=P1[0];	/* retrieve F1(z) */
		D1[1]=P1[1];
		D1[2]=P1[2];

		D2[0]=P1[3];	/*retrieve F2(z)*/
		D2[1]=P1[4];
		D2[2]=P1[5];
		NN=2;
		poly__product(C,D1,D2,NN);	/*compute F1(z)F2(z) */

	if(nsect>2)
	{
		D1[0]=C[0];	/*retrieve F1(z)F2(z) */
		D1[1]=C[1];
		D1[2]=C[2];
		D1[3]=C[3];
		D1[4]=C[4];
		D2[0]=P1[6];	/*retrieve F3(z) */
		D2[1]=P1[7];
		D2[2]=P1[8];
		D2[3]=0.0;
		D2[4]=0.0;
		NN=4;
		poly__product(C,D1,D2,NN);	/*compute the product F1(z)F2(z)F3(z) */
	}
	if(nsect>3)
	{
		D1[0]=C[0];	/*retrieve F1(z)F2(z)F3(z) */
		D1[1]=C[1];
		D1[2]=C[2];
		D1[3]=C[3];
		D1[4]=C[4];
		D1[5]=C[5];
		D1[6]=C[6];

		D2[0]=P1[9];	/*retrieve F4(z) */
		D2[1]=P1[10];
		D2[2]=P1[11];
		D2[3]=0.0;
		D2[4]=0.0;
		D2[5]=0.0;
		D2[6]=0.0;

		NN=6;
		poly__product(C,D1,D2,NN);	/*compute [F1(z)F2(z)][F2(z)F4(z)] */
	}
	if(nsect>4)
	{
		D1[0]=C[0];	/*retrieve F1(z)F2(z)F3(z)F4(z) */
		D1[1]=C[1];
		D1[2]=C[2];
		D1[3]=C[3];
		D1[4]=C[4];
		D1[5]=C[5];
		D1[6]=C[6];
		D1[7]=C[7];
		D1[8]=C[8];

		D2[0]=P1[9];	/*retrieve F5(z) */
		D2[1]=P1[10];
		D2[2]=P1[11];
		D2[3]=0.0;
		D2[4]=0.0;
		D2[5]=0.0;
		D2[6]=0.0;
		D2[7]=0.0;
		D2[8]=0.0;

		NN=8;
		poly__product(C,D1,D2,NN);	/*compute [F1(z)F2(z)][F2(z)F4(z)F5(z)] */
		}

		/* save result */

		for(i=0;i<(2*nsect+1);++i)
		{
			AB[i]=C[i];
		}
}
/*-----------------------------------------------------------------------*/
/* Function to compute the product of two ploynomials			 */

void poly__product(double C[], double D1[], double D2[], int NN)
{
	int i,j,k,j1;
	double sum1=0.0, sum2=0.0;

	for(i=0;i<size;++i)	/*initialize the result array */
	{
		C[i]=0.0;
	}
	for(i=0;i<(NN+1);++i)
	{
		j=NN-i;
		j1=i+NN;
		for(k=0;k<(j+1);++k)
		{
			sum1=sum1+D1[k]*D2[j-k];
			sum2=sum2+D1[i+k]*D2[NN-k];
		}
		C[j]=sum1;
		C[j1]=sum2;
		sum1=0.0;
		sum2=0.0;
	}
}

/*------------------------------------------------------------------------*/
/*Function to zero global arrays used as inputs */

void zero__arrays()
{
	long i;
	extern double A[size], B[size], Ni[size], Di[size];
	extern double ak[size], bk[size], h[size];

	for(i=0;i<size;++i)
	{
		A[i]=0.0;
		B[i]=0.0;
		Ni[i]=0;
		Di[i]=0;
		ak[i]=0;
		bk[i]=0;
		h[i]=0;
	}
}
/*------------------------------------------------------------------------*/
/* Function to compute the izt using the power series method		  */

void power__series()
{
	int i,k;
	long n;
	double sum1,sum2,sum3=0;
	double temp;
	extern double A[size], B[size], h[size];
	extern float l1n, l2n;
	extern long npt;
	extern int M;

	/* compute h[n] recursively */

	h[0]=A[0]/B[0];
	sum2=h[0];
	for(n=1;n<npt;++n)
	{
		sum1=0.0;
		k=n;
		if(n>M)
			k=M;
		for(i=1;i<=k;++i)
		{
			sum1=sum1+h[n-i]*B[i];
		}
		h[n]=(A[n]-sum1)/B[0];
		temp=signx(h[n])*h[n];
		sum2=sum2+temp;
		sum3=sum3+temp*temp;
	}
	l1n=sum2;
	l2n=sqrt(sum3);
}
/*-------------------------------------------------------------------------*/

int signx(double x)
{
	int temp;
	if(x<0)
		temp=-1;
	else
		temp=1;
	return(temp);
}