 /*
  * Khoros: $Id: ldfilter.c,v 1.2 1992/03/20 23:26:02 dkhoros Exp $
  */

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

 /*
  * $Log: ldfilter.c,v $
 * Revision 1.2  1992/03/20  23:26:02  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: ldfilter.c
 >>>>
 >>>>      Program Name: dfilter
 >>>>
 >>>> Date Last Updated: Thu Mar  5 09:39:28 1992 
 >>>>
 >>>>          Routines: ldfilter - the library call for dfilter
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
#include "vpoly.h"
#define LCLEANUP    {   if(data!=NULL){                                 \
                           for(i=0;i<num_vects;i++){                    \
                               free((char *)data[i]);                   \
                           }                                            \
                           free((char *)data);                          \
                        }                                               \
                        if(name!=NULL)free((char *)name);               \
                        if(indep_var!=NULL)free((char *)indep_var);     \
                    }
static int lzprepare(),ldprepare(),ldzfilter(),lddfilter();
/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldfilter - library call for dfilter
*
* Purpose:
*    
*    filters signals according to a transfer function
*    
*    

* Input:
*    
*    image          pointer to VIFF structure containing image data to
*                   be processed.
*    
*    poly           polynomial structure  containing  polynomial  that
*                   represents  the  transfer function to be used as a
*                   filter.
*    
*    procdir        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
*    
*    Jeremy Worley 17 Feb 1992 11:43 MST
*              Fixed problem with  sign  of  autoregressive  terms  of
*              filter.
*    
*    Jeremy Worley 17 Feb 1992 11:55 MST
*              Made implicit function type of poly_info() explicit.
*    
*    Jeremy Worley 05 Mar 1992 09:17 MST
*              Fixed up autoregressive case of transfer function  pro-
*              cessing in ldzfilter().
*    
*    

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


/* -library_def */
int ldfilter(image,poly,proc_dir)
    struct xvimage *image;
    struct poly_struct *poly;
    int proc_dir;
/* -library_def_end */

/* -library_code */
{
  char *program = "ldfilter";
  int dunload_vector(),poly_info();
  char **dload_vector(),*name=NULL,*indep_var=NULL;
  float **data,min_exp,max_exp;
  int ftype,max_delay,min_delay,data_type = 1, dimension,
      num_vects, i, delay=0;

/*
** the first thing we gotta do is reorganize the data from our file format
** into the vectors that we will be filtering.
*/

    if((data = (float **)dload_vector(image, &num_vects, &dimension,
               proc_dir)) == NULL) {
       (void) fprintf(stderr,"%s: dload_vector failed \n",program);
       return(0);
    }

/*
** it is convenient to have the data type represented in the form: 1 = float
** 2 = complex.  This assumes that data_type is set to one in the declaration
*/

    if(image->data_storage_type==VFF_TYP_COMPLEX){
       data_type = 2;
    }

/*
** check to see what kinda function we are dealing with.  At this point we
** expect to see POLY_NORM, POLY_MIXED, or POLY_DIFF.  If we see anything
** else, it is invalid.  Furthermore, what the fudge does it mean to have
** a mixture of difference terms and z terms (POLY_MIXED)?  This program
** cant do anything with them, so we give up in a most cowardly fashion.
*/

   if(!poly_info(poly,&ftype,&max_exp,&min_exp,&max_delay,&min_delay,
        &name,&indep_var)){
      fprintf(stderr,"%s:  Polynomial error.\n",program);
      LCLEANUP;
      return(0);
   }

   if(ftype!=POLY_NORM && ftype!=POLY_DIFF){
      fprintf(stderr,"%s:  illegal format for polynomial.\n",program);
      LCLEANUP;
      return(0);
   }

/*
** depending on what type of function we're dealing with, call a routine that
** does some symbolic preprocessing to delay equations and rearrange them into
** something useful.
*/

  if(ftype==POLY_NORM){
     if(!lzprepare(poly,max_exp,indep_var)){
        LCLEANUP; /* message has already been generated */
        return(0);
     }
  }else{
     if(!ldprepare(poly,min_delay,max_delay,name,indep_var)){
        LCLEANUP; /* message has already been generated */
        return(0);
     }
  }

/*
** Now that all that nonsense is finished, we can assume that either the poly 
** is in a form that can be dealt with, or that the user has been riddled with
** error messages and verbal attacks on his intelligence.  So, if everything
** has passed so far without error, let's filter that data and get the heck
** outa here.
*/

  for(i=0;i<num_vects;i++){
      if(ftype==POLY_NORM){
         if(!ldzfilter(data[i],dimension,delay,poly,data_type)){
            fprintf(stderr,"%s:  failure to filter data.\n",program);
            fprintf(stderr,"Hmmm...what a wonderfully useless message...\n");
            LCLEANUP;
            return(0);
         }
      }else{
         if(!lddfilter(data[i],dimension,poly,data_type)){
            fprintf(stderr,"%s:  failure to filter data.\n",program);
            fprintf(stderr,"Hmmm...what a wonderfully useless message...\n");
            LCLEANUP;
            return(0);
         }
      }
  } /* end for */


/*
** unload the vectors back into their original positions and exit routine.
*/

    if (!dunload_vector((char **)data, image, image->data_storage_type,
               num_vects, dimension, proc_dir)) {
        (void) fprintf (stderr,"%s: dunload_vector failed \n",program);
        LCLEANUP;
        return(0);
    }

    LCLEANUP;
    return(1);
}


