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

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

 /*
  * $Log: ldxcorr.c,v $
 * Revision 1.2  1992/03/20  23:29:29  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: ldxcorr.c
 >>>>
 >>>>      Program Name: dxcorr
 >>>>
 >>>> Date Last Updated: Thu Mar  5 12:06:58 1992 
 >>>>
 >>>>          Routines: ldxcorr - the library call for dxcorr
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
/* the cleanup routines free up all allocated memory prior to a return */
#define LDX_CLEANUP  { \
                      if(data1!=NULL){ \
                         for(i=0;i<num_vects1;i++) \
                             if(data1[i]!=NULL)free(data1[i]); \
                         free(data1); \
                      } \
                      if(data2!=NULL){ \
                         for(i=0;i<num_vects2;i++) \
                             if(data2[i]!=NULL)free(data2[i]); \
                         free(data2); \
                      }}
#define LDTX_CLEANUP {if(autor!=NULL)free(autor); \
                      if(autoi!=NULL)free(autoi);}
#define LDFX_CLEANUP {if(d1r!=NULL)free(d1r); \
                      if(d1i!=NULL)free(d1i); \
                      if(d2r!=NULL)free(d2r); \
                      if(d2i!=NULL)free(d2i); }

/* the following two defines implement a log2() function */
#define MAGIC 3.32192810
#define LOG2(x) (log10((double)x)*(double)MAGIC)

#define FFT_FORWARD 0
#define FFT_INVERSE 1

static void _lfxcorr();
/* -library_includes_end */


/****************************************************************
*
* Routine Name: ldxcorr - library call for dxcorr
*
* Purpose:
*    
*    determines a cross-correlation estimate  between  two  input
*    data sequences.
*    
*    

* Input:
*    
*    image1 (struct xvimage) - pointer  to  the  input/output  xvimage
*    structure.
*    image2 (struct xvimage) - pointer  to  the  input/output  xvimage
*    structure.
*    arith_type - 0 = scalar, 1 = complex.
*    out_option - 0 = biased, 1 = unbiased, 2 = FFT based.
*    points - length of cross-correlation estimate.
*    process_dir (int) - data processing direction.  0 = vector,  1  =
*    band.
*    
*    

* Output:
*    
*    image (struct xvimage) -  pointer  to  the  input/output  xvimage
*    structure.
*    
*    

*
* Written By: Ramiro Jordan, Jeremy Worley
*    
*    Jeremy Worley 24 Feb 1992 23:15 MST
*              changed call to lfft1d in ldfxcorr() to fft842_() since
*              lfft1d  is  being  phased  out.   Also  cleaned up some
*              implicitly defined functions and modularized ldfxcorr()
*              which  resulted  in  the  creation of a static function
*              called _lfxcorr().  Also fixed a small scaling  problem
*              with the output of ldfxcorr().
*    
*    Jeremy Worley 05 Mar 1992 12:05 MST
*              Fixed a call  to  fprintf()  that  was  missing  format
*              specifiers.   Added explicit declarations of ldfxcorr()
*              and ldtxcorr().
*    
*    

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


/* -library_def */
int
ldxcorr ( image1, image2, arith_type, out_option, xpoints, process_dir )

int out_option, arith_type, xpoints, process_dir;
struct xvimage *image1, *image2;
/* -library_def_end */

