 /*
  * Khoros: $Id: ldfilthp.c,v 1.3 1992/03/20 23:26:10 dkhoros Exp $
  */

#if !defined(lint) && !defined(SABER)
static char rcsid[] = "Khoros: $Id: ldfilthp.c,v 1.3 1992/03/20 23:26:10 dkhoros Exp $";
#endif

 /*
  * $Log: ldfilthp.c,v $
 * Revision 1.3  1992/03/20  23:26:10  dkhoros
 * VirtualPatch5
 *
  */

/*
 *----------------------------------------------------------------------
 *
 * Copyright 1992, University of New Mexico.  All rights reserved.
 * Permission to copy and modify this software and its documen-
 * tation only for internal use in your organization is hereby
 * granted, provided that this notice is retained thereon and
 * on all copies.  UNM makes no representations as to the sui-
 * tability and operability of this software for any purpose.
 * It is provided "as is" without express or implied warranty.
 * 
 * UNM DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
 * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FIT-
 * NESS.  IN NO EVENT SHALL UNM BE LIABLE FOR ANY SPECIAL,
 * INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY OTHER DAMAGES WHAT-
 * SOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
 * ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PER-
 * FORMANCE OF THIS SOFTWARE.
 * 
 * No other rights, including, for example, the right to redis-
 * tribute this software and its documentation or the right to
 * prepare derivative works, are granted unless specifically
 * provided in a separate license agreement.
 *---------------------------------------------------------------------
 */

#include "unmcopyright.h"        /* Copyright 1992 by UNM */

/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 >>>>
 >>>>         File Name: ldfilthp.c
 >>>>
 >>>>      Program Name: dfilthp
 >>>>
 >>>> Date Last Updated: Mon Mar  9 20:18:11 1992 
 >>>>
 >>>>          Routines: ldfilthp - the library call for dfilthp
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
/* second order filters: six coefficients */
#define NCOEF 6
#define ARCSINH(x) (log((double)x + sqrt((double)x * (double)x + 1.0)))
#define ARCCOSH(x) (log((double)x + sqrt((double)x * (double)x - 1.0)))
#define MAXSTAGES 100
#include "vpoly.h"
static int btwhigh(),chbihigh(),chbiihigh();
/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldfilthp - library call for dfilthp
*
* Purpose:
*    
*    1D highpass frequency domain filter design
*    
*    

* Input:
*    
*    f1             digital rejection frequency in hertz
*    
*    fr             digital cutoff frequency (needed for chebychev ii)
*                   in hertz
*    
*    tolc           magnitude gain at the passband edge.   for  Cheby-
*                   chev  I,  it  also  specifies the magnitude of the
*                   ripple.
*    
*    tolr           magnitude gain at the stopband edge.   for  Cheby-
*                   chev  II,  it  also specifies the magnitude of the
*                   ripple.
*    
*    sfreq          sampling frequency of the system.
*    
*    class          filter class:  0 specifies a Butterworth, 1 speci-
*                   fies  a  Chebychev  I, and 2 specifies a Chebychev
*                   II.
*    
*    

* Output:
*    
*    poly           pointer to a poly_struct that contains a  descrip-
*                   tion of the polynomial
*    
*    Return Value:  1 on success, 0 on failure.
*    
*    

*
* Written By: Jeremy Worley, Vena Margo
*    
*    Jeremy Worley 09 Mar 1992 19:54 MST
*              Added checks of the return values of some internal rou-
*              tines.
*    
*    

****************************************************************/


/* -library_def */
int ldfilthp(poly,f1,f2,tolc,tolr,sfreq,class)
    int class;
    float f1,f2,sfreq,tolc,tolr;
    struct poly_struct *poly;
/* -library_def_end */