/***********************************************************************
*
*  Routine Name: lzprepare()
*
*          Date: Thu Dec 20 02:48:44 MST 1990
*        
*       Purpose:  
*
*         Input: 
*
*        Output: 
*
*    Written By: Jeremy Worley 
*
* Modifications:
*
***********************************************************************/

static int lzprepare(poly,max_exp,indep_var)
    struct poly_struct *poly;
    float max_exp;
    char *indep_var;
{
  char *program = "ldfilter";
  int dmax_exp,i,j,offset=0;

/*
** check to make sure that the functions are appropriately written for this
** type of operation.  Perhaps we shouldn't do this.  On the otherhand,
** I think this prevents some idiocy.
*/

  if(strcmp(indep_var,"z")!=0){
     fprintf(stderr,"%s:  Must be a z-domain transfer function or diff. eq.\n",
             program);
     return(0);
  }

/*
** if we are dealing with an H(z), then divide the polynomial by any z^MAX,
** if MAX is greater than zero.  It is here that we also determine the nature
** of the transfer function, i.e. is it causal or non-causal.
** if we are dealing with a transfer function, then we need to ensure that
** it is causal.  If it is non-causal we will increase the delays uniformly
** to make it causal.  At that point, if we have no y(n) term, then this
** equation cannot be represented as a causal system.  This program is not
** currently able to handle such a situation, so we bail out.
*/

  dmax_exp = poly->terms[poly->nterms[0]].expon;

  if(max_exp>0.0){
     for(i=0;i<poly->stages;i++){
         for(j=offset;j<offset+poly->nterms[2*i];j++){
             poly->terms[j].expon -= max_exp;
         }
         offset +=poly->nterms[2*i];
         for(j=offset;j<offset+poly->nterms[2*i+1];j++){
             poly->terms[j].expon -= max_exp;
             dmax_exp = (poly->terms[j].expon>dmax_exp)?poly->terms[j].expon:
                          dmax_exp;
         }
         offset +=poly->nterms[2*i+1];
    } /* end for i */
  }else{
     for(i=0;i<poly->stages;i++){
         offset +=poly->nterms[2*i];
         for(j=offset;j<offset+poly->nterms[2*i+1];j++){
             dmax_exp = (poly->terms[j].expon>dmax_exp)?poly->terms[j].expon:
                          dmax_exp;
         }
         offset +=poly->nterms[2*i+1];
     } /* end for i */
  } /* end big if statement */

/*
** now we can see if anything can be done with the resulting mess...
*/

  return(1);
}

/***********************************************************************
*
*  Routine Name: ldprepare() 
*
*          Date: Thu Dec 20 02:48:35 MST 1990
*        
*       Purpose:  
*
*         Input: 
*
*        Output: 
*
*    Written By:  
*
* Modifications:
*
***********************************************************************/

