 /*
  * Khoros: $Id: ldzresp.c,v 1.2 1992/03/20 23:29:36 dkhoros Exp $
  */

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

 /*
  * $Log: ldzresp.c,v $
 * Revision 1.2  1992/03/20  23:29:36  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: ldzresp.c
 >>>>
 >>>>      Program Name: dzresp
 >>>>
 >>>> Date Last Updated: Mon Mar  9 20:20:42 1992 
 >>>>
 >>>>          Routines: ldzresp - the library call for dzresp
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
#include "vpoly.h"

#define LCLEANUP    {   if(data!=NULL){                         \
                           for(i=0;i<npolys;i++){               \
                               free(data[i]);                   \
                           }                                    \
                           free(data);                          \
                        }                                       \
                        if(resp!=NULL){                         \
                           for(i=0;i<npolys;i++){               \
                               if(resp[i]!=NULL)free(resp[i]);  \
                           }                                    \
                           free(resp);                          \
                        }                                       \
                    } 
#define EPSILON stepsize 
#define EVAL_INF -1

static int eval_poly();
/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldzresp - library call for dzresp
*
* Purpose:
*    
*    Calculates the magnitude, power or phase response  of  a  z-
*    domain system
*    
*    

* Input:
*    
*    poly           polynomials   described   in   terms   of   struct
*                   poly_struct.
*    
*    npolys         number of polynomials in poly.
*    
*    numpts         number of points per data set.
*    
*    side           determines whether response is  one-sided  (0)  or
*                   two-sided (1)
*    
*    resp_type      a value that specifies what type  of  response  is
*                   desired.   a  value  of  0  indicates magnitude; a
*                   value of 1 indicates power; and a value of 2 indi-
*                   cates phase.
*    
*    proc_dir       process direction:  0  indicated  vector  oriented
*                   processing, 1 indicates band oriented processing.
*    
*    

* Output:
*    
*    image          pointer to VIFF structure  containing  image  data
*                   after processing.
*    
*    Return Value:  1 on success, 0 on failure.
*    
*    

*
* Written By: Jeremy Worley
*    
*    J. Worley/R. Jordan 20 Nov 1990 15:52
*              added side so that -pi/2 - pi/2 or 0 - pi/2 can be gen-
*              erated
*    
*    Jeremy Worley 27 Nov 1990 00:52 MST
*              made modifications to support new  internal  polynomial
*              structure.
*    
*    Jeremy Worley 17 Dec 1990 12:58 MST
*              support for cases when the evaluation of the polynomial
*              results in infinity.
*    
*    Jeremy Worley 19 Dec 1990 17:36 MST
*              reinitialization bug in  the  evaluation  function  was
*              causing an reduction in bandwidth for multi-stage func-
*              tions.
*    
*    Jeremy Worley 27 Jan 1992 09:48 MST
*              changed declaration of eval_poly()  to  static  int  in
*              calling routines.
*    
*    Jeremy Worley 24 Feb 1992 20:33 MST
*              explicitly declared some routines that were  previously
*              implicitly declared.
*    
*    Jeremy Worley 09 Mar 1992 19:54 MST
*              Added checks of the return values of some internal rou-
*              tines.
*    
*    

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


/* -library_def */
int ldzresp(image,poly,npolys,numpts,side,resp_type,proc_dir)
    struct xvimage **image;
    struct poly_struct *poly;
    int npolys,numpts,resp_type,side,proc_dir;
/* -library_def_end */

