 /*
  * Khoros: $Id: ldarpsd.c,v 1.2 1992/03/20 23:25:07 dkhoros Exp $
  */

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

 /*
  * $Log: ldarpsd.c,v $
 * Revision 1.2  1992/03/20  23:25:07  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: ldarpsd.c
 >>>>
 >>>>      Program Name: darpsd
 >>>>
 >>>> Date Last Updated: Mon Mar  9 18:55:10 1992 
 >>>>
 >>>>          Routines: ldarpsd - the library call for darpsd
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldarpsd - library call for darpsd
*
* Purpose:
*    
*    1D Autoregressive (AR) PSD
*    
*    

* Input:
*    
*    image          pointer to VIFF structure containing image data to
*                   be processed.
*    
*    nfreq          number of data points for PSD estimate  (nfreq  >=
*                   0).
*    
*    arfrom         AR  coefficients:  come  from   a   tapped   delay
*                   filter(0) or from a lattice filter(1).
*    
*    estimate       Spectral estimate: phase spectral  estimate(0)  or
*                   PSD estimate(1).
*    
*    normalize      normalize PSD estimate: if yes (0) then normalize,
*                   else no (1) do nothing.
*    
*    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
*    
*    Jeremy Worley Mon Feb 17 23:24:36 MST 1992
*                                            1. argument  declarations
*                                            for  arfrom  and estimate
*                                            in ldarpsd() missing.  2.
*                                            propertype()   implicitly
*                                            declared...made  explicit
*                                            int.      3.    larpsde()
*                                            dropped   off   the   end
*                                            without a return() call.
*    
*    

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


/* -library_def */
int ldarpsd ( image, nfreq, arfrom, estimate, normalize, procdir )
int nfreq, procdir, normalize,arfrom,estimate;
struct xvimage *image;

/* -library_def_end */

/* -library_code */
{
  int i, num_vects, dimension, data_type, stages;
  int larpsde();
  char **dload_vector(),*malloc();
  int dunload_vector(),propertype();
  float **psde, **data;
  char *program = "ldarpsd";

  /* initialize parameters */
  data_type = 0;

  /* 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, "ldarpsd: 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);
  }

  stages = dimension;
  if ( image->data_storage_type == VFF_TYP_COMPLEX ) 
  {
     data_type = 1;
  }

  /* allocate for the new working arrays */
  if ( (psde = (float **) (malloc ((unsigned int) num_vects * 
               sizeof(float))) ) == NULL )
  {
    (void) fprintf(stderr,"ldmirror: Cannot allocate psde array.\n");
    return(0);
  }
  for ( i = 0; i < num_vects; i++ )
  {
    if ( (psde[i] = (float *) (malloc ((unsigned int) nfreq *
         sizeof(float) ))) == NULL )
    {
      (void)fprintf(stderr,"ldmirror: Cannot alloc psde array.\n");
      return(0);
    }
  }

  /* set working arrays to zero */
  for ( i = 0; i < num_vects; i++ )
  {
      bzero( (char *)psde[i], (int) (nfreq * sizeof(float)) );
  }

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

  /* begin processing for all vectors */ 
  for ( i = 0; i < num_vects; i++ )
  {
      larpsde ( data_type, nfreq, stages, arfrom, estimate, normalize, data[i], psde[i] );
  } /* end of vector processing */

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

  for (i=0; i<num_vects; i++)
  {
    free((char*)data[i]);
    free((char*)psde[i]);
  }
  free((char**)data);
  free((char**)psde);

  return(1); 

} /* end of ldarpsd */



/*********************************************************************
*
*       PROGRAM: larpsde.c
*
*       PURPOSE: This routine generates an autoregressive PSD estimate.
*
*        INPUT : - datatype: type of data (real or complex)
*                - nfreq: number of points of PSD estimate 
*                - stages: number of AR coefficients
*                - arfrom: origin of AR coefficients (transversal or lattice)
*                - estimate: PSD or phase estimate
*                - normalize: if 0 yes normalize, else 1 no
*                - arpar: array of AR parameters 
*
*       OUTPUT : - psde: AR PSD estimate
*
*********************************************************************/

