/* ellf.c
 * 
 * Read ellf.doc before attempting to compile this program.
 */


/* size of arrays: */
#define ARRSIZ 50

double atan2();

/* 1 = yes, 0 = no: */
#define DECPDP 0
#define UNK 1
#define CEPHES 0

#if DECPDP
#define asin arcsin
#define atan arctan
#define artan2(a,b) (atan2(a,b))
#endif


#if CEPHES
#define gtlin gets
#define artan2(a,b) (atan2(a,b))
#endif

#if UNK
#define gtlin gets
#define artan2(a,b) (atan2(b,a))
double PI   = 3.14159265358979323846;
double PIO2 = 1.57079632679489661923;
/* MACHEP is the relative resolution of your computer's
 * floating point arithmetic.  It will be 1.1E-16 for IEEE
 * double precision, 1.39E-17 for DEC VAX or PDP-11. */
double MACHEP = 1.1e-16;
double MAXNUM = 1.5e38;
#endif

extern double PI;
extern double PIO2;
extern double MACHEP;

static double aa[ARRSIZ] = {0.0};
static double pp[ARRSIZ] = {0.0};
static double y[ARRSIZ] = {0.0};
static double zs[ARRSIZ] = {0.0};
static double z[2][ARRSIZ] = {0.0};
static double wr = 0.0;
static double wc = 0.0;
static double rn = 8.0;
static double c = 0.0;
static double cgam = 0.0;
static double scale = 0.0;
static double fs = 1.0e4;
static double dbr = 0.5;
static double dbd = -40.0;
static double f1 = 1.5;
static double f2 = 2.0e3;
static double f3 = 2.4e3;
static double dbfac = 0.0;
static double a = 0.0;
static double b = 0.0;
static double p = 0.0;
static double q = 0.0;
static double r = 0.0;
static double ri = 0.0;

static double t1 = 0.0;
static double t2 = 0.0;
static double u = 0.0;
static double k = 0.0;
static double kp = 0.0;
static double m = 0.0;
static double Kk = 0.0;
static double Kk1 = 0.0;
static double Kpk = 0.0;
static double Kpk1 = 0.0;
static double eps = 0.0;
static double rho = 0.0;
static double phi = 0.0;
static double sn = 0.0;
static double cn = 0.0;
static double dn = 0.0;
static double sn1 = 0.0;
static double cn1 = 0.0;
static double dn1 = 0.0;
static double phi1 = 0.0;
static double m1 = 0.0;
static double m1p = 0.0;
static double cang = 0.0;
static double sang = 0.0;
static double bw = 0.0;
static double ang = 0.0;
static double fnyq = 0.0;
static double rr = 0.0;
static double ar = 0.0;
static double ai = 0.0;
static double br = 0.0;
static double bi = 0.0;
static double qr = 0.0;
static double qm = 0.0;
static double qa = 0.0;
static double sqr = 0.0;
static double sqi = 0.0;
static double qi = 0.0;
static double pn = 0.0;
static double an = 0.0;
static double gam = 0.0;
static double cng = 0.0;
static int lr = 0;
static int nt = 0;
static int i = 0;
static int j = 0;
static int jt = 0;
static int nc = 0;
static int ii = 0;
static int ir = 0;
static int ni = 0;
static int nr = 0;

static int zord = 0;
static int icnt = 0;
static int mh = 0;
static int jj = 0;
static int jh = 0;
static int jl = 0;
static int n = 8;
static int np = 0;
static int nz = 0;
static int type = 1;
static char salut[] =

{"Filter type:\n1 low pass\n2 band pass\n3 high pass\n4 band stop\n"};

double abs(), exp(), log(), cos(), sin(), sqrt();
double ellpk(), ellik(), asin(), atan();
double cay();


