 /*
  * Khoros: $Id: ldfiltbs.c,v 1.3 1992/03/20 23:25:56 dkhoros Exp $
  */

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

 /*
  * $Log: ldfiltbs.c,v $
 * Revision 1.3  1992/03/20  23:25:56  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: ldfiltbs.c
 >>>>
 >>>>      Program Name: dfiltbs
 >>>>
 >>>> Date Last Updated: Mon Mar  9 20:17:21 1992 
 >>>>
 >>>>          Routines: ldfiltbs - the library call for dfiltbs
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#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 btwstop(),chbistop(),chbiistop();
static int csqrt();

/* cleanup macros */
#define BTW_CLEANUP { \
      if(sreal!=NULL)free((char *)sreal); \
      if(simag!=NULL)free((char *)simag); \
      if(pr!=NULL)free((char *)pr); \
      if(pi!=NULL)free((char *)pi); \
      if(as!=NULL)free((char *)as); \
      if(bs!=NULL)free((char *)bs); \
      if(cs!=NULL)free((char *)cs); \
      if(ds!=NULL)free((char *)ds); \
      if(es!=NULL)free((char *)es); \
    }

/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldfiltbs - library call for dfiltbs
*
* Purpose:
*    
*    designs bandstop filters
*    
*    

* Input:
*    
*    f1             lower digital rejection frequency in hertz.
*    
*    f2             lower digital cutoff frequency in hertz.
*    
*    f3             upper digital cutoff frequency in hertz.
*    
*    f4             upper digital rejection frequency in hertz.
*    
*    tolc           cutoff  tolerance.   specifies  the  gain  at  the
*                   passband  edge.   Also determines the magnitude of
*                   the ripple for Chebychev I filters.
*    
*    tolr           rejection tolerance.  specifies the  gain  at  the
*                   stopband  edge.   Also determines the magnitude of
*                   the ripple for Chebychev II filters.
*    
*    sfreq          sampling frequency of the system.
*    
*    class          specifies the filter class  to  be  used.   0  for
*                   Butterworth,  1  for Chebychev I, and 2 for Cheby-
*                   chev II.
*    
*    

* Output:
*    
*    poly           polynomial structure that will contain the  filter
*                   transfer function at exit.
*    
*    Return Value:  1 on success, 0 on failure.
*    
*    

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

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


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

/* -library_code */
{
    char *program = "ldfiltbs";
    float *a,*b,*c,*d,*e,wc,wr,w1,w2,w3,w4,epsilon,lambda;
    int i,offset=0,stages;
/*
** check for user stupidity
*/

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

    if(f1>=f2 || f2>=f3 || f3>=f4){
       fprintf(stderr,"%s:  critical frequencies must progress \n",program);
       fprintf(stderr,"smallest to largest as follows: f1 f2 f f4.\n");
       return(0);
    }

    if(f1>0.5*sfreq || f2>0.5*sfreq || f3>0.5*sfreq || f4>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 bandstop 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);
    }

/*
** preliminary calculations of wc and wr
*/

    w1 = tan((double)XV_PI*f1/sfreq);
    w2 = tan((double)XV_PI*f2/sfreq);
    w3 = tan((double)XV_PI*f3/sfreq);
    w4 = tan((double)XV_PI*f4/sfreq);

    wc = w4 -w1;
    if(w3<=sqrt(w1*w4))
       wr = wc*wc*w2/(w1*w4 - w2*w2);
    else if(w2<sqrt(w1*w4) && w3>sqrt(w1*w4))
       wr = MIN((wc*wc*w2/(w1*w4 - w2*w2)) , (wc*wc*w3/(w3*w3 - w1*w4)));
    else
       wr = wc*wc*w3/(w3*w3 - w1*w4);

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