/* -library_code */
{
  char *program = "ldzresp";
  int  i,n;
  float **resp;
  char **dload_vector();
  int dunload_vector();
  struct xvimage *createimage();
  int ldpwresp(),ldphresp();
  char *comment = "response data created by dzresp.";

/*
** check input arguments to see that they are legal
*/

  if(numpts<=0){
     fprintf(stderr,"%s:  number of points must be greater than 0 (%d).\n",
             program,numpts);
     return(0);
  }

  if(resp_type<0 || resp_type>2){
     fprintf(stderr,"%s:  unknown response type (%d).\n",program,resp_type);
     return(0);
  }

  if(proc_dir != DSP_VECTOR && proc_dir != DSP_BAND){
     fprintf(stderr,"%s:  unknown process direction (%d).\n",program,proc_dir); 
     return(0);
  }

  if(poly==NULL){
     fprintf(stderr,"%s:  no polynomials to process.\n",program);
     return(0);
  }

/*
** create image to be used for output
*/

  if(*image==NULL){
     *image = createimage(numpts,1,VFF_TYP_FLOAT,1,1,comment,
                         (unsigned long)0, /* map_row_size */
                         (unsigned long)0, /* map_col_size */
                         (unsigned long)VFF_MS_NONE,
                         (unsigned long)VFF_MAPTYP_NONE,
                         (unsigned long)VFF_LOC_IMPLICIT,
                         (unsigned long)0);/* location dimension */
     if(*image==NULL){ 
        fprintf(stderr,"%s:  unable to allocate image memory.\n",program);
        return(0);
     }
  }

/*
** allocate memory for response data
*/

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

  for(n=0;n<npolys;n++){
      resp[n] = (float *)malloc(numpts*sizeof(float));
      if(resp[n]==NULL){
         fprintf(stderr,"%s:   [2] memory allocation failure.\n",program);
         return(0);
      }
  }

/*
** now let's do some work!
*/

  for(n=0;n<npolys;n++){
      switch(resp_type){
        case 0 : if(!ldpwresp(resp[n],numpts,side,&poly[n])){
                    fprintf(stderr,"%s: Failed call to ldpwresp().\n",program);
                 }
                 for(i=0;i<numpts;i++) resp[n][i] = sqrt(resp[n][i]);
                 break;
        case 1 : if(!ldpwresp(resp[n],numpts,side,&poly[n])){
                    fprintf(stderr,"%s: Failed call to ldpwresp().\n",program);
                 }
                 break;
        case 2 : if(!ldphresp(resp[n],numpts,side,&poly[n])){
                    fprintf(stderr,"%s: Failed call to ldpwresp().\n",program);
                 }
                 break;
      }
  }

/*
** return the data to its appropriate format and exit
*/

  if(!dunload_vector((char **)resp,*image,(unsigned long)VFF_TYP_FLOAT,npolys,
     numpts, proc_dir)) {
     fprintf (stderr,"%s: dunload_vector failed \n",program);
     return(0);
  }

  return(1);
}


/***********************************************************************
*
*  Routine Name: ldpwresp()
*
*          Date: Wed Aug  8 12:55:36 MDT 1990
*        
*       Purpose: calculates the power response of a system 
*
*         Input: numpts      - desired number of points in output (also
*                               dimension of resp)
*                side        - 0 for one sided
*                              1 for two sided
*                poly        - polynomial of z to be evaluated.
*
*        Output: resp        - response of polynomial function
*                return code - 0 for failure
*                              1 for normal termination
*
*    Written By: Jeremy Worley 
*
* Modifications: Tue Nov 20 15:52:05 MST 1990 JW/RJ 
*                       added side so that -pi/2 to pi/2 or 0 to pi/2 can 
*                       be generated.
*                Mon Dec 17 12:16:12 MST 1990 JW
*                       added handling so that if an overflow occurs in 
*                       eval_poly(), then the response at that point is
*                       set to XV_MAXFLOAT. 
*
***********************************************************************/

int ldpwresp(resp,numpts,side,poly)
  struct poly_struct *poly;
  float *resp;
  int numpts,side;
{
  char *program = "ldpwresp";
  int n,mflag;
  float stepsize,x,y,areal,aimag,arg;

  stepsize = XV_2PI / (numpts*(2-side));
  arg = (side==0) ? 0.0 : -XV_PI;

  for(n=0;n<numpts;n++){
      areal = cos(arg);
      aimag = sin(arg);

      mflag = eval_poly(&x,&y,poly,areal,aimag);
      if(mflag==0){
         fprintf(stderr,"%s: function terms not allowed.\n",program);
         return(0);
      }else if(mflag==EVAL_INF){
         resp[n] = XV_MAXFLOAT;
      }else{ 
         resp[n] = x*x + y*y;
      }

      arg += stepsize;
   }

   return(1);
}


/***********************************************************************
*
*  Routine Name: ldphresp()
*
*          Date: Wed Aug  8 12:58:12 MDT 1990
*        
*       Purpose: calculates the phase response of a system 
*
*         Input: numpts      - desired number of points in output (also
*                               dimension of resp)
*                side        - 0 for one sided
*                              1 for two sided
*                poly        - polynomial to be evaluated
*
*        Output: resp        - response of polynomial function
*                return code - 0 for failure
*                              1 for normal termination
*
*    Written By: Jeremy Worley 
*
* Modifications: Tue Nov 20 15:52:05 MST 1990 JW/RJ 
*                       added side so that -pi/2 to pi/2 or 0 to pi/2 can 
*                       be generated.
*                Mon Dec 17 12:28:04 MST 1990 JW
*                       added a little intelligence to that if Nan or Inf
*                       is returned from eval_poly(), it is trapped and
*                       and the phase is not screwed because of it.     
*
***********************************************************************/