main()
{
char str[80];

dbfac = 10.0/log(10.0);

top:

printf( "%s ? ", salut );	/* ask for filter type */
gtlin( str );			/* gtlin() is like gets() */
sscanf( str, "%d", &type );
printf( "%d\n", type );
if( (type <= 0) || (type > 4) )
	exit(0);

getnum( "Order of filter", &rn ); /* see below for getnum() */
n = rn;
if( n <= 0 )
	{
specerr:
	printf( "? Specification error\n" );
	goto top;
	}
rn = n;	/* ensure it is an integer */
getnum( "Passband ripple, db", &dbr );
if( dbr <= 0.0 )
	goto specerr;
eps = exp( dbr/dbfac );
scale = 1.0;
if( (n & 1) == 0 )
	scale = sqrt( eps );
eps = sqrt( eps - 1.0 );

getnum( "Sampling frequency", &fs );
if( fs <= 0.0 )
	goto specerr;

fnyq = fs/2.0;

getnum( "Passband edge", &f2 );
if( (f2 <= 0.0) || (f2 >= fnyq) )
	goto specerr;

if( (type & 1) == 0 )
	{
	getnum( "Other passband edge", &f1 );
	if( (f1 <= 0.0) || (f1 >= fnyq) )
		goto specerr;
	}
else
	f1 = 0.0;

if( f2 < f1 )
	{
	a = f2;
	f2 = f1;
	f1 = a;
	}
if( type == 3 )	/* high pass */
	{
	bw = f2;
	a = fnyq;
	}
else
	{
	bw = f2 - f1;
	a = f2;
	}
ang = bw * PI / fs;
cang = cos( ang );
c = sin(ang) / cang;
cgam = cos( (a+f1) * PI / fs ) / cang;

getnum( "Stop band edge or -(db down)", &dbd );
if( dbd > 0.0 )
	f3 = dbd;
else
	{ /* calculate band edge from db down */
	a = exp( -dbd/dbfac );
	m1 = eps/sqrt( a - 1.0 );
	m1 *= m1;
	m1p = 1.0 - m1;
	Kk1 = ellpk( m1p );
	Kpk1 = ellpk( m1 );
	q = exp( -PI * Kpk1 / (rn * Kk1) );
	k = cay(q);
	if( type >= 3 )
		wr = k;
	else
		wr = 1.0/k;
	if( type & 1 )
		{
		f3 = atan( c * wr ) * fs / PI;
		}
	else
		{
		a = c * wr;
		a *= a;
		b = a * (1.0 - cgam * cgam) + a * a;
		b = (cgam + sqrt(b))/(1.0 + a);
		f3 = (PI/2.0 - asin(b)) * fs / (2.0*PI);
		}
	}

switch( type )
	{
    case 1:
	if( f3 <= f2 )
		goto specerr;
	break;
    case 2:
	if( (f3 > f2) || (f3 < f1) )
		break;
	goto specerr;
    case 3:
	if( f3 >= f2 )
		goto specerr;
	break;
    case 4:
	if( (f3 <= f1) || (f3 >= f2) )
		goto specerr;
	break;
	}

ang = f3 * PI / fs;
cang = cos(ang);
sang = sin(ang);

if( type & 1 )
	{
	wr = sang/(cang*c);
	}
else
	{
	q = cang * cang  -  sang * sang;
	sang = 2.0 * cang * sang;
	cang = q;
	wr = (cgam - cang)/(sang * c);
	}

if( type >= 3 )
	wr = 1.0/wr;
if( wr < 0.0 )
	wr = -wr;
y[0] = 1.0;
y[1] = wr;
if( type >= 3 )
	y[1] = 1.0/y[1];

if( type & 1 )
	{
	for( i=1; i<=2; i++ )
		{
		aa[i] = atan( c * y[i-1] ) * fs / PI ;
		}
	printf( "pass band %.9E\n", aa[1] );
	printf( "stop band %.9E\n", aa[2] );
	}
else
	{
	for( i=1; i<=2; i++ )
		{
		a = c * y[i-1];
		b = atan(a);
		q = sqrt( 1.0 + a * a  -  cgam * cgam );
		q = artan2( cgam, q );
		aa[i] = (q + b) * fnyq / PI;
		pp[i] = (q - b) * fnyq / PI;
		}
	printf( "pass band %.9E %.9E\n", pp[1], aa[1] );
	printf( "stop band %.9E %.9E\n", pp[2], aa[2] );
	}

lampln();	/* find locations in lambda plane */
if( (2*n+2) > ARRSIZ )
	goto toosml;

spln();		/* find s plane poles and zeros */

if( ((type & 1) == 0) && ((4*n+2) > ARRSIZ) )
	goto toosml;

zplna();	/* convert s plane to z plane */
zplnb();
zplnc();
goto top;

toosml:
printf( "Cannot continue, storage arrays too small\n" );
goto top;
}


