 /*
  * Khoros: $Id: ldllatt.c,v 1.2 1992/03/20 23:27:47 dkhoros Exp $
  */

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

 /*
  * $Log: ldllatt.c,v $
 * Revision 1.2  1992/03/20  23:27:47  dkhoros
 * VirtualPatch5
 *
  */

/*
 *----------------------------------------------------------------------
 *
 * Copyright 1991, 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 1991 by UNM */

/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 >>>>
 >>>>         File Name: ldllatt.c
 >>>>
 >>>>      Program Name: dllatt
 >>>>
 >>>> Date Last Updated: Sat Feb 15 16:26:20 1992 
 >>>>
 >>>>          Routines: ldllatt - the library call for dllatt
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
/* clean up code */
#define DLLATT_CLEANUP \
{  /* cleanup workspace before leaving */ \
   if(forwr!=NULL)free((char *)forwr); \
   if(forwi!=NULL)free((char *)forwi); \
   if(backwr!=NULL)free((char *)backwr); \
   if(backwi!=NULL)free((char *)backwi); \
   if(prevwr!=NULL)free((char *)prevwr); \
   if(prevwi!=NULL)free((char *)prevwi); \
   if(parcorr!=NULL)free((char *)parcorr); \
   if(parcori!=NULL)free((char *)parcori); \
} /* end of ldllatt cleanup code */

/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldllatt - library call for dllatt
*
* Purpose:
*    
*    1D LMS Lattice Predictor Filter
*    
*    

* Input:
*    
*    image          pointer to VIFF structure containing image data to
*                   be processed.
*    
*    alpha          convergence parameter (0<alpha<1).
*    
*    stages         used to determine the dimension (length or  number
*                   of filter coefficients) of the filter.  If it is a
*                   positive, non-zero integer.  Default value is  set
*                   to 1.
*    
*    arith_type     type of arithmetic to be used on complex data.   0
*                   specifies scalar arithmetic and 1 specifies vector
*                   arithmetic.
*    
*    out_option     specifies the output  file  to  be  generated.   0
*                   specifies  an error sequence, 1 specifies a signal
*                   estimate sequence, and 2 specifies a filter  coef-
*                   ficient sequence.
*    
*    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: Ramiro Jordan
*    
*    

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


/* -library_def */
int ldllatt ( image, alpha, stages, arith_type, out_option, procdir )
int stages, out_option, arith_type, procdir;
float alpha;
struct xvimage *image;

/* -library_def_end */