int ldphresp(resp,numpts,side,poly)
  struct poly_struct *poly;
  float *resp;
  int numpts,side;
{
  char *program = "ldphresp";
  int n,mflag1,mflag2=0; 
  float stepsize,x,y,areal,aimag,arg;

  stepsize = XV_2PI / (numpts*(2-side));

  arg = (side==0) ? 0.0 : -XV_PI;

  for(n=0;n<numpts;n++){
      areal = cos(arg);
      aimag = sin(arg);

      arg += stepsize;

      mflag1 = eval_poly(&x,&y,poly,areal,aimag);
      if(mflag1==0){
         fprintf(stderr,"%s: function terms not allowed.\n",program);
         return(0);
      }else if(mflag1==EVAL_INF){ 
         if(n==numpts-1){               /* if were at the last point, then  */
            resp[n] = resp[n-1];        /* set it to the previous value,    */
         }else{                         /* otherwise we set it the value of */
            mflag2=1;                   /* the next one.                    */
         }
      }else{
         if(y==0.0 && x==0.0){
            resp[n] = 0.0;
         }else{
            resp[n] = atan2(y,x);
            if((fabs((double)resp[n]) >= (double)(XV_PI - EPSILON)) && 
               (fabs((double)resp[n]) <= (double)(XV_PI + EPSILON))){
                if(arg>=0.0)
                  resp[n] = XV_PI;
                else
                  resp[n] = -XV_PI;
            }
         }
         if(mflag2){                    /* simply setting previous value to */
            resp[n-1]=resp[n];          /* current value if the previous    */
            mflag2=0;                   /* evaluated to Nan or Inf.         */
         }
      } /*end if mflag1*/
  }/*end for*/

  return(1);
}


/***********************************************************************
*
*  Routine Name: eval_poly()
*
*          Date: Tue Nov 27 00:38:20 MST 1990
*        
*       Purpose: evaluates a polynomial.  
*
*         Input: 
*
*        Output: 
*
*    Written By: Jeremy Worley
*
* Modifications: Mon Dec 17 12:30:01 MST 1990 JW
*                       added processing to that it traps divide by zero
*                       situations and returns an EVAL_INF value that 
*                       indicates that the result was infinity, and lets
*                       the programmer deal with it anyway he wants to.
*
*                Wed Dec 19 17:42:08 MST 1990 JW
*                       reinitialization of xn,yn,xd,yd inside the stage
*                       loop was necessary to get correct results.
*
***********************************************************************/

static int eval_poly(rreal,rimag,poly,areal,aimag)
   float *rreal, *rimag, areal, aimag;
   struct poly_struct *poly;
{
   int stages,s,t,nnum,dnum, offset = 0;
   float cpr,cpi,xn,ynn,xd,yd;
   int cpower(),cmul(),cdiv();

   *rreal = 1.0;
   *rimag = 0.0;
  
   stages = poly->stages; 
   if(stages<=0)stages = 1;
 
   for(s=0;s<stages;s++){
       xn = ynn = xd = yd = 0.0;
       nnum = poly->nterms[2*s];
       dnum = poly->nterms[2*s+1];
     
       for(t=0;t<nnum;t++){
           if(poly->terms[offset+t].type==_FUNCTION)return(0);
           (void)cpower(&cpr,&cpi,areal,aimag,
                  (int)poly->terms[offset+t].expon);

           xn += poly->terms[offset+t].coef * cpr;
           ynn += poly->terms[offset+t].coef * cpi;
       }
       offset += nnum;

       for(t=0;t<dnum;t++){ 
           if(poly->terms[offset+t].type==_FUNCTION)return(0);
           (void)cpower(&cpr,&cpi,areal,aimag,
                  (int)poly->terms[offset+t].expon);
           xd += poly->terms[offset+t].coef * cpr;
           yd += poly->terms[offset+t].coef * cpi;
    
       }
       offset += dnum;

       if(dnum==0){
          xd = 1.0;
          yd = 0.0;
       }

       if(xd==0.0 && yd==0.0) return(EVAL_INF);
       cdiv(&cpr,&cpi,xn,ynn,xd,yd);

       cmul(rreal,rimag,*rreal,*rimag,cpr,cpi);
   }

   return(1);
}

int cpower(x2,y2,x1,yy1,e)
    float *x2,*y2,x1,yy1;
    int e;
{
  int pos_e,i;
  int abs();

  pos_e = abs(e);

  *x2 = 1.0;
  *y2 = 0.0;

  for(i=0;i<pos_e;i++)cmul(x2,y2,*x2,*y2,x1,yy1);

  if(e<0)cdiv(x2,y2,1.0,0.0,*x2,*y2);

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