#define TWOPI 6.28318531

int
larpsde ( datatype, newfreq, stages, arfrom, estimate, normalize, arpar, psde )    
int   newfreq, stages, datatype, arfrom, estimate, normalize;
float *arpar, *psde;
{
    int    freq, sta, k, cadd(), cmul();
    float  omega, mag, real, imag, maxw;
    float  rdenom, idenom, tempc, temps;
    double ang, cmagsq();

   /* for real AR parameters (datatype=0) */
    if ( !datatype )
    {
       if(!arfrom){ /* tapped */ 
          for ( freq = 0; freq < newfreq; freq++ ) 
          {
              omega = TWOPI * (float) freq / (float) newfreq;
              rdenom = arpar[0];
              idenom = 0.0;
              for ( sta = 1; sta < stages; sta++ ) 
              {
                  ang = -omega * (float)sta;
                  tempc = cos(ang) * arpar[sta];
                  temps = sin(ang) * arpar[sta];
                  rdenom += tempc;
                  idenom += temps;
               }
               /* PSD estimate for tapped and lattice ar coefficients */
               psde[freq] = (rdenom * rdenom + idenom * idenom);
           }
       }
       else /* lattice */
       {
          for ( freq = 0; freq < newfreq; freq++ )
          {
              omega = TWOPI * (float) freq / (float) newfreq;
              rdenom = 1.0;
              idenom = 0.0;
              for ( sta = 1; sta <= stages; sta++ )
              {
                  ang = -omega * (float)sta;
                  tempc = cos(ang) * arpar[sta-1];
                  temps = sin(ang) * arpar[sta-1];
                  rdenom += tempc;
                  idenom += temps;
               }
               /* PSD estimate for tapped and lattice ar coefficients */
               psde[freq] = (float) (1.0/(rdenom * rdenom + idenom * idenom));
            }
        }

        /* PSD or Phase estimate */
        if ( !estimate )
        {
           psde[freq] = 0.0;
        }
    }
    else 
    {
       stages *= 2;
       for ( freq = 0; freq < newfreq; freq++ )
       {
           /* PSD estimate for tapped and lattice ar coefficients */
           if ( !arfrom )
           {
              omega = TWOPI * (float) freq / (float) newfreq;
              rdenom = arpar[0];
              idenom = arpar[1];
              for ( k = 2, sta = 1; k < stages; k += 2, sta++ )
              {
                  ang = (double) (-omega * (float)sta);
                  real = (float) cos(ang);
                  imag = (float) sin(ang);
                  cmul ( &tempc, &temps, real, imag, 
                         arpar[k], arpar[k+1] );
                  cadd ( &rdenom, &idenom, rdenom, idenom, tempc, temps );
               }
              if(estimate)
                 mag = (float) cmagsq ( rdenom, idenom );
              else
                 mag = (float) cang(rdenom,idenom);
              psde[freq] = mag; 
           }
           else
           {
              omega = TWOPI * (float) freq / (float) newfreq;
              rdenom = 1;
              idenom = 0;
              for ( k = 0, sta = 1; k < stages; k += 2, sta++ )
              {
                  ang = (double) (-omega * (float)sta);
                  real = (float) cos(ang);
                  imag = (float) sin(ang);
                  cmul ( &tempc, &temps, real, imag, 
                         arpar[k], -arpar[k+1] );
                  cadd ( &rdenom, &idenom, rdenom, idenom, tempc, temps );
               }
               if(estimate){
                  mag = (float) cmagsq ( rdenom, idenom );
                  psde[freq] = (float) ( 1.0 / mag );
               }else{
                  mag = (float) cang(rdenom,idenom);
                  psde[freq] = -mag; 
               } 
            }
        }
    }

   /* normalize psd */
    if ( normalize == 1 && estimate) {
       maxw = psde[0];
       for (k = 1; k < newfreq; k++)
       {
           if (psde[k] > maxw) 
           {  
              maxw = psde[k];
           }
       }
       for (k = 0; k < newfreq; k++)
       {
           psde[k] /=  maxw;
       }
    }

    return(1);
} /* end of larpsde */

/* -library_code_end */