static int ldprepare(poly,min_delay,max_delay,name,indep_var)
    struct poly_struct *poly;
    int min_delay,max_delay;
    char *name,*indep_var;
{
  char *program = "ldfilter";
  int diff_zero=0,i,j,dmax_delay;

  if(poly->stages>1){
     fprintf(stderr,"%s:  This program is not doesn't handle multistage ",
             program);
     fprintf(stderr,"difference equations.\n");
     return(0);
  }

  if(poly->nterms[1]!=0){
     fprintf(stderr,"%s:  This program can not handle difference equations ",
             program);
     fprintf(stderr,"with denominators.\n");
     return(0);
  }

  if(strcmp(indep_var,"n")!=0){
     fprintf(stderr,"%s: Difference equations must be stated in terms of n.\n",
             program);
     return(0);
  }

/*
** The next three sets of tests are an attempt to restrict the format of
** difference equations to a form that this program can handle.  The error
** messages should explain the nature of the tests sufficiently.
*/

  if(strcmp(poly->func_name,"y")!=0){
     fprintf(stderr,"%s:  Difference equations must be stated as a function ",
             program);
     fprintf(stderr,"called y(n).\n");
     return(0);
  }

  for(i=0;i<poly->nterms[0];i++){
      if(strcmp(poly->terms[i].fname,"x")!=0 &&
         strcmp(poly->terms[i].fname,"y")!=0){
         fprintf(stderr,
                 "%s:  Difference equations should be stated in terms ",
                 program);
         fprintf(stderr,"of functions 'x' and 'y' only. i.e.:  y(n-1).\n");
         return(0);
      }
  }

/*
** in order for the next part of the symbolic processing to be completed
** correctly on difference equations, we need to make sure that the
** minimum delay is less than or equal to 1 (its already causal).  If this
** is not the case, then we need to subtract the y(n) term that is located
** on the left side of the difference equation from the right side.  Then,
** once all of the delay processing is complete, we choose a new y(n) from
** the y terms.
*/

  if(min_delay<0){
     if(realloc((char *)(poly->terms),
         poly->nterms[0]*sizeof(struct term_struct))==NULL){
        fprintf(stderr,"%s:  Unable to realloc polynomial structure.\n",
         program);
        return(0);
     }
     poly->terms[poly->nterms[0]].type = 1;
     poly->terms[poly->nterms[0]].coef = -1.0;
     poly->terms[poly->nterms[0]].delay = 0;
     if((poly->terms[poly->nterms[0]].fname=(char *)malloc(strlen(name)*
        sizeof(char)))==NULL){
        fprintf(stderr,"%s:  Unable to malloc function name in polynomial.\n",
          program);
        return(0);
     }
     strcpy(poly->terms[poly->nterms[0]].fname,name);
     diff_zero = 1; /* set the flag to indicate that the left side is zero */
  }

/*
** next we play games with this thing to attempt to delay it and get it
** into a causal form.
*/

  dmax_delay=poly->terms[0].delay;

  if(max_delay>0){
     for(j=0;j<poly->nterms[0];j++){
         poly->terms[j].delay -= max_delay;
         dmax_delay = (poly->terms[j].delay>dmax_delay) ?
                                poly->terms[j].delay : dmax_delay;
     }
  }else{
     for(j=0;j<poly->nterms[0];j++){
         dmax_delay = (poly->terms[j].delay>dmax_delay) ?
                                poly->terms[j].delay : dmax_delay;
     }
  }

/*
** now we can see if anything can be done with the resulting mess...
*/

  if(dmax_delay!=0){
     fprintf(stderr,"%s:  Uh-oh! This program is in disorder.\n",program);
     fprintf(stderr,"Please consult your local Khoros guru...\n");
     return(0);
  }

/*
** if diff_zero is set, that means that we must select an appropriate
** y term for use as the new left side...Incidently, if diff_zero is set,
** that implies that there is a difference equation...
*/

  if(diff_zero){
     diff_zero = -1;
     for(i=0;i<poly->nterms[0];i++){
         if(!strcmp(poly->terms[i].fname,"y") && poly->terms[i].delay==0){
            diff_zero = i;
         }
     }
     if(diff_zero==-1){
        fprintf(stderr,
            "%s:  Unable to find a suitable y term in the difference",program);
        fprintf(stderr," equation.");
        return(0);
     }
     for(i=0;i<poly->nterms[0];i++){
         if(i==diff_zero){
            poly->terms[i].coef = 0.0;
         }else{
            poly->terms[i].coef /= (0.0 - poly->terms[diff_zero].coef);
         }
     }
  } /* end if diff_zero */

/*
** at this point, all we can do is hope that everything is cool...
*/

  return(1);
}