/*
** design the filter
*/

    switch(class){
       case 0 :  if(!btwstop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to btwstop().\n",program);
                 }
                 break;
       case 1 :  if(!chbistop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to chbistop().\n",program);
                 }
                 break;
       case 2 :  if(!chbiistop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,&stages)){
                    fprintf(stderr,"%s: failed call to chbiistop().\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 = 0.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 = -2.0;
        poly->terms[offset+3].coef = 1.0;  poly->terms[offset+3].expon = 0.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 = -2.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: btwstop() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a band stop 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
* 
* Modifications:
*
***********************************************************************/

static int btwstop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float epsilon,lambda,wc,wr,w1,w4,*a,*b,*c,*d,*e;
{
    float *sreal, *simag, poweri, scalar,
          ctempr, ctempi, *pr, *pi, *as, *bs, *cs,
          *ds, *es, denom, br, bi, bsqrdr, bsqrdi, fouracr;
    int i,stages,order,lowstages;

/*
** calculate order and number of stages
*/

    order = 2*ceil(log(lambda/epsilon)/log(wr/wc));
    order += order%4;
    stages = order/2;
    *nstages = stages;

/*
** allocate temporary memory
*/

   if(((sreal = (float *)malloc(sizeof(float)*2*stages))==NULL) ||
      ((simag = (float *)malloc(sizeof(float)*2*stages))==NULL) ||
      ((pr = (float *)malloc(sizeof(float)*2*stages))==NULL)    ||
      ((pi = (float *)malloc(sizeof(float)*2*stages))==NULL)    ||
      ((as = (float *)malloc(sizeof(float)*stages))==NULL)      ||
      ((bs = (float *)malloc(sizeof(float)*stages))==NULL)      ||
      ((cs = (float *)malloc(sizeof(float)*stages))==NULL)      ||
      ((ds = (float *)malloc(sizeof(float)*stages))==NULL)      ||
      ((es = (float *)malloc(sizeof(float)*stages))==NULL) ){
      BTW_CLEANUP;
      return(0);
   }

/*
** compute the s-plane lowpass poles
*/

    lowstages = stages;

    for(i=0;i<lowstages;i++){
        poweri = XV_PI*(2.0*(i+1)+lowstages-1)/(2.0*lowstages);
        scalar = wc*(pow((double)epsilon,(double)(-1.0/(float)lowstages)));
        sreal[i] = scalar*cos(poweri);
        simag[i] = scalar*sin(poweri);
    }

/*
** compute corresponding bandstop poles.
** 
**      Pn = ((wc^2/s[n]) +/- sqrt((wc^2/s[n]) - 4*w1*w4))/2.0
*/

    for(i=0;i<lowstages;i++){
        cdiv(&br,&bi,(float)pow((double)wc,(double)2.0),0.0,sreal[i],simag[i]);
        cmul(&bsqrdr,&bsqrdi,br,bi,br,bi);
        fouracr = 4.0*w1*w4;

        csqrt(&ctempr,&ctempi,bsqrdr-fouracr,bsqrdi);
        cadd(&ctempr,&ctempi,br,bi,ctempr,ctempi);
        pr[2*i] = ctempr / 2.0;
        pi[2*i] = ctempi / 2.0;

        csqrt(&ctempr,&ctempi,bsqrdr-fouracr,bsqrdi);
        csub(&ctempr,&ctempi,br,bi,ctempr,ctempi);
        pr[2*i+1] = ctempr / 2.0;
        pi[2*i+1] = ctempi / 2.0;
    }

/*
** compute the s-plane bandpass filter coefficients; use these values to 
** compute the digital coefficients.  if it is the first stage that we are
** calculating coefficients for, normalize the numerator's coefficients.
*/

    for(i=0;i<stages;i++){
        as[i] = -1.0;
        bs[i] = 0.0;
        cs[i] = -w1*w4;
        ds[i] = -2.0*pr[i];
        es[i] = (float)pow((double)vcabs(pr[i],pi[i]),(double)2.0);
 
        denom = 1.0 + ds[i] + es[i];
        a[i] = (as[i] + bs[i] + cs[i])/denom;
        b[i] = (2.0*cs[i] - 2.0*as[i])/denom;
        c[i] = (as[i] - bs[i] + cs[i])/denom;
        d[i] = (2.0*es[i] - 2.0)/denom;
        e[i] = (1.0 - ds[i] + es[i])/denom;
    }

    BTW_CLEANUP;
    return(1);
}

/***********************************************************************
*
*  Routine Name: chbistop() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a band stop 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
* 
* Modifications:
*
***********************************************************************/

static int chbistop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float epsilon,lambda,wc,wr,w1,w4,*a,*b,*c,*d,*e;
{
    int n,stages,order,lowstages;
    float alpha,bn,sr,si,tr,ti,c1,c2,bsqrdr,bsqrdi,*poler,*polei;
    float fltden,fouracr,as,bs,cs,ds,es,br,bi;

/*
** calculate useful numbers
*/

    order = 2*(int)ceil((ARCCOSH(lambda/epsilon)/ARCCOSH(wr/wc)));
    order += order%4;

    stages = order/2;
    *nstages = stages;

/*
** allocate memory for poles
*/

    poler = (float *)malloc(stages*2*sizeof(float));
    if(poler==NULL)return(0);

    polei = (float *)malloc(stages*2*sizeof(float));
    if(polei==NULL){
       free(poler);
       return(0);
    }

/*
** calculate some useful numbers
*/

    alpha = ARCSINH(1.0/epsilon)/stages;

    c1 = wc*sinh(alpha);
    c2 = wc*cosh(alpha);

    fouracr = 4.0*w1*w4;

    lowstages = stages;
    for(n=0;n<lowstages;n++){
        bn = XV_PI*(((float)(2.0*(n+1)-1.0+lowstages))/(2.0*(float)lowstages));

        sr = c1*cos(bn);
        si = c2*sin(bn);

/*
** compute the bandstop poles
*/

        cdiv(&br,&bi,wc*wc,0.0,sr,si);
        cmul(&bsqrdr,&bsqrdi,br,bi,br,bi);

        csqrt(&tr,&ti,bsqrdr-fouracr,bsqrdi);
        csub(&tr,&ti,br,bi,tr,ti);
        cdiv(&poler[2*n],&polei[2*n],tr,ti,2.0,0.0);

        csqrt(&tr,&ti,bsqrdr-fouracr,bsqrdi);
        cadd(&tr,&ti,br,bi,tr,ti);
        cdiv(&poler[2*n+1],&polei[2*n+1],tr,ti,2.0,0.0);
     }
/*
** compute the s-plane coefficients; use these to compute the digital
** coefficients
*/

    for(n=0;n<stages;n++){
        as = -1.0;
        bs = 0.0;
        cs = -w1*w4;
        ds = -2.0*poler[n];
        es = pow((double)vcabs(poler[n],polei[n]),(double)2.0);

        fltden = 1.0 + ds + es;
        a[n] = (as + bs + cs)/fltden;
        b[n] = (2.0*cs - 2.0*as)/fltden;
        c[n] = (as - bs + cs)/fltden;
        d[n] = (2.0*es - 2.0)/fltden;
        e[n] = (1.0 - ds + es)/fltden;

        if(n==0){
           tr = sqrt(1.0+epsilon*epsilon);
           a[n] /= tr;
           b[n] /= tr;
           c[n] /= tr;
        }
    } /* end for */ 

    free(poler);
    free(polei);
    return(1);
}

/***********************************************************************
*
*  Routine Name: chbiistop() 
*
*          Date: Fri Sep  7 15:35:49 MDT 1990
*        
*       Purpose: computes coefficients for a band stop 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
* 
* Modifications:
*
***********************************************************************/


static int chbiistop(wc,wr,w1,w4,epsilon,lambda,a,b,c,d,e,nstages)
    int *nstages;
    float epsilon,lambda,wc,wr,w1,w4,*a,*b,*c,*d,*e;
{
    int stages,n,order,lowstages;
    float alpha,bn,sr,si,tr,ti,c1,c2,bsqrdr,bsqrdi,
          *poler,*polei,*zeror,*zeroi,epslnhat;
    float fltden,as,bs,cs,ds,es, fourac, br, bi;

/*
** calculate useful numbers
*/

    order = 2*(int)ceil((ARCCOSH(lambda/epsilon)/ARCCOSH(wr/wc)));
    order += order%4;

    stages = order/2;
    *nstages = stages;

/*
** allocate temporary workspace for pole and zero calculation
*/

    poler = (float *)malloc(stages*2*sizeof(float));
    if(poler==NULL)return(0);

    polei = (float *)malloc(stages*2*sizeof(float));
    if(polei==NULL){
       free(poler);
       return(0);
    }

    zeror = (float *)malloc(stages*2*sizeof(float));
    if(zeror==NULL){
       free(poler);
       free(polei);
       return(0);
    }

    zeroi = (float *)malloc(stages*2*sizeof(float));
    if(zeroi==NULL){
       free(poler);
       free(polei);
       free(zeror);
       return(0);
    }

/*
** calculate some useful numbers
*/

    epslnhat = 1.0/(epsilon* (cosh((double)stages*ARCCOSH(wr/wc))));
    alpha = ARCSINH(1.0/epslnhat)/stages;

    c1 = wc*sinh(alpha);
    c2 = wc*cosh(alpha);
    fourac = 4.0*w1*w4;

    lowstages = stages;

    for(n=0;n<lowstages;n++){
        bn = XV_PI*(((float)(2.0*(n+1)-1.0+lowstages))/(2.0*(float)lowstages));

        sr = c1*cos(bn);
        si = c2*sin(bn);

/*
** compute the bandstop poles
*/

/*        cdiv(&br,&bi,wc/wr,0.0,sr,si); */
        cmul(&br,&bi,wc/wr,0.0,sr,si);
        cmul(&bsqrdr,&bsqrdi,br,bi,br,bi);

        csub(&tr,&ti,bsqrdr,bsqrdi,fourac,0.0);
        csqrt(&tr,&ti,tr,ti);
        cadd(&tr,&ti,br,bi,tr,ti);
        cdiv(&poler[2*n],&polei[2*n],tr,ti,2.0,0.0);

        csub(&tr,&ti,bsqrdr,bsqrdi,fourac,0.0);
        csqrt(&tr,&ti,tr,ti);
        csub(&tr,&ti,br,bi,tr,ti);
        cdiv(&poler[2*n+1],&polei[2*n+1],tr,ti,2.0,0.0);

/*
** compute the bandstop zeros
*/

/*        cdiv(&br,&bi,0.0,pow(wr,2.0)*sin(bn),wr,0.0); */
        cdiv(&br,&bi,0.0,pow(wc,2.0)*sin(bn),wr,0.0);
        cmul(&bsqrdr,&bsqrdi,br,bi,br,bi);

        csub(&tr,&ti,bsqrdr,bsqrdi,fourac,0.0);
        csqrt(&tr,&ti,tr,ti);
        cadd(&tr,&ti,br,bi,tr,ti);
        cdiv(&zeror[2*n],&zeroi[2*n],tr,ti,2.0,0.0);

        csub(&tr,&ti,bsqrdr,bsqrdi,fourac,0.0);
        csqrt(&tr,&ti,tr,ti);
        csub(&tr,&ti,br,bi,tr,ti);
        cdiv(&zeror[2*n+1],&zeroi[2*n+1],tr,ti,2.0,0.0);
    }

/*
** compute the s-plane coefficients; use these to compute the digital
** coefficients
*/

    for(n=0;n<stages;n++){
        as = 1.0;
        bs = -2.0*zeror[n];
        cs = pow((double)vcabs(zeror[n],zeroi[n]),(double)2.0);
        ds = -2.0*poler[n];
        es = pow((double)vcabs(poler[n],polei[n]),(double)2.0);

        fltden = 1.0 + ds + es;
        a[n] = (as + bs + cs)/fltden;
        b[n] = (2.0*cs - 2.0*as)/fltden;
        c[n] = (as - bs + cs)/fltden;
        d[n] = (2.0*es - 2.0)/fltden;
        e[n] = (1.0 - ds + es)/fltden;
    } /* end for */ 

    free(poler);
    free(polei);
    free(zeror);
    free(zeroi);
    return(1);
}


/***********************************************************************
*
*  Routine Name: csqrt() 
*
*          Date: Fri Oct 19 14:04:56 MDT 1990
*        
*       Purpose:  
*
*         Input: 
*
*        Output: 
*
*    Written By:  
*
* Modifications:
*
***********************************************************************/

static int csqrt(sreal,simag,areal,aimag)
    float *sreal, *simag, areal, aimag;
{
    float ar,ai,r,w;

    if(areal==0.0 && aimag == 0.0){
       *sreal = 0.0;
       *simag = 0.0;
    }else{
       ar = (float)fabs((double)areal);
       ai = (float)fabs((double)aimag);

       if(ar >= ai){
          r = ai/ar;
          w = sqrt(ar) * sqrt(0.5*(1.0+sqrt(1.0+r*r)));
       }else{
          r = ar/ai;
          w = sqrt(ai) * sqrt(0.5*(r+sqrt(1.0+r*r)));
       }
       if(areal >= 0.0){
          *sreal = w;
          *simag = aimag/(2.0*w);
       }else{
          *simag = (aimag >= 0) ? w : -w;
          *sreal = aimag/(2*(*simag));
       }
    }
    return(1);
} 

/* -library_code_end */