lampln()
{

wc = 1.0;
k = wc/wr;

m = k * k;
Kk = ellpk( 1.0 - m );
Kpk = ellpk( m );

q = exp( -PI * rn * Kpk / Kk );	/* the nome of k1 */
m1 = cay(q); /* see below */

/* Note m1 = eps / sqrt( A*A - 1.0 ) */
a = eps/m1;
a =  a * a + 1;
a = 10.0 * log(a) / log(10.0);
printf( "dbdown %.9E\n", a );

a = 180.0 * asin( k ) / PI;
b = 1.0/(1.0 + eps*eps);
b = sqrt( 1.0 - b );
printf( "theta %.9E, rho %.9E\n", a, b );
m1 *= m1;
m1p = 1.0 - m1;
Kk1 = ellpk( m1p );
Kpk1 = ellpk( m1 );

r = Kpk1 * Kk / (Kk1 * Kpk);
printf( "consistency check: n= %.14E\n", r );

/*   -1
 * sn   j/eps\m  =  j ellik( atan(1/eps), m )
 */
b = 1.0/eps;
phi = atan( b );
u = ellik( phi, m1p );
printf( "phi %.7e m %.7e u %.7e\n", phi, m1p, u );

/* consistency check on inverse sn */

ellpj( u, m1p, &sn, &cn, &dn, &phi );
a = sn/cn;
printf( "consistency check: sn/cn = %.9E = %.9E = 1/eps\n", a, b );


u = u * Kk / (rn * Kk1);	/* or, u = u * Kpk / Kpk1 */
}


/* calculate s plane poles and zeros, normalized to wc = 1 */
spln()
{
np = (n+1)/2;
nz = n/2;
ellpj( u, 1.0-m, &sn1, &cn1, &dn1, &phi1 );
for( i=0; i<ARRSIZ; i++ )
	zs[i] = 0.0;
for( i=0; i<nz; i++ )
	{	/* zeros */
	a = n - 1 - i - i;
	b = (Kk * a) / rn;
	ellpj( b, m, &sn, &cn, &dn, &phi );
	lr = 2*np + 2*i;
	zs[ lr ] = 0.0;
	a = wc/(k*sn);	/* k = sqrt(m) */
	zs[ lr + 1 ] = a;
	}
for( i=0; i<np; i++ )
	{	/* poles */
	a = n - 1 - i - i;
	b = a * Kk / rn;		
	ellpj( b, m, &sn, &cn, &dn, &phi );
	r = k * sn * sn1;
	b = cn1*cn1 + r*r;
	a = -wc*cn*dn*sn1*cn1/b;
	lr = i + i;
	zs[lr] = a;
	b = wc*sn*dn1/b;
	zs[lr+1] = b;
	}	
if( type >= 3 )
{
nt = np + nz;
for( j=0; j<nt; j++ )
	{
	ir = j + j;
	ii = ir + 1;
	b = zs[ir]*zs[ir] + zs[ii]*zs[ii];
	zs[ir] = zs[ir] / b;
	zs[ii] = zs[ii] / b;
	}
while( np > nz )
	{
	ir = ii + 1;
	ii = ir + 1;
	nz += 1;
	zs[ir] = 0.0;
	zs[ii] = 0.0;
	}
}

printf( "s plane poles:\n" );
j = 0;
for( i=0; i<np+nz; i++ )
	{
	a = zs[j];
	++j;
	b = zs[j];
	++j;
	printf( "%.9E %.9E\n", a, b );
	if( i == np-1 )
		printf( "s plane zeros:\n" );
	}
}

getnum( line, val )
char *line;
double *val;
{
char s[40];

printf( "%s = %.9E ? ", line, *val );
gtlin( s );
if( s[0] != '\0' )
	{
	sscanf( s, "%lf", val );
	printf( "%.9E\n", *val );
	}
}

/*		cay()
 *
 * Find parameter corresponding to given nome by expansion
 * in theta functions:
 * AMS55 #16.38.5, 16.38.7
 *
 *       1/2
 * ( 2K )                   4     9
 * ( -- )     =  1 + 2q + 2q  + 2q  + ...  =  Theta (0,q)
 * ( pi )                                          3
 *
 *
 *       1/2
 * ( 2K )     1/4       1/4        2    6    12    20
 * ( -- )    m     =  2q    ( 1 + q  + q  + q   + q   + ...) = Theta (0,q)
 * ( pi )                                                           2
 *
 * The nome q(m) = exp( - pi K(1-m)/K(m) ).
 *
 *                                1/2
 * Given q, this program returns m   .
 */