/***********************************************************************
*
*  Routine Name: static ldzfilter()
*
*          Date: Sun Nov 18 18:55:47 MST 1990
*        
*       Purpose: filters a single vector based on a frequency domain
*                (z) transfer function by implementing the difference
*                equation that is represented by the polynomial.  
*
*         Input: data - data to be filtered (is destroyed) 
*                ddim - data dimension
*                poly - polynomial structure that will be used to filter
*                       the signal.
*                fdel - in certain cases, data should start at zero
*                       for initial data if y(n) cant be generated and
*                       keep the system causal
*                type - data type (1 = real, 2 = complex)
*
*        Output: data - filtered data. 
*
*    Written By: Jeremy Worley 
*
* Modifications:
*
***********************************************************************/

static int ldzfilter(data,ddim,fdel,poly,type)
   float *data;
   struct poly_struct *poly;
   int type,ddim,fdel;
{
  int delay,s,t,n,i,j,maxy,offset=0,nnum,dnum,idx;
  float weight,divterm,*y;
 
/*
 * malloc some temporary shit
 */

  if((y = (float *)malloc(ddim*type*sizeof(float)))==NULL)return(0);
  bzero(y,ddim*type*sizeof(float));

/*
 * this little section checks for the case that the transfer function does
 * not have a constant term in the denominator.  If this is the case, then
 * we must use the term with the minimum exponent.  This generates delays
 * that must be taken into consideration during the filtering process.
 */

  maxy = (int)(poly->terms[poly->nterms[1]].expon);

  for(i=0;i<poly->stages;i++){
      for(j=offset;j<offset+poly->nterms[2*i+1];j++){
          maxy = (poly->terms[j].expon>maxy)?(int)(poly->terms[j].expon):maxy;
      }
      offset += poly->nterms[2*i]+poly->nterms[2*i+1];
  }

/*
 * do the filtering
 */
 
  offset = 0;    
  for(s=0;s<MAX(poly->stages,1);s++){
      nnum = poly->nterms[2*s];
      dnum = poly->nterms[2*s+1];
      for(n=fdel;n<ddim;n++){
          divterm = 1.0;
          idx = type*n;

          for(t=0;t<nnum;t++){
              delay  = 0 - (int)poly->terms[offset+t].expon;
              weight = poly->terms[offset+t].coef;
              if(n-delay>=0)y[idx-maxy] += weight*data[idx-type*delay]; 
              if(n-delay>=0&&type==2)y[idx+1-maxy] += 
                         weight*data[idx-type*delay+1]; 
          }

          for(t=0;t<dnum;t++){
              delay  = 0 - (int)poly->terms[offset+t + nnum].expon;
              weight = 0.0 - poly->terms[offset+t + nnum].coef;
              if(delay!=maxy){
                 if(n-delay>=0)y[idx-maxy] += weight*y[idx-type*delay];
                 if(n-delay>=0&&type==2)y[idx+1-maxy] += weight*y[idx-type*delay+1];
              }else{
                 divterm *=weight ;
              }
          }
          y[idx] /= divterm;
          if(type==2)y[idx+1] /= divterm;
      }

      offset += nnum + dnum;
      bcopy(y,data,ddim*type*sizeof(float));
      bzero(y,ddim*type*sizeof(float));
  }/*for s*/

/*
** normal return
*/

  return(1);

} 


/***********************************************************************
*
*  Routine Name: lddfilter
*
*          Date:
*        
*       Purpose: filter data given a difference equation 
*
*         Input: 
*
*        Output: 
*
*    Written By: Jeremy Worley
*
* Modifications: Jeremy Worley Mon Feb 17 11:44:29 MST 1992
*                Changed "y[n] += weight*y[n-delay]" to 
*                "y[n] -= weight*y[n-delay]" because the 
*                + is incorrect.
*
***********************************************************************/

static int lddfilter(data,ddim,poly,type)
   float *data;
   struct poly_struct *poly;
   int type,ddim;
{
   char *program="lddfilter";
   float *y,weight;
   int n,t,delay;

   if((y=(float *)malloc(ddim*type*sizeof(float)))==NULL){
      fprintf(stderr,"%s:  Memory allocation error.\n",program);
      return(0);
   }

   bzero(y,ddim*type*sizeof(float));

   for(n=0;n<ddim;n++){
       for(t=0;t<poly->nterms[0];t++){
           delay  = 0.0 - poly->terms[t].delay;
           weight = poly->terms[t].coef;
           if(n-delay>=0){
              if(poly->terms[t].fname[0]=='x'){
                 y[n] += weight*data[n-delay]; 
              }else{
                 y[n] += weight*y[n-delay];
              }
           }
       }
   }

   bcopy(y,data,ddim*type*sizeof(float));

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