/* -library_code */
{
  int num_vects, dimension, i, stage2, size;
  int llatp(), llatpx(), llatpxx(), dunload_vector();
  char **dload_vector(), *malloc();
  float **data, *error, *sigest, *parcor;

  char *program = "ldllatt";

  /* perform error checking of data types */
  if ( !(propertype(program, image, VFF_TYP_FLOAT, FALSE)) &&
       !(propertype(program, image, VFF_TYP_COMPLEX, FALSE)) )
  {
       (void) fprintf (stderr, "\n\n%s:   ", program);
       (void) fprintf (stderr, "ldllatt: data storage type must be of type float or complex\n\n");
       return (0);
  }

  /* reorganize the data into our internal representation */
  if ((data = (float **)dload_vector(image, &num_vects, &dimension, procdir))
            == NULL)
  {
     (void) fprintf(stderr,"%s: dload_vector failed \n",program);
     return(0);
  }

  size = dimension;
  stage2 = stages;
  if ( image->data_storage_type == VFF_TYP_COMPLEX )
  {
     size *= 2;
     stage2 *= 2;
  }

  /* allocate working arrays */
  error = (float *) malloc((unsigned int) size * sizeof(float));
  sigest = (float *) malloc((unsigned int) size * sizeof(float));
  parcor = (float *) malloc((unsigned int) stage2 * sizeof(float));

  /* set working arrays to zero */
  bzero( (char *)error, (int) (size * sizeof(float)) );
  bzero( (char *)sigest, (int) (size * sizeof(float)) );
  bzero( (char *)parcor, (int) (stage2 * sizeof(float)) );

  free ((char *) image->imagedata);

  /* begin processing for all vectors */
  for ( i = 0; i < num_vects; i++ )
  {
      if ( image->data_storage_type == VFF_TYP_FLOAT )
      {
         llatp ( data[i], dimension, stages, alpha, 
                    error, sigest, parcor );
      }
      else if ( image->data_storage_type == VFF_TYP_COMPLEX )
      {
           if ( arith_type == 0 )
           {
              llatpx ( data[i], dimension, stages, alpha, 
                          error, sigest, parcor );
           }
           else if ( arith_type == 1 )
           {
                llatpxx ( data[i], dimension, stages, alpha, 
                             error, sigest, parcor );
           }
           else
           {
                (void) fprintf (stderr,"ldllatt: unknown arithmetic type, %d\n",
 arith_type);
           }
      }
      if ( out_option == 0 )
      {
         (void) bcopy ((char*)error, (char *) data[i], (int)size*sizeof(float));

      }
      else if ( out_option == 1 )
      {
           (void) bcopy ((char*)sigest, (char *) data[i], (int)size*sizeof(float));
      }
      else if ( out_option == 2 )
      {
           (void) bcopy ((char*)parcor, (char *) data[i], (int)stage2*sizeof(float));
      }
      else
      {
           (void) fprintf (stderr,"ldllatt: unknown output file selection, %d\n", out_option);
      }

  } /* end of vector processing */

  /* return data to its original format */
  if ( out_option == 2 )
  {
     dimension = stages;
  }
  if (!dunload_vector((char **)data, image, image->data_storage_type, num_vects,
 dimension, procdir))
  {
     (void) fprintf (stderr,"%s: dunload_vector failed \n",program);
     return(0);
  }

  return(1);

} /* end of ldllatt */




/*********************************************************************
*
*       PROGRAM: llatp.c
*
*       PURPOSE: This routine generates a prediction error sequence,
*                a signal estimate sequence and a filter weights 
*                sequence from a lattice filter realization.
*                Uses the LMS adaptive procedure.
*
*        INPUT : - datain: inputa data record
*                - points: number of points to process
*                - stages: number of filter coefficients
*                - alpha:  convergence parameter (0<alpha<1)
*
*       OUTPUT : - error:  error sequence
*                - sigest: signal estimate
*                - parcor: filter coefficients
*
*********************************************************************/

int
llatp ( datain, points, stages, alpha, error, sigest, parcor )

int   stages, points;
float alpha;
float *datain, *error, *sigest, *parcor;
{
    int   tindex, lat;
    char  *malloc();
    float twoalpha, *forw, *backw, *prevw;

   /* initialize parameters */
    twoalpha = 2.0 * alpha;

   /* allocate space for working arrays */
    forw = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    backw = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    prevw = (float *) malloc((unsigned int) (stages+1) * sizeof(float));

   /* initialize arrays to zero */
    bzero((char *) forw, (int) (stages+1) * sizeof(float));
    bzero((char *) backw, (int) (stages+1) * sizeof(float));
    bzero((char *) prevw, (int) (stages+1) * sizeof(float));

   /* begin adaptive lattice predictor filter */
    for ( tindex = 0; tindex < points; tindex++) 
    {
        forw[0] = backw[0] = datain[tindex];

        for (lat = 0; lat < stages; lat++) 
        {
            forw[lat+1] = forw[lat] - parcor[lat] * prevw[lat];
            backw[lat+1] = prevw[lat] - parcor[lat] * forw[lat];

            parcor[lat] = parcor[lat] + twoalpha * ( forw[lat+1] * prevw[lat]
                               + forw[lat] * backw[lat+1] );

            prevw[lat] = backw[lat];
        } 

        prevw[stages] = backw[stages];
        error[tindex] = forw[stages];
        sigest[tindex] = datain[tindex] - error[tindex];
    }

    free((char *)forw);
    free((char *)backw);
    free((char *)prevw);

} /* end of llatp */