/* -library_code */
{
    int   i, data_type, dimension1, dimension2;
    int   num_vects1, num_vects2, points;
    int   dunload_vector(), lscale(), ldfxcorr(),ldtxcorr();
    float **data1, **data2;
    char  **dload_vector();

    char *program = "ldxcorr";

    /* initialize parameters */
    points = xpoints * 2 - 1; /* number of (two-sided) cross-corr.points */

    /* determine data type of input files */
    if ( image1->data_storage_type == VFF_TYP_COMPLEX &&
         image2->data_storage_type == VFF_TYP_COMPLEX )
    {
       data_type = 1;
    }
    else if (image1->data_storage_type == VFF_TYP_FLOAT &&
         image2->data_storage_type == VFF_TYP_FLOAT )
    {
       data_type = 0;
    } 
    else 
    {
       fprintf(stderr,"%s:  image data types must be of the same type.\n",
                program);
       return(0);
    }

    /* down load vector(s) of input file 1 */
    if((data1 = (float **)dload_vector(image1, &num_vects1, &dimension1, process_dir)) == NULL)
    {
      (void) fprintf(stderr,"%s: dload_vector failed \n",program);
      return(0);
    }

    /* down load vector(s) of input file 2 */
    if((data2 = (float **)dload_vector(image2, &num_vects2, &dimension2, process_dir)) == NULL)
    {
      (void) fprintf(stderr,"%s: dload_vector failed \n",program);
      LDX_CLEANUP;
      return(0);
    }

    /* verify that both images have the same number of vectors */
    if(num_vects1!=num_vects2)
    {
          (void)fprintf(stderr,"%s:\n\n:  ",program);
          (void)fprintf(stderr,"number of vectors in each file");
          (void)fprintf(stderr," must be the same.\n");
          LDX_CLEANUP;
          return(0);
    }

    /* verify if both input files have the same length */
    if ( dimension1 != dimension2 )
    {
          (void) fprintf (stderr, "\n\n%s:   ", program);
          (void) fprintf (stderr, "length of each file");
          (void) fprintf (stderr, " must be the same\n\n");
          LDX_CLEANUP;
          return (0);
    }

    /* verify cross-correlation points */
    if ( points > dimension1 )
    {
          (void) fprintf (stderr, "\n\n%s:   ", program);
          (void) fprintf (stderr, "number of cross-correlation points ");
          (void) fprintf (stderr, "cannot be greater than half of the ");
          (void) fprintf (stderr, "total number of input data points\n\n");
          LDX_CLEANUP;
          return (0);
    }

    /* find the cross-correlation sequence */
    if ( out_option == 2 )              
    {
       for ( i = 0; i < num_vects1; i++ )
       {
           if ( !ldfxcorr ( data1[i], data2[i], dimension1, points, data_type, arith_type ) )
           {
              (void) fprintf ( stderr,"ldfxcorr failed\n");
              LDX_CLEANUP;
              return(0);
           }
       }
    }
    else 
    {
       for ( i = 0; i < num_vects1; i++ )
       {
           if ( !ldtxcorr ( data1[i], data2[i], dimension1, points, data_type, out_option, arith_type ) )
           {
              (void) fprintf ( stderr,"ldtxcorr failed\n");
              LDX_CLEANUP;
              return(0);
           }
       }
    }

    /* up load vector(s) of result */
    if (!dunload_vector((char **)data1, image1, image1->data_storage_type, num_vects1, points, process_dir))
    {
       (void) fprintf (stderr,"%s: dunload_vector failed \n",program);
       LDX_CLEANUP;
       return(0);
    }

    LDX_CLEANUP;
    return(1);

} /* end of ldxcorr */


/*
*****************************************************************************
*
*       data1:          input1 - input data points
*                       output - cross-correlation sequence
*
*       data2:          input2 - input data points
*
*       xpoints:        number of points of desired cross-correlation
*                       sequence (two sided).
*
*       dpoints:        number of points of input data sequences 
*
*       data_type:      0 - real data
*                       1 - complex data
*
*       out_option:     0 - biased cross-correlation estimate
*                       1 - unbiased cross-correlation estimate
*
*       arith_type:     0 - scalar arithmetic 
*                       1 - vector arithmetic
*
*****************************************************************************
*/

int
ldtxcorr ( data1, data2, dpoints, xpoints, data_type, out_option, arith_type )