double cay(q)
double q;
{
double a, b, p, r;
double t1, t2;
double abs(), sqrt();

a = 1.0;
b = 1.0;
r = 1.0;
p = q;

do
{
r *= p;
a += 2.0 * r;
t1 = abs( r/a );

r *= p;
b += r;
p *= q;
t2 = abs( r/b );
if( t2 > t1 )
	t1 = t2;
}
while( t1 > MACHEP );

a = b/a;
a = 4.0 * sqrt(q) * a * a;	/* see above formulas, solved for m */
return(a);
}

/*		zpln.c
 * Program to convert s plane poles and zeros to the z plane.
 */


zplna()
{

for( i=0; i<ARRSIZ; i++ )
	{
	z[0][i] = 0.0;
	z[1][i] = 0.0;
	}


nc = np;
jt = -1;
ii = -1;

for( icnt=0; icnt<2; icnt++ )
{
	/* The maps from s plane to z plane */
do
{
ir = ii + 1;
ii = ir + 1;
rr = zs[ir];
ri = zs[ii];
b = 1.0 - rr * c;
b *= b;
b += ri * ri * c * c;

switch( type )
{
case 1:
case 3:

jt += 1;
z[0][jt] = (1.0 - (rr * rr + ri * ri)*c*c)/b;
z[1][jt] = -2.0 * ri * c / b;
if( ri == 0.0 )
	break;

jt += 1;
z[0][jt] = z[0][jt-1 ];
z[1][jt] = -z[1][jt-1 ];
break;

case 2:
case 4:

rr = c * rr;
ri = c * ri;
b = 1.0 - rr;
b *= b;
b += ri * ri;
ar = (1.0 - rr) * cgam / b;
ai = cgam * ri / b;
br = (1.0 - rr * rr - ri * ri) / b;
bi = 2.0 * ri / b;
qr = ar * ar - ai * ai - br;
qi = 2.0 * ar * ai - bi;
qm = sqrt( qr * qr + qi * qi );
qm = sqrt( qm );
qa = artan2( qr, qi ) / 2.0;
sqr = qm * cos( qa );
sqi = qm * sin( qa );
jt += 1;
z[0][jt] = ar + sqr;
z[1][jt] = ai + sqi;
jt += 1;
z[0][jt] = ar - sqr;
z[1][jt] = ai - sqi;

if( ri > 0.0 )
	{
	jt += 2;
	for( i=0; i<=1; i++ )
		{
		jj = jt - 1 + i;
		z[0][jj] = z[0][jj-2];
		z[1][jj] = -z[1][jj-2];
		}
	}
} /* end switch */

}
while( --nc > 0 );

if( icnt == 0 )
	{
	zord = jt+1;
	if( nz <= 0 )
		break;
	}
nc = nz;
}	/* end for() loop */

}

zplnb()
{

while( 2*zord - 1 > jt )
	{
	jt += 1;
	z[0][jt] = -1.0;
	z[1][jt] = 0.0;
	if( (type == 2) || (type == 4) )
		{
		jt += 1;
		z[0][jt] = 1.0;
		z[1][jt] = 0.0;
		}
	}
printf( "order = %d\n", zord );

/* Expand the poles and zeros into numerator and
 * denominator polynomials
 */
for( icnt=0; icnt<2; icnt++ )
{
for( j=0; j<ARRSIZ; j++ )
	{
	pp[j] = 0.0;
	y[j] = 0.0;
	}

pp[0] = 1.0;

for( j=0; j<zord; j++ )
	{
	jj = j;
	if( icnt )
		jj += zord;
	a = z[0][jj];
	b = z[1][jj];
	for( i=0; i<=j; i++ )
		{
		jh = j - i;
		pp[jh+1] = pp[jh+1] - a * pp[jh] + b * y[jh];
		y[jh+1] =  y[jh+1]  - b * pp[jh] - a * y[jh];
		}
	}

if( icnt == 0 )
	{
	for( j=0; j<=zord; j++ )
		aa[j] = pp[j];
	}
}

/* Scale factors of the pole and zero polynomials */

a = 1.0;

switch( type )
{

case 3:

a = -1.0;

case 1:
case 4:

pn = 1.0;
an = 1.0;
for( j=1; j<=zord; j++ )
	{
	pn = a * pn + pp[j];
	an = a * an + aa[j];
	}
break;

case 2:

gam = PI/2.0 - asin( cgam );
mh = zord/2;
pn = pp[mh];
an = aa[mh];
ai = 0.0;

if( mh > ((zord/4)*2) )
	{
	ai = 1.0;
	pn = 0.0;
	an = 0.0;
	}

for( j=1; j<=mh; j++ )
	{
	a = gam * j - ai * PI / 2.0;
	cng = cos(a);
	jh = mh + j;
	jl = mh - j;
	pn = pn + cng * (pp[jh] + (1.0 - 2.0 * ai) * pp[jl]);
	an = an + cng * (aa[jh] + (1.0 - 2.0 * ai) * aa[jl]);
	}
}
}