/*********************************************************************
*
*       PROGRAM: llatpx.c
*
*       PURPOSE: This routine generates a prediction error sequence,
*                a signal estimate sequence and a filter weights 
*                sequence from a lattice filter realization.
*                Uses the LMS adaptive procedure.
*                Operates on complex data using real arithmetic,
*                that is, real and imaginary numbers are treated 
*                separatedly when processed.
*
*        INPUT : - datain: inputa data record
*                - points: number of points to process
*                - stages: number of filter coefficients
*                - alpha:  convergence parameter (0<alpha<1)
*
*       OUTPUT : - error:  error sequence
*                - sigest: signal estimate
*                - parcor: filter coefficients
*
*********************************************************************/

int
llatpx ( datain, points, stages, alpha, error, sigest, parcor )

int   stages, points;
float alpha;
float *datain, *error, *sigest, *parcor;
{
    int   i, j, tindex, lat, size;
    char  *malloc();
    float twoalpha, *forwr, *forwi;
    float *backwr, *backwi, *prevwr, *prevwi;
    float *parcorr, *parcori;

   /* initialize parameters */
    size = points * 2;
    twoalpha = 2.0 * alpha;

   /* allocate space for working arrays */
    forwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    forwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    backwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    backwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    prevwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    prevwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    parcorr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    parcori = (float *) malloc((unsigned int) (stages+1) * sizeof(float));

   /* initialize arrays to zero */
    bzero((char *) forwr, (int) (stages+1) * sizeof(float));
    bzero((char *) forwi, (int) (stages+1) * sizeof(float));
    bzero((char *) backwr, (int) (stages+1) * sizeof(float));
    bzero((char *) backwi, (int) (stages+1) * sizeof(float));
    bzero((char *) prevwr, (int) (stages+1) * sizeof(float));
    bzero((char *) prevwi, (int) (stages+1) * sizeof(float));
    bzero((char *) parcorr, (int) (stages+1) * sizeof(float));
    bzero((char *) parcori, (int) (stages+1) * sizeof(float));

   /* begin adaptive lattice predictor filter */
    for ( tindex = 0; tindex < size; tindex += 2 ) 
    {
        forwr[0] = backwr[0] = datain[tindex];
        forwi[0] = backwi[0] = datain[tindex+1];

        for ( lat = 0; lat < stages; lat++ ) 
        {
            forwr[lat+1] = forwr[lat] - parcorr[lat] * prevwr[lat];
            backwr[lat+1] = prevwr[lat] - parcorr[lat] * forwr[lat];
            forwi[lat+1] = forwi[lat] - parcori[lat] * prevwi[lat];
            backwi[lat+1] = prevwi[lat] - parcori[lat] * forwi[lat];

            parcorr[lat] = parcorr[lat] + twoalpha * 
                          (forwr[lat+1] * prevwr[lat] + forwr[lat] * backwr[lat+1]);
            parcori[lat] = parcori[lat] + twoalpha * 
                          (forwi[lat+1] * prevwi[lat] + forwi[lat] * backwi[lat+1]);

            prevwr[lat] = backwr[lat];
            prevwi[lat] = backwi[lat];
        } 

        prevwr[stages] = backwr[stages];
        prevwi[stages] = backwi[stages];
        error[tindex] = forwr[stages];
        error[tindex+1] = forwi[stages];
        sigest[tindex] = datain[tindex] - error[tindex];
        sigest[tindex+1] = datain[tindex+1] - error[tindex+1];
    }

    for ( i = 0, j = 0; i < stages*2; i += 2, j++ )
    {
        parcor[i] = parcorr[j];
        parcor[i+1] = parcori[j];
    }

    DLLATT_CLEANUP;

} /* end of llatpx */