int out_option, arith_type, xpoints, dpoints, data_type;
float *data1, *data2;
{
    int   i, j, lag, off, length, length2;    
    int   cadd(), cmul();
    float *autor, *autoi=NULL, real, imag, tempr, tempi;

    char *program = "ldtxcorr";

    /* initialize parameters */
    length = (xpoints + 1) / 2;
    length2 = length * 2;

    if ( xpoints > dpoints )
    {
          (void) fprintf (stderr, "\n\n%s:   ", program);
          (void) fprintf (stderr, "number of cross-correlation points cannot");
          (void) fprintf (stderr, "be greater than number of data points\n\n");
          return (0);
    }

   /* allocate space for working arrays */
    autor = (float *) malloc((unsigned int) (xpoints) * sizeof(float));
    if(autor==NULL){
       fprintf(stderr,"%s: [1]memory allocation failed.\n",program);
       return(0);
    }

    if ( data_type!=0 ) {
       autoi = (float *) malloc((unsigned int) (xpoints) * sizeof(float));
       if(autoi==NULL){
          fprintf(stderr,"%s: [2]memory allocation failed.\n",program);
          LDTX_CLEANUP;
          return(0);
       }
    }

    /* find the cross-correlation estimate */
    if ( !data_type )                   /* real data */
    {
       /* calculate negative cross correlation points */
       off = length - 2;
       for ( i = 0, lag = 1; lag < length; lag++, i++ )
       {
           tempr = 0.0;
           for ( j = lag; j < dpoints; j++ )
           {
               tempr = tempr + data1[j] * data2[j-lag];
           }

           if ( !out_option )
           {
              autor[off-i] = tempr / (float) (dpoints);
           }
           else if ( out_option == 1 )
           {
              autor[off-i] = tempr / (float) (dpoints-lag);
           }
       } 

       /* calculate positive cross correlation points */
       off += 1;
       for ( lag = 0; lag < length; lag++ )
       {
           tempr = 0.0;
           for ( j = 0; j < dpoints-lag; j++ )
           {   
               tempr = tempr + data1[j] * data2[lag+j];
           }
           if ( !out_option )
           {  
              autor[off+lag] = tempr / (float) (dpoints);
           }
           else if ( out_option == 1 )
           {
              autor[off+lag] = tempr / (float) (dpoints-lag);
           }
       }
       bcopy ( (char *)autor, (char *)data1, (int)xpoints * sizeof(float) );
    } 
    else if ( data_type == 1 )          /* complex data */
    {
       off = length - 2;
       xpoints *= 2;
       if ( !arith_type )               /* scalar arithmetic */
       {
          for ( i = 0, lag = 2; lag < length2; lag += 2, i++ )
          {
              tempr = 0.0;
              tempi = 0.0;
              for ( j = lag; j < dpoints*2; j += 2 )
              {  
                  tempr = tempr + data1[j] * data2[j-lag];
                  tempi = tempi + data1[j+1] * data2[j+1-lag];
              }
              if ( !out_option )
              {  
                 autor[off-i] = tempr / (float) (dpoints);
                 autoi[off-i] = tempi / (float) (dpoints);
              }
              else if ( out_option == 1 )
              {
                 autor[off-i] = tempr / (float) (dpoints-i);
                 autoi[off-i] = tempi / (float) (dpoints-i);
              }
          }
          off += 1; 
          for ( i = 0, lag = 0; lag < length2; lag += 2, i++ )
          {
              tempr = 0.0;
              tempi = 0.0;
              for ( j = 0; j < 2*dpoints-lag; j += 2 )
              {   
                  tempr = tempr + data1[j] * data2[lag+j];
                  tempi = tempi + data1[j+1] * data2[lag+j+1]; 
              }
              if ( !out_option )
              {  
                 autor[off+i] = tempr / (float) (dpoints);
                 autoi[off+i] = tempi / (float) (dpoints);
              }
              else if ( out_option == 1 )
              {
                 autor[off+i] = tempr / (float) (dpoints-i);
                 autoi[off+i] = tempi / (float) (dpoints-i);
              }
          }
          for ( i = 0, lag = 0; lag < xpoints; lag += 2, i++ )
          {
              data1[lag] = autor[i];
              data1[lag+1] = autoi[i];
          }
       } 
       else if ( arith_type == 1 )      /* vector arithmetic */
       {
          for ( i = 0, lag = 2; lag < length2; lag += 2, i++ )
          {
              real = 0.0;
              imag = 0.0;
              tempr = 0.0;
              tempi = 0.0;
              for ( j = lag; j < dpoints*2; j += 2 )
              {
                  cmul ( &tempr, &tempi, data1[j], data1[j+1],
                         data2[j-lag], -data2[j+1-lag] );
                  cadd ( &real, &imag, real, imag, tempr, tempi );
              }
              if ( !out_option )
              {
                 autor[off-i] = real / (float) (dpoints);
                 autoi[off-i] = imag / (float) (dpoints);
              }
              else if ( out_option == 1 )
              {
                 autor[off-i] = real / (float) (dpoints-i);
                 autoi[off-i] = imag / (float) (dpoints-i);
              }
          }
          off += 1; 
          for ( i = 0, lag = 0; lag < length2; lag += 2, i++ )
          {
              real = 0.0;
              imag = 0.0;
              tempr = 0.0;
              tempi = 0.0;
              for ( j = 0; j < 2*dpoints-lag; j += 2 )
              {
                  cmul ( &tempr, &tempi, data1[j], data1[j+1],
                         data2[lag+j], -data2[lag+j+1] );
                  cadd ( &real, &imag, real, imag, tempr, tempi );
              }
              if ( !out_option )
              {
                 autor[off+i] = real / (float) (dpoints);
                 autoi[off+i] = imag / (float) (dpoints);
              }
              else if ( out_option == 1 )
              {
                 autor[off+i] = real / (float) (dpoints-i);
                 autoi[off+i] = imag / (float) (dpoints-i);
              }
          }
          for ( i = 0, lag = 0; lag < xpoints; lag += 2, i++ )
          {
              data1[lag] = autor[i];
              data1[lag+1] = autoi[i];
          }
       } 
    } 

    LDTX_CLEANUP;
    return(1);

} /* end of ldtxcorr */