zplnc()
{

q = an/(pn*scale);
printf( "constant gain factor %23.13E\n", q );
for( j=0; j<=zord; j++ )
	pp[j] = q * pp[j];

printf( "z plane Denominator      Numerator\n" );
for( j=0; j<=zord; j++ )
	{
	printf( "%2d %17.9E %17.9E\n", j, aa[j], pp[j] );
	}
printf( "poles and zeros with corresponding quadratic factors\n" );
for( j=0; j<zord; j++ )
	{
	a = z[0][j];
	b = z[1][j];
	if( b >= 0.0 )
		{
		printf( "pole  %23.13E %23.13E\n", a, b );
		quadf( a, b, 1 );
		}
	jj = j + zord;
	a = z[0][jj];
	b = z[1][jj];
	if( b >= 0.0 )
		{
		printf( "zero  %23.13E %23.13E\n", a, b );
		quadf( a, b, 0 );
		}
	}
}

/* display quadratic factors */

quadf( x, y, pzflg )
double x, y;
int pzflg;	/* 1 if poles, 0 if zeros */
{
double a, b, r, f, g, g0;
double asin(), sqrt();

if( y > 1.0e-16 )
	{
	a = -2.0 * x;
	b = x*x + y*y;
	}
else
	{
	a = -x;
	b = 0.0;
	}
printf( "q. f.\nz**2 %23.13E\nz**1 %23.13E\n", b, a );
if( b != 0.0 )
	{
	/* resonant frequency */
	r = sqrt(b);
	f = PI/2.0 - asin( -a/(2.0*r) );
	f = f * fs / (2.0 * PI );
	/* gain at resonance */
	g = 1.0 + r;
	g = g*g - (a*a/r);
	g = (1.0 - r) * sqrt(g);
	g0 = 1.0 + a + b;	/* gain at d.c. */
	}
else
	{
	/* It is really a first-order network.
	 * Give the gain at fnyq and D.C.
	 */

	f = fnyq;
	g = 1.0 - a;
	g0 = 1.0 + a;
	}

if( pzflg )
	{
	g = 1.0/g;
	g0 = 1.0/g0;
	}
printf( "f0 %16.8E  gain %12.4E  DC gain %12.4E\n\n", f, g, g0 );
}

/* double precision absolute value */

double abs(x)
double x;
{
if( x < 0.0 )
	return( -x );
else
	return( x );
}
#if !CEPHES
/*							polevl.c
 *							p1evl.c
 *
 *	Evaluate polynomial
 *
 *
 *
 * SYNOPSIS:
 *
 * int N;
 * double x, y, coef[N+1], polevl[];
 *
 * y = polevl( C, x, N );
 *
 *
 *
 * DESCRIPTION:
 *
 * Evaluates polynomial of degree N:
 *
 *                     2          N
 * y  =  C  + C x + C x  +...+ C x
 *        0    1     2          N
 *
 * Coefficients are stored in reverse order:
 *
 * coef[0] = C  , ..., coef[N] = C  .
 *            N                   0
 *
 *  The function p1evl() assumes that coef[N] = 1.0 and is
 * omitted from the array.  Its calling arguments are
 * otherwise the same as polevl().
 *
 *
 * SPEED:
 *
 * In the interest of speed, there are no checks for out
 * of bounds arithmetic.  This routine is used by most of
 * the functions in the library.  Depending on available
 * equipment features, the user may wish to rewrite the
 * program in microcode or assembly language.
 *
 */
/*							polevl.c */


double polevl( x, coef, N )
double x;
double coef[];
int N;
{
double ans;
short i;
register double *p;

p = coef;
ans = 0.0;
i = N+1;

do
	ans = ans * x  +  *p++;
while( --i );

return( ans );
}

/*							p1evl()	*/
/*                                          N
 * Evaluate polynomial when coefficient of x  is 1.0.
 * Otherwise same as polevl.
 */

double p1evl( x, coef, N )
double x;
double coef[];
int N;
{
double ans;
register double *p;
short i;

p = coef;
ans = x + *p++;
i = N-1;

do
	ans = ans * x  + *p++;
while( --i );

return( ans );
}
#endif	/*!CEPHES*/