/* -library_code */
{
    char *program = "ldfilthp";
    float *a,*b,*c,*d,*e;
    int i,offset=0,stages;
    float wc,wr,w1,w2,epsilon,lambda;

/*
** check for user stupidity
*/

    if(poly==NULL){
       fprintf(stderr,"%s:  polynomial structure must not be NULL\n",program);
       return(0);
    }

    if(f1>=f2){
       fprintf(stderr,"%s:  f1 must be less than f2.\n",program);
       return(0);
    }

    if(f1>0.5*sfreq || f2>0.5*sfreq){
       fprintf(stderr,"%s:  critical frequencies for the filter ",program);
       fprintf(stderr,"must be less than one half of the sampling \n");
       fprintf(stderr,"frequency.\n");
       return(0);
    }

    if(tolr>=1.0 || tolr<=0.0){
       fprintf(stderr,"%s:  rejection tolerance is out of range.\n",program);
       fprintf(stderr,"legal range is between 0.0 and 1.0\n");
       return(0);
    }

    if(tolc>=1.0 || tolc<=0.0){
       fprintf(stderr,"%s:  cutoff tolerance is out of range.\n",program);
       fprintf(stderr,"legal range is between 0.0 and 1.0\n");
       return(0);
    }

    if(tolr>=tolc){
       fprintf(stderr,"%s:  for a highpass filter, the rejection tolerance.\n",
               program);
       fprintf(stderr,"must be less than the cutoff tolerance.\n");
       return(0);
    }

/*
** allocate memory for junk passed into low level filter routines 
*/

    a = (float *)malloc(MAXSTAGES*sizeof(float));
    if(a==NULL){
       fprintf(stderr,"%s:  [1] memory allocation failed.\n",program);
       return(0);
    }

    b = (float *)malloc(MAXSTAGES*sizeof(float));
    if(b==NULL){
       fprintf(stderr,"%s:  [2] memory allocation failed.\n",program);
       return(0);
    }

    c = (float *)malloc(MAXSTAGES*sizeof(float));
    if(c==NULL){
       fprintf(stderr,"%s:  [3] memory allocation failed.\n",program);
       return(0);
    }

    d = (float *)malloc(MAXSTAGES*sizeof(float));
    if(d==NULL){
       fprintf(stderr,"%s:  [4] memory allocation failed.\n",program);
       return(0);
    }

    e = (float *)malloc(MAXSTAGES*sizeof(float));
    if(e==NULL){
       fprintf(stderr,"%s:  [5] memory allocation failed.\n",program);
       return(0);
    }

/*
** define wc and wr
*/

    epsilon = sqrt((1.0 - tolc*tolc)/(tolc*tolc));
    lambda = sqrt((1.0 - tolr*tolr)/(tolr*tolr));

    w1 = tan((double)XV_PI*f1/sfreq);
    w2 = tan((double)XV_PI*f2/sfreq);

    wc = w2;
    wr = w2*w2/w1;

/*
** design the filter
*/

    switch(class){
       case 0 :  if(!btwhigh(wc,wr,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to btwhigh.\n",program);
                 }
                 break;
       case 1 :  if(!chbihigh(wc,wr,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to chbihigh.\n",program);
                 }
                 break;
       case 2 :  if(!chbiihigh(wc,wr,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to chbiihigh.\n",program);
                 }
                 break;
    }

/*
** now rearrange data into a format that writepoly() can understand
**
** the following code segment assumes that the polynomial for a stage is:
**
**    H(z)  = (a*z^2 + b*z + c)/(z^2 + d*z +e)
**
*/


    for(i=0;i<stages;i++){
        poly->terms[offset].coef = a[i]; poly->terms[offset].expon = 2.0;
        poly->terms[offset+1].coef = b[i]; poly->terms[offset+1].expon = 1.0;
        poly->terms[offset+2].coef = c[i]; poly->terms[offset+2].expon = 0.0;
        poly->terms[offset+3].coef = 1.0;  poly->terms[offset+3].expon = 2.0;
        poly->terms[offset+4].coef = d[i]; poly->terms[offset+4].expon = 1.0;
        poly->terms[offset+5].coef = e[i]; poly->terms[offset+5].expon = 0.0;
        poly->nterms[2*i] = 3; poly->nterms[2*i+1] = 3;
        offset += 6;
    }
    for(i=0;i<stages*6;i++){
        poly->terms[i].delay = 0;
        poly->terms[i].type = _STDTERM;
        if((poly->terms[i].varname = (char *)malloc(2*sizeof(char)))==NULL){
           fprintf(stderr,"%s:  malloc error after filter design\n",program);
           return(0);
        }
        strcpy(poly->terms[i].varname,"z");
    }
    strcpy(poly->indep_var,"z");
    strcpy(poly->func_name,"H");
    poly->stages = stages;
    return(1);
}

/***********************************************************************
*
*  Routine Name: btwhigh() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a high pass butterworth filter
*                made up of 'stages' stages of 2nd order filters. 
*                The function used for one stage is:  
*
*                       H(z) = (a*z^2 + b*z + c)/(z^2 + d*z + e)
*
*         Input: stages  - number of second order stages
*                wc      - analog cutoff frequency
*
*        Output: a - f   - coefficients as explained above
*
*    Written By: Jeremy Worley, Vena Margo
* 
* Modifications:
*
***********************************************************************/

static int btwhigh(wc,wr,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float wc,wr,epsilon,lambda,*a,*b,*c,*d,*e;
{
    int n,order,stages;
    float r,rs,ws,cb,gain,beta,wc4;

    order = log(lambda/epsilon)/log(wr/wc) + 1;
    if((float)(order/2)!=(float)order/2.0)order+=1;

    stages = order/2;
    *nstages = stages;

    r     = wc*pow((double)epsilon,(double)(-1.0/order));
    rs    = r*r;
    ws    = wc*wc;
    wc4   = pow((double)wc,(double)4.0);

    for(n=0;n<stages;n++){
        beta = ((float)(2.0*(n+1) - 1.0 + order)/(float)(2.0*order))*XV_PI;
        cb = cos(beta);
        gain = 1.0/(rs - 2.0*ws*r*cb + wc4);

        a[n] = rs*gain;
        b[n] = -2.0*a[n];
        c[n] = a[n];
        d[n] = 2.0*(wc4-rs)*gain;
        e[n] = (rs + 2.0*ws*r*cb + wc4)*gain;
    }

    return(1);
}


/***********************************************************************
*
*  Routine Name: chbihigh() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a high pass chebychev I filter
*                made up of 'stages' stages of 2nd order filters. 
*                The function used for one stage is:  
*
*                       H(z) = (a*z^2 + b*z + c)/(z^2 + d*z + e)
*
*         Input: stages  - number of second order stages
*                wc      - analog cutoff frequency
*
*        Output: a - f   - coefficients as explained above
*
*    Written By: Jeremy Worley, Vena Margo
* 
* Modifications:
*
***********************************************************************/

static int chbihigh(wc,wr,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float wc,wr,epsilon,lambda,*a,*b,*c,*d,*e;
{
    int n,order,stages;
    float gain,alpha,bn,xn,ynn,rn,qn,wc4,temp;

    order = (int)(ARCCOSH(lambda/epsilon)/ARCCOSH(wr/wc)) + 1;
    if((float)(order/2)!=(float)order/2.0)order+=1;

    stages = order/2;
    *nstages = stages;

    alpha = ARCSINH(1.0/epsilon)/stages;
    wc4 = pow((double)wc,(double)4.0);

    for(n=0;n<stages;n++){
        bn = XV_PI*(((float)(2.0*(n+1)-1.0+stages))/(2.0*(float)stages));
        xn = sinh(alpha)*cos(bn);
        ynn = cosh(alpha)*sin(bn);
        rn = wc*wc*(xn*xn + ynn*ynn);
        qn = 2.0*wc*xn;

        gain = 1.0/(rn - qn*wc*wc + wc4);

        a[n] = rn*gain;
        b[n] = -2.0*a[n];
        c[n] = a[n];
        d[n] = 2.0*(wc4 - rn)*gain;
        e[n] = (rn + qn*wc*wc + wc4)*gain;

        if(n==0){
           temp = 1 + epsilon*epsilon;
           a[n] /= temp;
           b[n] /= temp;
           c[n] /= temp;
        }
    }
    
    return(1);
}


/***********************************************************************
*
*  Routine Name: chbiihigh() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a high pass chebychev II filter
*                made up of 'stages' stages of 2nd order filters. 
*                The function used for one stage is:  
*
*                       H(z) = (a*z^2 + b*z + c)/(z^2 + d*z + e)
*
*         Input: stages  - number of second order stages
*                wc      - analog cutoff frequency
*                wr      - analog rejection frequency
*
*        Output: a - f   - coefficients as explained above
*
*    Written By: Jeremy Worley, Vena Margo
* 
* Modifications:
*
***********************************************************************/

static int chbiihigh(wc,wr,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float wc,wr,epsilon,lambda,*a,*b,*c,*d,*e;
{
    int n,order,stages;
    float gain,alpha,epslnhat,bn,xn,ynn,rn,vn,sbn,wrs, wc4;

    order = (int)(ARCCOSH(lambda/epsilon)/ARCCOSH(wr/wc)) + 1;
    if((float)(order/2)!=(float)order/2.0)order+=1;

    stages = order/2;
    *nstages = stages;

    vn = cosh((float)stages*ARCCOSH(wr/wc));
    epslnhat = 1.0/(epsilon*vn);
    alpha = ARCSINH(1.0/epslnhat)/stages;
    wc4 = pow((double)wc,(double)4.0);

    for(n=0; n<stages; n++){
        bn = XV_PI*(((float)(2.0*(n+1)-1.0+stages))/(2.0*(float)stages));
        xn = sinh(alpha)*cos(bn);
        ynn = cosh(alpha)*sin(bn);
        rn = xn*xn + ynn*ynn;

        sbn = sin(bn)*sin(bn);
        wrs = wr*wr;

        gain = 1.0/(wc4*rn - 2.0*wr*xn*wc*wc + wr*wr);

        a[n] = (wc4*sbn + wrs)*gain;
        b[n] = 2.0*(wc4*sbn - wrs)*gain;
        c[n] = a[n];
        d[n] = 2.0*(wc4*rn - wrs)*gain;
        e[n] = (wc4*rn + 2.0*wr*xn*wc*wc + wrs)*gain;
    }

    return(1);
}
/* -library_code_end */