/*
*****************************************************************************
*
*       data1:          input1 - input data points
*                       output - cross-correlation sequence
*
*       data2:          input2 - input data points
*
*       xpoints:        number of points of desired autocorrelation
*                       sequence
*
*       dpoints:        number of points of input data sequence 
*
*       data_type:      0 - real data
*                       1 - complex data
*
*       arith_type:     0 - scalar arithmetic 
*                       1 - vector arithmetic
*
*****************************************************************************
*/

int
ldfxcorr ( data1, data2, dpoints, xpoints, data_type, arith_type )

int arith_type, xpoints, dpoints, data_type;
float *data1, *data2;
{
    int i, j, length, dpoints2;    
    int powtwo();
    int cmul();
    float *d1r, *d1i, *d2r, *d2i;

    char *program = "ldfxcorr";

/*
 * initialize parameters 
 */

    dpoints2 = dpoints * 2;

/* 
 * force length to be a power of two 
 */

    length = dpoints;
    if ( !powtwo (length) )
    {
      int temp;
      temp = (int)(LOG2(length)+1);
      length = (int) (pow((double)2,(double)temp));
    }

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

    if((d1i = (float *)malloc((unsigned)length * sizeof(float)))==NULL){
       fprintf(stderr,"%s: [2]memory allocation failed.\n",program);
       return(0);
    }

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

    if((d2i = (float *)malloc((unsigned)length * sizeof(float)))==NULL){
       fprintf(stderr,"%s: [2]memory allocation failed.\n",program);
       return(0);
    }

/* 
 * convert a real data array into a complex data array 
 * make sure the imaginary part of each number is zero 
 * take the forward FFT, multiply the two sequences together and 
 * finally determine the inverse FFT (xcorrlution sequence) 
 */

    if ( !data_type ){
       for(i=0;i<dpoints;i++){
           d1r[i] = data1[i];
           d2r[i] = data2[i];
           d1i[i] = d2i[i] = 0.0;
       }

       (void)_lfxcorr(d1r,d1i,d2r,d2i,length,dpoints,xpoints);

       (void)bcopy(d1r,data1,dpoints*sizeof(float));

/*
 * complex data
 */

    }else{
       if(!arith_type){

          /*
           * do the real part first
           */

          for (i=0,j=0; i < dpoints2;i+=2,j++ ) {
              d1r[j] =  data1[i];
              d2r[j] =  data2[i];
              d1i[j] =  d2i[j] = 0.0;
          }

          (void)_lfxcorr(d1r,d1i,d2r,d2i,length,dpoints,xpoints);

         /*
          * copy dr back into datain real part and prepare dr,di for
          * imaginary part of calculation
          */

          for (i=0,j=0; i < dpoints2;i+=2,j++ ) {
              data1[i] = d1r[j];
              d1r[j] =  data1[i+1];
              d2r[j] =  data2[i+1];
              d1i[j] =  d2i[j] = 0.0;
          }

          (void)_lfxcorr(d1r,d1i,d2r,d2i,length,dpoints,xpoints);

          for(i=0,j=0;i<dpoints2;i+=2,j++) data1[i+1] = d1r[j];

       } else {         /* vector arithmetic */
          for (i=0,j=0; i < dpoints2;i+=2,j++ ) {
              d1r[j] =  data1[i];
              d1i[j] =  data1[i+1];
              d2r[j] =  data2[i];
              d2i[j] =  data2[i+1];
          }

          (void)_lfxcorr(d1r,d1i,d2r,d2i,length,dpoints,xpoints);

         /*
          * copy dr back into datain real part and zero imaginary
          * part
          */

          for (i=0,j=0; i < dpoints2;i+=2,j++ ) {
              data1[i] = d1r[j];
              data1[i+1] = d1i[j];
          }

       }
    }

    LDFX_CLEANUP; 
    return(1);

} /* end of ldfxcorr */