/*********************************************************************
*
*       PROGRAM: llatpxx.c
*
*       PURPOSE: This routine generates a prediction error sequence,
*                a signal estimate sequence and a filter weights 
*                sequence from a lattice filter realization.
*                Uses the LMS adaptive procedure.
*                Operates on complex data using complex arithmetic,
*                that is, real and imaginary numbers are treated 
*                as one number when processed.
*
*        INPUT : - datain: inputa data record
*                - points: number of points to process
*                - stages: number of filter coefficients
*                - alpha:  convergence parameter (0<alpha<1)
*
*       OUTPUT : - error:  error sequence
*                - sigest: signal estimate
*                - parcor: filter coefficients
*
*********************************************************************/

int
llatpxx ( datain, points, stages, alpha, error, sigest, parcor )

int   stages, points;
float alpha;
float *datain, *error, *sigest, *parcor;
{
    int     tindex, lat, size, i;
    int     cadd(), csub(), cmul(), cdiv();
    char    *malloc();
    float   real, imag, tempr, tempi;
    float   *forwr, *forwi;
    float   *backwr, *backwi, *prevwr, *prevwi;
    float   *parcorr, *parcori;

   /* initialize parameters */
    size = points * 2;

   /* allocate space for working arrays */
    forwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    forwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    backwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    backwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    prevwr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    prevwi = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    parcorr = (float *) malloc((unsigned int) (stages+1) * sizeof(float));
    parcori = (float *) malloc((unsigned int) (stages+1) * sizeof(float));

   /* initialize arrays to zero */
    bzero((char *) forwr, (int) (stages+1) * sizeof(float));
    bzero((char *) forwi, (int) (stages+1) * sizeof(float));
    bzero((char *) backwr, (int) (stages+1) * sizeof(float));
    bzero((char *) backwi, (int) (stages+1) * sizeof(float));
    bzero((char *) prevwr, (int) (stages+1) * sizeof(float));
    bzero((char *) prevwi, (int) (stages+1) * sizeof(float));
    bzero((char *) parcorr, (int) (stages+1) * sizeof(float));
    bzero((char *) parcori, (int) (stages+1) * sizeof(float));

   /* begin adaptive lattice predictor filter */
    for ( tindex = 0; tindex < size; tindex += 2 ) 
    {
        forwr[0] = backwr[0] = datain[tindex];
        forwi[0] = backwi[0] = datain[tindex+1];

        for ( lat = 0; lat < stages; lat++ ) 
        {
          /* forward and backward propagation errors */
            cmul (&tempr, &tempi, parcorr[lat], -parcori[lat],
                  prevwr[lat], prevwi[lat]);
            csub (&forwr[lat+1], &forwi[lat+1], forwr[lat], forwi[lat],
                  tempr, tempi);
            cmul (&real, &imag, parcorr[lat], parcori[lat],
                  forwr[lat], forwi[lat]);
            csub (&backwr[lat+1], &backwi[lat+1], prevwr[lat], prevwi[lat],
                  real, imag);

           /* lattice filter coefficients update */
            cmul (&tempr, &tempi, prevwr[lat], prevwi[lat],
                  forwr[lat+1], -forwi[lat+1]);
            cmul (&real, &imag, forwr[lat], forwi[lat],
                  backwr[lat+1], -backwi[lat+1]); 
        /* printf("(%f,%f)  (%f,%f)\n",real,imag,tempr,tempi); */
            cadd (&real, &imag, real, imag, tempr, tempi);
            cmul (&real, &imag, real, imag, alpha, 0.0);
            cadd (&parcorr[lat], &parcori[lat], parcorr[lat], parcori[lat],
                  real, imag);

            prevwr[lat] = backwr[lat];
            prevwi[lat] = backwi[lat];
        } 

        prevwr[stages] = backwr[stages];
        prevwi[stages] = backwi[stages];
        error[tindex] = forwr[stages];
        error[tindex+1] = forwi[stages];
        sigest[tindex] = datain[tindex] - error[tindex];
        sigest[tindex+1] = datain[tindex+1] - error[tindex+1];
    }

    for ( lat = 0, i = 0; lat < stages*2; lat += 2, i++ )
    {
        parcor[lat] = parcorr[i];
        parcor[lat+1] = parcori[i];
    }

    DLLATT_CLEANUP;

} /* end of llatpxx */

/* -library_code_end */