/***********************************************************************
*
*  Routine Name: static void _lfxcorr()
*
*          Date: Mon Feb 17 08:51:21 MST 1992
*
*       Purpose: performs time domain cross correlation function in the
*                frequency domain.
*
*         Input: float *d1r   - real part of first sequence
*                float *d1i   - imaginary part of first sequence
*                float *d2r   - real part of second sequence
*                float *d2i   - imaginary part of second sequence
*                int points  - number of points in input sequence (must
*                              be a power of two)
*                int dpoints - number of points in original data sequence
*                              (need not be a power of two)
*                int xpoints - number of cross correlation points
*                              desired.
*
*        Output: returns dr as the autocorrelation sequence.
*
*    Written By:  Jeremy Worley
*
* Modifications:
*
***********************************************************************/

static void _lfxcorr(d1r,d1i,d2r,d2i,points,dpoints,xpoints)
  float *d1r, *d1i,*d2r,*d2i;
  int points,dpoints,xpoints;
{
  int direction,i;
  float tempr,tempi;
  int cdiv(),cmul();
  void fft842_();

    /*
     * take FFT of data set
     */

    direction = FFT_FORWARD;
    (void) fft842_ ( &direction, &points,d1r,d1i);
    (void) fft842_ ( &direction, &points,d2r,d2i);

    for (i=0;i<points;i++) {
        cmul (&tempr,&tempi,d1r[i],d1i[i],d2r[i],d2i[i]);
        d1r[i] = tempr;
        d1i[i] = tempi;
    }

    direction = FFT_INVERSE;
    (void) fft842_ ( &direction, &points,d1r,d1i);

   /*
    * scale by number of data points
    */

    for (i = 0; i < xpoints; i++ ) {
        d1r[i] /= (float) dpoints;
        d1i[i] /= (float) dpoints;
    }

    return;
} /* end of _lfxcorr() */

/* -library_code_end */
