/* -*- Mode: C; indent-tabs-mode: nil; c-basic-offset: 4 c-style: "K&R" -*- */

/*
   libgpiv - library for Particle Image Velocimetry

   Copyright (C) 2002, 2003, 2004, 2005, 2006, 2007, 2008
   Gerber van der Graaf

   This file is part of libgpiv.

   Libgpiv is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software Foundation,
   Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.



------------------------------------------------------------------------------
FILENAME:                piv.c
LIBRARY:                 libgpiv
EXTERNAL FUNCTIONS:
                         gpiv_piv_count_pivdata_fromimage
			 gpiv_piv_select_int_point
                         gpiv_piv_interrogate_img
                         gpiv_piv_interrogate_ia
                         gpiv_piv_isizadapt
                         gpiv_piv_write_deformed_image
			 gpiv_piv_weight_kernel_1
			 gpiv_piv_weight_kernel_lin
			 gpiv_fread_fftw_wisdom
			 gpiv_fwrite_fftw_wisdom


LAST MODIFICATION DATE:  $Id: piv.c,v 1.5 2008-09-25 13:19:53 gerber Exp $
---------------------------------------------------------------------------- */

#include <gpiv.h>
#undef USE_MTRACE
#ifdef USE_MTRACE
#include <mcheck.h>
#endif
#include "my_utils.h"

#define FFTWISDOM "gpiv_fftwis"	/* filename containg a string of wisdom for fft */
#define FFTWISDOM_INV "gpiv_fftwis_inv"	/* filename containg a string of wisdom for inverse fft */



/*-------------------------------------------------------------------------
 Levenbergh-Margardt parameter estimation constants */
/* #define MA 4 */			/* Number of parameters to be estimated by
				   marquardt fit of cov peak */
/* #define NPT 9 */			/* Numbers of points to be fitted by mrqmin */
/* #define SPREAD 0.001 */		/* multiplication factor for individual
				   variances in measuremnts, i.e. covariance
				   peak values */
/* #undef NPT */
/* #undef SPREAD */



#define NMIN_TO_INTERPOLATE 2

enum Position {
    LOWER,
    NEXT_LOWER,
    HIGHER,
    NEXT_HIGHER
};

enum HorizontalPosition {
    WEST,
    WEST_WEST,
    EAST,
    EAST_EAST
};

enum VerticalPosition {
    NORTH,
    NORTH_NORTH,
    SOUTH, 
    SOUTH_SOUTH, 
};



/*
 * Local functions prototypes
 */

static GpivPivData *
alloc_pivdata_gridgen (const GpivImagePar *image_par, 
                       const GpivPivPar *piv_par
                       );

static void 
report_progress (gint *progress_old,
                 gint index_y,
                 gint index_x,
                 GpivPivData *piv_data,
                 GpivPivPar *piv_par,
                 gint sweep,
                 gfloat cum_residu
                 );

static gboolean
assign_img2intarr (gint ipoint_x,
                   gint ipoint_y,
                   guint16 **img_1,
                   guint16 **img_2,
                   gint int_size_f,
                   gint int_size_i,
                   gfloat **int_area_1,
                   gfloat **int_area_2,
                   gint pre_shift_row,
                   gint pre_shift_col,
                   gint nrows,
                   gint ncolumns,
                   gint int_size_0
                   );

static gboolean
assign_img2intarr_central (gint ipoint_x,
                           gint ipoint_y,
                           guint16 **img_1,
                           guint16 **img_2,
                           gint int_size_f,
                           gint int_size_i,
                           gfloat **int_area_1,
                           gfloat **int_area_2,
                           gint pre_shift_row,
                           gint pre_shift_col,
                           gint nrows,
                           gint ncolumns,
                           gint int_size_0
                           );

static gboolean
assign_img2intarr_forward (gint ipoint_x,
                           gint  ipoint_y,
                           guint16 **img_1,
                           guint16 **img_2,
                           gint int_size_f,
                           gint int_size_i,
                           gfloat **int_area_1,
                           gfloat **int_area_2,
                           gint pre_shift_row,
                           gint pre_shift_col,
                           gint nrows,
                           gint ncolumns,
                           gint int_size_0
                           );

static float
int_mean_old (guint16 **img,
              int int_size,
              int int_size_i,
              int ipoint_x,
              int ipoint_y
              );

static gfloat
int_mean (gfloat **int_area,
          gint int_size
          );

static gfloat
int_range (gfloat **int_area,
           gint int_size
           );

static gboolean
int_const (gfloat **int_area,
           const guint int_size
           );

static void
cov_min_max (GpivCov *cov
             );

static void
search_top (GpivCov *cov,
            gint peak_act,
            gint x_corr,
            gint sweep,
            gint i_skip_act,
            gint j_skip_act,
            float *z_max,
            long *i_max,
            long *j_max
            );

static char *
cov_subtop (float **z,
            long *i_max,
            long *j_max,
            float *epsi_x,
            float *epsi_y,
            int ifit,
            int peak_act
            );

static int
cov_top (GpivPivPar piv_par,
         GpivPivData * piv_data,
         int index_y,
         int index_x,
         GpivCov *cov,
         int x_corr,
         int ifit,
         int sweep,
         int last_sweep,
         int peak,
         int peak_act,
         int pre_shift_row_act,
         int pre_shift_col_act,
         int i_skip_act,
         int j_skip_act,
         gboolean *flag_subtop
         );

static
void pack_cov (float **covariance,
               GpivCov *cov,
               int int_size_0
               );

static void
piv_weight_kernel_lin (const guint int_size_0,
                       GpivCov *w_k
                       );

static void
weight_cov (GpivCov *cov,
            GpivCov *w_k
            );

static gchar *
filter_cov_spof (fftw_complex *a, 
                 fftw_complex *b,
                 gint m,
                 gint n
                 );

static gchar *
cova (int int_size,
      float **int_area_1,
      float **int_area_2,
      GpivCov *cov,
      gboolean spof_filter
      );

static gchar *
ia_weight_gauss (gint int_size, 
                 float **int_area
                 );

/*
 * Origined from piv_speed
 */
static void
nearest_point (gint i,
               gfloat x, 
               gfloat point_x, 
               gfloat *min, 
               gint *index, 
               gboolean *exist
               );

static gboolean
nearest_index (enum Position pos,
               gint vlength,
               gfloat *src_point, 
               gfloat dest_point,
               gint *index
               );

static gdouble
bilinear_interpol (gdouble alpha_hor,
                   gdouble alpha_vert,
                   gdouble src_dx_nw,
                   gdouble src_dx_ne,
                   gdouble src_dx_sw,
                   gdouble src_dx_se
                   );

static void *
intpol_facts (gfloat *src_point, 
              gfloat *dest_point, 
              gint src_vlength,
              gint dest_vlength,
              gint *index_l,
              gint *index_h,
              gint *index_ll,
              gint *index_hh,
              double *alpha
              );

static void
dxdy_at_new_grid_block (const GpivPivData *piv_data_src, 
                        GpivPivData *piv_data_dest,
                        gint expansion_factor,
                        gint smooth_window
                        );

static gchar *
update_pivdata_imgdeform_zoff (const GpivImage *image, 
                               GpivImage *lo_image, 
                               const GpivPivPar *piv_par, 
                               const GpivValidPar *valid_par, 
                               GpivPivData *piv_data, 
                               GpivPivData *lo_piv_data, 
                               gfloat *cum_residu, 
                               gboolean *cum_residu_reached,
                               gfloat *sum_dxdy, 
                               gfloat *sum_dxdy_old,
                               gboolean isi_last,
                               gboolean grid_last,
                               gboolean sweep_last,
                               gboolean verbose
                               );

/*
 * Some MPI routines
 */
#ifdef ENABLE_MPI
static GpivPivData *
piv_interrogate_img__scatterv_pivdata (GpivPivData *piv_data);

static GpivPivData *
piv_interrogate_img__gatherv_pivdata(GpivPivData *lo_piv_data, 
                                     GpivPivData *piv_data);

guint
substr_noremain(guint n, 
                guint m);

#endif /* ENABLE_MPI */

/*
 * Public functions
 */

gchar *
gpiv_piv_count_pivdata_fromimage (const GpivImagePar *image_par,
                                  const GpivPivPar *piv_par,
                                  guint *nx,
                                  guint *ny
                                  )
/*-----------------------------------------------------------------------------
 *     Calculates the number of interrogation areas from the image sizes,
 *     pre-shift and area of interest
 *     NULL on success or error message on failure
 *---------------------------------------------------------------------------*/
{
    gchar *err_msg = NULL;
    int row, column, row_1, column_1,
	pre_shift_row_max, pre_shift_col_max, count_x = 0, count_y = 0;
    int row_max, row_min, column_max, column_min;

    int ncolumns = image_par->ncolumns;
    int nrows = image_par->nrows;

    int int_geo = piv_par->int_geo;
    int row_start = piv_par->row_start;
    int row_end = piv_par->row_end;
    int col_start = piv_par->col_start;
    int col_end = piv_par->col_end;
    int int_line_col = piv_par->int_line_col;
    int int_line_col_start = piv_par->int_line_col_start;
    int int_line_col_end = piv_par->int_line_col_end;
    int int_line_row = piv_par->int_line_row;
    int int_line_row_start = piv_par->int_line_row_start;
    int int_line_row_end = piv_par->int_line_row_end;
    int int_point_col = piv_par->int_point_col;
    int int_point_row = piv_par->int_point_row;
    int int_size_f = piv_par->int_size_f;
    int int_size_i = piv_par->int_size_i;
    int int_shift = piv_par->int_shift;
    int pre_shift_row = piv_par->pre_shift_row;
    int pre_shift_col = piv_par->pre_shift_col;


    *nx = 0;
    *ny = 0;


    row_min = gpiv_min(-int_size_f / 2 + 1,
                       pre_shift_row - int_size_i / 2 + 1);
    column_min = gpiv_max(-int_size_f / 2 + 1,
                          pre_shift_col - int_size_i / 2 + 1);
    row_max = gpiv_max(int_size_f / 2, pre_shift_row + int_size_i / 2);
    column_max = gpiv_max(int_size_f / 2, pre_shift_col + int_size_i / 2);


    if (int_geo == GPIV_POINT) {
        *nx = 1;
        *ny = 1;


/*
 * Counts number of Interrrogation Area for a single row
 */
    } else if (int_geo == GPIV_LINE_R) {
        if ((int_size_f - int_size_i) / 2 + pre_shift_col < 0) {
            column_1 = int_line_col_start -
                ((int_size_f - int_size_i) / 2 +
                 pre_shift_col) + int_size_f / 2 - 1;
        } else {
                column_1 = int_line_col_start + int_size_f / 2 - 1;
        }

        for (column = column_1; column <= int_line_col_end - column_max;
             column += int_shift) {
            count_x++;
        }

        *nx = count_x;
        *ny = 1;

/*
 * Counts number of Interrrogation Area for a single column
 */
    } else if (int_geo == GPIV_LINE_C) {
        if ((int_size_f - int_size_i) / 2 + pre_shift_row < 0) {
            row_1 = int_line_row_start -
                ((int_size_f - int_size_i) / 2 +
                     pre_shift_row) + int_size_f / 2 - 1;
        } else {
            row_1 = int_line_row_start + int_size_f / 2 - 1;
        }

        for (row = row_1; row <= int_line_row_end - row_max;
             row += int_shift) {
            count_y++;
        }

        *ny = count_y;
        *nx = 1;


/*
 * Counts number of Interrrogation Area for a Area Of Interest
 */
    } else if (int_geo == GPIV_AOI) {
	if ((int_size_f - int_size_i) / 2 + pre_shift_row < 0) {
	    row_1 =
		row_start - ((int_size_f - int_size_i) / 2 +
			     pre_shift_row) + int_size_f / 2 - 1;
	} else {
	    row_1 = row_start + int_size_f / 2 - 1;
	}
	if ((int_size_f - int_size_i) / 2 + pre_shift_col < 0) {
	    column_1 =
		col_start - ((int_size_f - int_size_i) / 2 +
			     pre_shift_col) + int_size_f / 2 - 1;
	} else {
	    column_1 = col_start + int_size_f / 2 - 1;
	}


	pre_shift_col_max = gpiv_max (pre_shift_col, 0);
	column_max =
	    gpiv_max(int_size_f / 2, pre_shift_col + int_size_i / 2);
	pre_shift_row_max = gpiv_max (pre_shift_row, 0);
	row_max = gpiv_max (int_size_f / 2, pre_shift_row + int_size_i / 2);


	for (row = row_1; row + row_max <= row_end; row += int_shift) {
	    for (column = column_1; column + column_max <= col_end;
		 column += int_shift) {
		count_x++;
	    }
	    if (count_x > *nx)
		*nx = count_x;
	    count_x = 0;
	    count_y++;
	}
	if (count_y > *ny)
	  *ny = count_y;
    } else {
        err_msg = "gpiv_piv_count_pivdata_fromimage: should not arrive here";
        gpiv_warning ("%s", err_msg);
        return err_msg;
    }

    if (*nx == 0 || *ny == 0) {
        err_msg = "gpiv_piv_count_pivdata_fromimage: line or AOI too small: nx=0 ny=0";
        gpiv_warning("gpiv_piv_count_pivdata_fromimage: line or AOI too small: nx = %d ny = %d\n", 
                     *nx, *ny);
        return err_msg;
    }

#ifdef DEBUG
    g_message ("gpiv_piv_count_pivdata_fromimage:: 2 nx = %d, ny = %d", *nx, *ny);
#endif
    return err_msg;
}



GpivPivData *
gpiv_piv_interrogate_img (const GpivImage *image,
                          const GpivPivPar *piv_par,
                          const GpivValidPar *valid_par,
                          const gboolean verbose
                          )
/* ----------------------------------------------------------------------------
 * PIV interrogation of an image pair at an entire grid or single point
 *
 *     @param[in] image           image containing data and header info
 *     @param[in] piv_par         image evaluation parameters
 *     @param[in] valid_par       structure of PIV data validation parameters
 *     @param[out] verbose        prints progress of interrogation to stdout
 *     @return                    GpivPivData containing PIV estimators on succes
 *                                or NULL on failure
 */
/*---------------------------------------------------------------------------*/
{
    GpivPivData *piv_data = NULL;       /* piv data to be returned */
    gchar *err_msg = NULL;              /* error message */
    guint index_x = 0, index_y = 0;     /* array indices */

    /*
     * Local variables with prefix lo_ to distinguish from global or from 
     * parameter list
     */
    GpivImage *lo_image = NULL;         /* local image that might be deformed */
    GpivPivData *lo_piv_data = NULL;    /* local piv data */
    GpivPivPar *lo_piv_par = NULL;      /* local piv parameters */
    
    gfloat **intreg1;                   /* first interrogation area */
    gfloat **intreg2;                   /* second interrogation area */
    guint int_size_0;                   /* zero-padded interrogation area size */

    GpivCov *cov = NULL;                /* covariance */
    guint sweep = 1;                    /* itaration counter */
    gboolean grid_last = FALSE;         /* flag if final grid refinement has been
                                           reached */
    gboolean isi_last = FALSE;          /* flag if final interrogation area shift
                                           has been reached */
    gboolean cum_residu_reached = FALSE;/* flag if max. cumulative residu has 
                                           been reached */
    gboolean sweep_last = FALSE;        /* perform the last iteration sweep */
    gboolean sweep_stop = FALSE;        /* stop the current iteration at the end */
    gfloat sum_dxdy = 0.0, sum_dxdy_old = 0.0;  /* */
    gfloat cum_residu = 914.6;          /* initial, large, arbitrary cumulative 
                                           residu */
    guint progress_old = 0;             /* for monitoring calculation progress */

#ifdef ENABLE_MPI
    GpivPivData *mpi_piv_data = NULL;  /* received / gathered piv data to be used for 
                                          parallel processing */
    gint nprocs, rank, count = 0;
    gboolean scatter;
    gint i, *counts = NULL, *displs = NULL;
#endif

    /*     initvars_interrogate_img(); */
    if (verbose) printf ("\n");

    /*
     * Testing parameters on consistency and initializing derived 
     * parameters/variables
     */
    if ((err_msg = 
         gpiv_piv_testonly_parameters (image->header, piv_par))
        != NULL) {
        gpiv_warning ("gpiv_piv_interrogate_img: %s", err_msg);
        return NULL;
    }

    if ((err_msg = 
         gpiv_valid_testonly_parameters (valid_par))
        != NULL) {
        gpiv_warning ("gpiv_piv_interrogate_img: %s", err_msg);
        return NULL;
    }

    /*
     * Local (actualized) parameters
     * Setting initial parameters and variables for adaptive grid and 
     * Interrogation Area dimensions
     */
    lo_piv_par = gpiv_piv_cp_parameters (piv_par);

    if (lo_piv_par->int_scheme == GPIV_ZERO_OFF_FORWARD
        || lo_piv_par->int_scheme == GPIV_ZERO_OFF_CENTRAL
	|| lo_piv_par->int_scheme == GPIV_IMG_DEFORM
        || lo_piv_par->int_size_i > lo_piv_par->int_size_f) {
        lo_piv_par->int_size_f = lo_piv_par->int_size_i;
        sweep_last = FALSE;
    } else {
        sweep_last = TRUE;
    }
    
    if (lo_piv_par->int_shift < lo_piv_par->int_size_i / GPIV_SHIFT_FACTOR) {
        lo_piv_par->int_shift = lo_piv_par->int_size_i / GPIV_SHIFT_FACTOR;
    }
    
    /*
     * A copy of the image and PIV data are needed when image deformation is used.
     * To keep the algorithm simple (well, what's in the name :), copies are made 
     * unconditionally.
     */
    lo_image = gpiv_cp_img (image);
    piv_data = alloc_pivdata_gridgen (image->header, lo_piv_par);

#ifdef ENABLE_MPI
    MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#endif /* ENABLE_MPI */

    lo_piv_data = gpiv_cp_pivdata (piv_data);

#ifdef DEBUG
    gpiv_write_pivdata(NULL, lo_piv_data, FALSE);
    fflush(stdout);
#endif /* DEBUG */

    gpiv_0_pivdata (lo_piv_data);
    
    /*
     * Reads eventually existing fftw wisdom
     */
    gpiv_fread_fftw_wisdom (1);
    gpiv_fread_fftw_wisdom (-1);

#ifdef _OPENMP
    fftw_init_threads();
#endif

    while (sweep <= GPIV_MAX_PIV_SWEEP 
           && !sweep_stop) {

        /*
         * Memory allocation of interrogation area's and covariance. 
         * These memory chunks are allocated here to optimize calculation 
         * speed and for eventually monitoring their contents.
         */
        int_size_0 = GPIV_ZEROPAD_FACT * lo_piv_par->int_size_i;
	intreg1 = gpiv_matrix (int_size_0, int_size_0);
	intreg2 = gpiv_matrix (int_size_0, int_size_0);
        cov = gpiv_alloc_cov (int_size_0, image->header->x_corr);
        
        /*
         * Interrogates a single interrogation area
         */
	if (lo_piv_par->int_geo == GPIV_POINT) {

            if ((err_msg = 
                 gpiv_piv_interrogate_ia (0, 
                                          0, 
                                          lo_image, 
                                          lo_piv_par,
                                          sweep, 
                                          sweep_last, 
                                          intreg1,
                                          intreg2,
                                          cov,
                                          lo_piv_data
                                          ))
                != NULL) {
                gpiv_free_img (lo_image);
                gpiv_free_pivdata (lo_piv_data);
                gpiv_free_pivdata (piv_data);
 	        gpiv_free_matrix (intreg1);
	        gpiv_free_matrix (intreg2);
                gpiv_free_cov (cov);
                gpiv_warning ("gpiv_piv_interrogate_img: %s", err_msg);
                return NULL;
            }

	} else {
            /*
             * Interrogates at a rectangular grid of points within the Area Of
             * Interest of the image
             */

#ifdef ENABLE_MPI
            /*
             * Scatter the PIV data over the rows to the different nodes.
             */
            lo_piv_data = piv_interrogate_img__scatterv_pivdata(lo_piv_data);
#endif /* ENABLE_MPI */

            for (index_y = 0; index_y < lo_piv_data->ny; index_y++) {
                for (index_x = 0; index_x < lo_piv_data->nx; index_x++) {

                    /*                     if (rank==0) g_message ("gpiv_piv_interrogate_img:: 0e rank=%d i_x=%d i_y=%d",  */
                    /*                                                   rank, index_x, index_y); */
                    /*
                     * Interrogates a single interrogation area.
                     */
                        if ((err_msg = 
                             gpiv_piv_interrogate_ia (index_y, 
                                                      index_x, 
                                                      lo_image,
                                                      lo_piv_par,
                                                      sweep, 
                                                      sweep_last,
                                                      intreg1,
                                                      intreg2,
                                                      cov,
                                                      lo_piv_data
                                                      ))
                            != NULL) {
                            gpiv_free_img (lo_image);
                            gpiv_free_pivdata (lo_piv_data);
                            gpiv_free_pivdata (piv_data);
                            gpiv_free_matrix (intreg1);
                            gpiv_free_matrix (intreg2);
                            gpiv_free_cov (cov);
                            gpiv_warning ("gpiv_piv_interrogate_img: %s", err_msg);
#ifdef ENABLE_MPI
                            MPI_Finalize();
#endif
                            return NULL;
                        }

                        /*
                         * Printing the progress of processing
                         */
                        if (verbose ) {
                            report_progress (&progress_old,
                                             index_y,
                                             index_x, 
                                             lo_piv_data, 
                                             lo_piv_par,
                                             sweep,
                                             cum_residu);
                        }


                    }
                }
#ifdef ENABLE_MPI
            /*
             * Gather the scattered PIV data 
             * and broadcasts the entire array to all nodes.
             */
            lo_piv_data = piv_interrogate_img__gatherv_pivdata(lo_piv_data, 
                                                               piv_data);
            gpiv_piv_mpi_bcast_pivdata (lo_piv_data);
#endif
        }

        /*
         * De-allocating memory: other (smaller) sizes are eventually needed 
         * for a next iteration sweep
         */
        gpiv_free_matrix (intreg1);
        gpiv_free_matrix (intreg2);
        gpiv_free_cov (cov);

        if (sweep_last) {
            sweep_stop = TRUE;
        }

        if (lo_piv_par->int_scheme == GPIV_IMG_DEFORM
            || lo_piv_par->int_scheme == GPIV_ZERO_OFF_FORWARD
            || lo_piv_par->int_scheme == GPIV_ZERO_OFF_CENTRAL) {

            if ((err_msg = 
                 update_pivdata_imgdeform_zoff (image, lo_image, lo_piv_par, 
                                                valid_par, piv_data, lo_piv_data, 
                                                &cum_residu, &cum_residu_reached,
                                                &sum_dxdy, &sum_dxdy_old,
                                                isi_last, grid_last, 
                                                sweep_last, verbose))
                != NULL) {
                g_warning ("gpiv_piv_interrogate_img: %s", err_msg);
                gpiv_free_img (lo_image);
                gpiv_free_pivdata (lo_piv_data);
                gpiv_free_pivdata (piv_data);
                return NULL;
            }

        } else {

            /*
             * Apply results to output piv_data
             */
            gpiv_free_pivdata (piv_data);
            piv_data = gpiv_cp_pivdata (lo_piv_data);
            cum_residu_reached = TRUE;
        }

        /*
         * Adapt grid. 
         * If final grid has been reached, grid_last will be set.
         */
        if (lo_piv_par->int_shift > piv_par->int_shift
            && !sweep_stop) {
            GpivPivData *pd = NULL;

            pd = gpiv_piv_gridadapt (image->header,
                                     piv_par, 
                                     lo_piv_par,
                                     piv_data, 
                                     sweep, 
                                     &grid_last);
            gpiv_free_pivdata (piv_data);
            piv_data = gpiv_cp_pivdata (pd);
            gpiv_free_pivdata (pd);

            gpiv_free_pivdata (lo_piv_data);
            lo_piv_data = gpiv_cp_pivdata (piv_data);

            if (lo_piv_par->int_scheme == GPIV_IMG_DEFORM) {
                gpiv_0_pivdata (lo_piv_data);
            }

        } else {
            grid_last = TRUE;
        }

        /*
         *  Adapt interrogation area size. 
         *  If final size has been reached, isi_last will be set.
         */
        gpiv_piv_isizadapt (piv_par, 
                            lo_piv_par, 
                            &isi_last);
        
        /*
         * Test if all conditions have been reached
         */
        if (cum_residu_reached && isi_last && grid_last) {
            sweep_last = TRUE;
        }

        sweep++;
    }


    /*
     * Writes existing fftw wisdom
     * Cleans up allocated memory
     * and returns resulting PIV data to caller
     */
    gpiv_fwrite_fftw_wisdom (1);
    gpiv_fwrite_fftw_wisdom (-1);
    fftw_forget_wisdom();
#ifdef _OPENMP
    fftw_cleanup_threads();
#else
    fftw_cleanup ();
#endif
    gpiv_free_img (lo_image);
    gpiv_free_pivdata (lo_piv_data);


    if (verbose) printf ("\n");
    return piv_data;
}



gchar *
gpiv_piv_interrogate_ia (const guint index_y,
                         const guint index_x,
                         const GpivImage *image,
                         const GpivPivPar *piv_par,
                         const guint sweep,
                         const guint last_sweep,
                         gfloat **int_area_1,
                         gfloat **int_area_2,
                         GpivCov *cov,
                         GpivPivData *piv_data
                         )
/**----------------------------------------------------------------------------
 *     Interrogates a single Interrogation Area
 */
{
    gchar *err_msg = NULL;

    guint ncolumns = image->header->ncolumns;
    guint nrows = image->header->nrows;
    gboolean x_corr = image->header->x_corr;

    guint ifit = piv_par->ifit;
    guint int_size_f = piv_par->int_size_f;
    guint int_size_i = piv_par->int_size_i;
    gint peak = piv_par->peak;
    int pre_shift_row = piv_par->pre_shift_row;
    int pre_shift_col = piv_par->pre_shift_col;
    enum GpivIntScheme int_scheme = piv_par->int_scheme;

    int return_val;
    int idum = gpiv_max((int_size_i - int_size_f) / 2, 0);
    int m = 0, n = 0;
    float int_area_1_mean = 0.0, int_area_2_mean = 0.0;
/*     BUGFIX: gpiv_piv_interrogate_ia: disabled normalization I.A */
#ifdef NORM_AI
    float int_area_1_range = 0.0, int_area_2_range = 0.0;
    float norm_fact = 0.0;
   guint img_top = (1 << image->header->depth) - 1;
#endif
    int ipoint_x;
    int ipoint_y;

    int pre_shift_row_act = 0, pre_shift_col_act = 0;
    int peak_act = 0, i_skip_act = 0, j_skip_act = 0;

    gboolean flag_subtop = FALSE, flag_intar0 = FALSE, flag_accept = FALSE;
/*
 * Interrogation area with zero padding
 */
    GpivCov *w_k = NULL;                /* covariance weighting kernel */
    int int_size_0 = GPIV_ZEROPAD_FACT * int_size_i;


/*
 * Checking for memory allocation of input variables
 */
    g_return_val_if_fail (image->frame1[0] != NULL, "image->frame1[0] != NULL");
    g_return_val_if_fail (image->frame2[0] != NULL, "image->frame2[0] != NULL");
    g_return_val_if_fail (int_area_1[0] != NULL, "int_area_1[0] != NULL");
    g_return_val_if_fail (int_area_2[0] != NULL, "int_area_2[0] != NULL");
    g_return_val_if_fail (cov != NULL, "cov != NULL");
    g_return_val_if_fail (piv_data->point_x != NULL, "piv_data->point_x != NULL");
    g_return_val_if_fail (piv_data->point_y != NULL, "piv_data->point_y != NULL");
    g_return_val_if_fail (piv_data->dx != NULL, "piv_data->dx != NULL");
    g_return_val_if_fail (piv_data->dy != NULL, "piv_data->dy != NULL");
    g_return_val_if_fail (piv_data->snr != NULL, "piv_data->snr != NULL");
    g_return_val_if_fail (piv_data->peak_no != NULL, "piv_data->peak_no != NULL");

    ipoint_x = (int) piv_data->point_x[index_y][index_x];
    ipoint_y = (int) piv_data->point_y[index_y][index_x];
/*
 * uses  piv values from previous estimation as pre-shifts and
 * searches closest Int. Area
 */
    if (int_scheme == GPIV_ZERO_OFF_FORWARD
        || int_scheme == GPIV_ZERO_OFF_CENTRAL) {
	pre_shift_col_act = piv_data->dx[index_y][index_x] + pre_shift_col;
	pre_shift_row_act = piv_data->dy[index_y][index_x] + pre_shift_row;
        piv_data->dx[index_y][index_x] = 0.0;
        piv_data->dy[index_y][index_x] = 0.0;
    } else {
	pre_shift_col_act = pre_shift_col;
	pre_shift_row_act = pre_shift_row;
    }

    peak_act = peak;


/*     gpiv_warning ("gpiv_piv_interrogate_ia:: 0"); */
/*
 * Assigning image data to the interrogation area's
 */
    if (int_scheme == GPIV_ZERO_OFF_CENTRAL ) {
        flag_accept = assign_img2intarr_central(ipoint_x, ipoint_y,
                                                image->frame1, image->frame2,
                                                int_size_f, int_size_i,
                                                int_area_1, int_area_2,
                                                pre_shift_row_act,
                                                pre_shift_col_act,
                                                nrows,
                                                ncolumns,
                                                int_size_0);

    } else if (int_scheme == GPIV_ZERO_OFF_FORWARD) {
        flag_accept = assign_img2intarr_central(ipoint_x, ipoint_y,
                                                image->frame1, image->frame2,
                                                int_size_f, int_size_i,
                                                int_area_1, int_area_2,
                                                pre_shift_row_act,
                                                pre_shift_col_act,
                                                nrows,
                                                ncolumns,
                                                int_size_0);
    } else {
        flag_accept = assign_img2intarr(ipoint_x, ipoint_y,
                                        image->frame1, image->frame2,
                                        int_size_f, int_size_i,
                                        int_area_1, int_area_2,
                                        pre_shift_row_act,
                                        pre_shift_col_act,
                                        nrows,
                                        ncolumns,
                                        int_size_0);
    }

    if (flag_accept) {

/*
 * Weighting Interrogation Area with Gaussian function
 */
       if (piv_par->gauss_weight_ia) {
            if ((err_msg = ia_weight_gauss (int_size_f, int_area_1))
                != NULL) {
                return (err_msg);
            }

            if ((err_msg = ia_weight_gauss (int_size_i, int_area_2))
                != NULL) {
                return (err_msg);
            }
        }

/*
 * The mean value of the image data within an interrogation area will be
 * subtracted from the data
 * BUXFIX: test on differences in estimator!
 */
/*         int_area_1_meand = int_mean_old(image->frame1, int_size_f, int_size_i,  */
/*                                    ipoint_x, ipoint_y); */
/*         int_area_2_meand = int_mean_old(image->frame2, int_size_i, int_size_i,  */
/*                                    ipoint_x, ipoint_y); */

        int_area_1_mean = int_mean (int_area_1, int_size_f);
        int_area_2_mean = int_mean (int_area_2, int_size_i);
#ifdef NORM_AI
        int_area_1_range = int_range (int_area_1, int_size_f);
        int_area_2_range = int_range (int_area_2, int_size_i);
#endif
/*         g_message(":: sweep = %d int_area_1_mean[%d][%d] = %f int_area_2_mean[%d][%d] = %f", */
/*                   sweep, */
/*                   ipoint_y, ipoint_x, int_area_1_meand,  */
/*                   ipoint_y, ipoint_x, int_area_2_meand); */
/*         g_message(":: sweep = %d int_area_1_mean[%d][%d] = %f int_area_2_mean[%d][%d] = %f", */
/*                   sweep, */
/*                   ipoint_y, ipoint_x, int_area_1_mean,  */
/*                   ipoint_y, ipoint_x, int_area_2_mean); */


/*
 * BUGFIX: this might be substituted by counting the number of particles within
 * Int. Area, as done in PTV
 */
        if (int_area_1_mean == 0.0 || int_area_2_mean == 0.0
            || int_const (int_area_1, int_size_f)
            || int_const (int_area_2, int_size_i)
            ) {
/*             err_msg = "gpiv_piv_interrogate_ia: int_area_1/2_mean = 0.0"; */
            flag_intar0 = TRUE;
/*             return err_msg; */

            }

#ifdef NORM_AI
        norm_fact = (gfloat) img_top / int_area_1_range;
/*         g_message(":: top = %d range = %f ==> fact = %f", */
/*                   img_top, */
/*                   int_area_1_range, */
/*                   norm_fact); */
#endif
#pragma omp parallel for
        for (m = 0; m < int_size_f; m++) {
            for (n = 0; n < int_size_f; n++) {
                int_area_1[m + idum ][n + idum ] -= int_area_1_mean;
#ifdef NORM_AI
                int_area_1[m + idum ][n + idum ] *= norm_fact;
#endif
            }
        }

#ifdef NORM_AI
        norm_fact = (gfloat) img_top / int_area_2_range;
#endif
#pragma omp parallel for
        for (m = 0; m < int_size_i; m++) {
            for (n = 0; n < int_size_i; n++) {
                int_area_2[m][n] -= int_area_2_mean;
#ifdef NORM_AI
                int_area_2[m][n] *= norm_fact;
#endif
            }
        }

/*
 * Calculate covariance function
 */
        if (!flag_intar0) {
            if ((err_msg = cova (int_size_i, int_area_1, int_area_2,
                                 cov, piv_par->spof_filter))
                != NULL) {
                gpiv_warning("%s", err_msg);
                return err_msg;
            }

/*
 * Weighting covariance data with weight kernel
 */
            if (piv_par->int_scheme == GPIV_LK_WEIGHT) {
                w_k = gpiv_alloc_cov (int_size_0, image->header->x_corr);
                piv_weight_kernel_lin (int_size_0, w_k);
                weight_cov (cov, w_k);
                gpiv_free_cov (w_k);
            }

/*
 * Searching maximum peak in covariance function
 */
            if ((return_val = cov_top (*piv_par, piv_data, index_y, index_x,
                                       cov, x_corr, ifit, sweep, last_sweep,
                                       peak, peak_act, pre_shift_row_act,
                                       pre_shift_col_act,
                                       i_skip_act, j_skip_act,
                                       &flag_subtop)) != 0) {
                err_msg = "gpiv_piv_interrogate_ia: Unable to call cov_top";
                gpiv_warning("%s", err_msg);
                return err_msg;
            }

/*
 * Writing values to piv_data
 */
            piv_data->dx[index_y][index_x] =
                (double) pre_shift_col_act + 
                (double) cov->top_x + cov->subtop_x;

            piv_data->dy[index_y][index_x] =
                (double) pre_shift_row_act + 
                (double) cov->top_y + cov->subtop_y;

/*
 * Disabled as outliers are tested after each iteration
 */
/*             if (last_sweep == 1) { */
                piv_data->snr[index_y][index_x] = cov->snr;
                piv_data->peak_no[index_y][index_x] = peak_act;
/*             } */

        }
/*
 * Check on validity of data
 */
        if (isnan(piv_data->dx[index_y][index_x]) != 0
            || isnan(piv_data->dy[index_y][index_x]) != 0
            || isnan(piv_data->snr[index_y][index_x]) != 0
            || flag_subtop
            || flag_intar0
            ) {
            piv_data->dx[index_y][index_x] = 0.0;
            piv_data->dy[index_y][index_x] = 0.0;
            piv_data->snr[index_y][index_x] = GPIV_SNR_NAN;
            piv_data->peak_no[index_y][index_x] = -1;
        }

/*
 * Uses old piv data and sets flag peak_no to -1 if:
 * for zero offsetting: 2nd Interrogation Area is out of image boundaries
 * for zero offsetting with central diff: one of the Interrogation Area's
 * is out of image  boundaries
 */
    } else {
        piv_data->dx[index_y][index_x] = piv_data->dx[index_y][index_x];
        piv_data->dy[index_y][index_x] = piv_data->dy[index_y][index_x];
        piv_data->snr[index_y][index_x] = piv_data->snr[index_y][index_x];
        piv_data->peak_no[index_y][index_x] = -1;

    }

    return err_msg;
}



void
gpiv_piv_isizadapt (const GpivPivPar *piv_par_src,
                    GpivPivPar *piv_par_dest,
                    gboolean *isiz_last
                    )
/*---------------------------------------------------------------------------*/
/**
 *     Adjusts interrogation area sizes. For each interrogation sweep,
 *     (dest) int_size_i is halved, until it reaches (src)
 *     int_size_f. Then, isiz_last is set TRUE, which will avoid
 *     changing the interrogation sizes in next calls.
 *
 *     @param[in] piv_par_src        original parameters
 *     @param[out] piv_par_dest       actual parameters, to be modified during sweeps
 *     @param[out] isiz_last               flag for last interrogation sweep
 *     @return void
 */
/*---------------------------------------------------------------------------*/

{

/*     if (piv_par_dest->int_size_i == piv_par_src->int_size_i) */
/*         piv_par_dest->ad_int = 0; */

/*     if (piv_par_dest->ad_int == 1) { */
    if ((piv_par_dest->int_size_i) / 2 <= piv_par_src->int_size_f) {
        *isiz_last = TRUE;
        piv_par_dest->int_size_f = 
            (piv_par_src->int_size_f - GPIV_DIFF_ISI);
        piv_par_dest->int_size_i = 
            piv_par_src->int_size_f;
    } else {
        piv_par_dest->int_size_f = piv_par_dest->int_size_i / 2;
        piv_par_dest->int_size_i = piv_par_dest->int_size_i / 2;
    }

/*     } else if (piv_par_src->int_scheme == GPIV_ZERO_OFF_FORWARD */
/*                || piv_par_src->int_scheme == GPIV_ZERO_OFF_CENTRAL */
/*                || piv_par_src->int_scheme == GPIV_IMG_DEFORM */
/*                ) { */
/*         *isiz_last = TRUE; */
/*         piv_par_dest->ifit = piv_par_src->ifit; */
/*         piv_par_dest->int_size_f = (piv_par_src->int_size_f -  */
/*                                          GPIV_DIFF_ISI); */
/*         piv_par_dest->int_size_i = piv_par_src->int_size_f; */
/*     } */

}



/* #define SAVE_TMP */

gchar *
gpiv_piv_write_deformed_image (GpivImage *image
                               )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Stores deformed image to file system with pre defined name to TMPDIR
 *     and prints message to stdout
 *
 * INPUTS:
 *     img1:                   first image of PIV image pair
 *     img2:                   second image of PIV image pair
 *     image_par:              image parameters to be stored in header
 *     verbose:                prints the storing to stdout
 *
 * OUTPUTS:
 *
 * RETURNS:
 *     char * to NULL on success or *err_msg on failure
 *---------------------------------------------------------------------------*/
{
    gchar *err_msg = NULL;
    gchar *def_img;
    FILE *fp;


    def_img =  g_strdup_printf ("%s%s",  GPIV_DEFORMED_IMG_NAME, 
                                GPIV_EXT_PNG_IMAGE);

#ifdef SAVE_TMP
    if ((fp = my_utils_fopen_tmp (def_img, "wb")) == NULL) {
        err_msg = "gpiv_piv_write_deformed_image: Failure opening for output";
        return err_msg;
    }

    g_message ("gpiv_piv_write_deformed_image: Writing deformed image to: %s",
               g_strdup_printf ("%s/%s/%s", tmp_dir, user_name, def_img));
#else
    fp = fopen (def_img, "wb");
    g_message ("gpiv_piv_write_deformed_image: Writing deformed image to: %s",
               def_img);
#endif
    if ((err_msg = 
         gpiv_write_png_image (fp, image, FALSE)) != NULL) {
        fclose (fp);
        return err_msg;
    }

    fclose(fp);
    g_free (def_img);
    return err_msg;
}

#ifdef SAVE_TMP
#undef SAVE_TMP
#endif



static void
piv_weight_kernel_lin (const guint int_size_0,
                       GpivCov *w_k
                       )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *      Calculate linear weight kernel values for covariance data
 *
 * INPUTS:
 *
 * OUTPUTS:
 *	w_k:            Structure containing weighting data
 *      int_size_0:     zero-padded interrogation size
 *
 * RETURNS:
 *
 *---------------------------------------------------------------------------*/
{
    int i, j;
    int z_rnl = w_k->z_rnl, z_rnh = w_k->z_rnh, z_rpl = w_k->z_rpl,
        z_rph = w_k->z_rph;
    int z_cnl = w_k->z_cnl, z_cnh = w_k->z_cnh, z_cpl = w_k->z_cpl,
        z_cph = w_k->z_rph;


    g_return_if_fail (w_k != NULL);

    for (i = z_rnl; i < z_rnh; i++) {
	for (j = z_cnl; j < z_cnh; j++) {
	    w_k->z[i - int_size_0][j - int_size_0] =
		(1 - abs(z_rnh - i) / (int_size_0 / 2.0)) 
                * (1 - abs(z_cnh - j) / (int_size_0 / 2.0));
	}

	for (j = z_cpl; j < z_cph; j++) {
	    w_k->z[i - int_size_0][j] =
		(1 - abs(z_rnh - i) / (int_size_0 / 2.0)) 
                * (1 - abs(z_cpl - j) / (int_size_0 / 2.0));
	}
    }


    for (i = z_rpl; i < z_rph; i++) {
	for (j = z_cnl; j < z_cnh; j++) {
	    w_k->z[i][j - int_size_0] =
		(1 - abs(z_rpl - i) / (int_size_0 / 2.0)) 
                * (1 - abs(z_cnh - j) / (int_size_0 / 2.0));
	}

	for (j = z_cpl; j < z_cph; j++) {
	    w_k->z[i][j] = (1 - abs(z_rpl - i) / (int_size_0 / 2.0))
		* (1 - abs(z_cpl - j) / (int_size_0 / 2.0));
	}
    }
}



void
write_cov (int int_x,
           int int_y,
           int int_size,
           float **cov_area,
           int weight
           )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *   Prints covariance data
 *
 * INPUTS:
 *   int_x:
 *   int_y
 *   cov_area:
 *   int_size,
 *
 * SOME MNENOSYNTACTICS OF LOCAL VARIABLES:
 *   cov_area:         name of array
 *   r:                row
 *   c:                column
 *   n:                negative displacements ; from 3/4 to 1 of int_size_0
 *   p:                positive displacements; from 0 to 1/4 of int_size_0
 *   l:                lowest index
 *   h:                highest index
 *---------------------------------------------------------------------------*/
{
    int i, j;
    int cov_area_rnl, cov_area_rnh, cov_area_rpl, cov_area_rph,
	cov_area_cnl, cov_area_cnh, cov_area_cpl, cov_area_cph;
    float weight_kernel;
    int int_size_0 = GPIV_ZEROPAD_FACT * int_size;


    cov_area_rnl = 3.0 * (int_size_0) / 4 + 1;
    cov_area_rnh = int_size_0;
    cov_area_rpl = 0;
    cov_area_rph = int_size_0 / 4;

    cov_area_cnl = 3.0 * (int_size_0) / 4 + 1;
    cov_area_cnh = int_size_0;
    cov_area_cpl = 0;
    cov_area_cph = int_size_0 / 4;


    for (i = cov_area_rnl; i < cov_area_rnh; i++) {
	for (j = cov_area_cnl; j < cov_area_cnh; j++) {
	    if (weight == 1) {
		weight_kernel =
		    (1 - abs(cov_area_rnh - i) / (int_size_0 / 2.0)) * 
		  (1 - abs(cov_area_cnh - j) / (int_size_0 / 2.0));
	    }
	}
	for (j = cov_area_cpl; j < cov_area_cph; j++) {
	    if (weight == 1) {
		weight_kernel =
		    (1 - abs(cov_area_rnh - i) / (int_size_0 / 2.0)) * 
		  (1 - abs(cov_area_cpl - j) / (int_size_0 / 2.0));
	    }
	}
    }


    for (i = cov_area_rpl; i < cov_area_rph; i++) {
	for (j = cov_area_cnl; j < cov_area_cnh; j++) {
	    if (weight == 1) {
		weight_kernel =
		    (1 - abs(cov_area_rpl - i) / (int_size_0 / 2.0)) * 
		  (1 - abs(cov_area_cnh - j) / (int_size_0 / 2.0));
	    }
	}
	for (j = cov_area_cpl; j < cov_area_cph; j++) {
	    if (weight == 1) {
		weight_kernel =
		    (1 - abs(cov_area_rpl - i) / (int_size_0 / 2.0)) * 
		  (1 - abs(cov_area_cpl - j) / (int_size_0 / 2.0));
	    }
	}
    }
}



void
gpiv_fread_fftw_wisdom (const gint dir
                        )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Reads fftw wisdoms from file and stores into a string
 *
 * INPUTS:
 *     dir:            direction of fft; forward (+1) or inverse (-1)
 *
 * OUTPUTS:
 *
 * RETURNS:
 *     fftw_wisdom
 *---------------------------------------------------------------------------*/
{
    gchar *fftw_filename;
    FILE *fp;


    g_return_if_fail ( dir == 1 || dir == -1);

/*
 * Forward FFT or inverse FFT
 */
    if (dir == 1) {
	fftw_filename = g_strdup_printf ("%s", FFTWISDOM);
    } else {
	fftw_filename = g_strdup_printf ("%s", FFTWISDOM_INV);
    }

    if ((fp = my_utils_fopen_tmp (fftw_filename, "r")) != NULL) {
	fftw_import_wisdom_from_file(fp);
        fclose(fp);
    }

    g_free (fftw_filename);
}



void
gpiv_fwrite_fftw_wisdom (const gint dir
                         )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *      Writes fftw wisdoms to a file
 *
 * INPUTS:
 *      dir:            direction of fft; forward (+1) or inverse (-1)
 *
 * OUTPUTS:
 *
 * RETURNS:
 *
 *---------------------------------------------------------------------------*/
{
    gchar *fftw_filename;
    FILE *fp;
 
    g_return_if_fail ( dir == 1 || dir == -1);

/*
 * Forward FFT or inverse FFT
 */
    if (dir == 1) {
	fftw_filename = g_strdup_printf ("%s", FFTWISDOM);
    } else {
	fftw_filename = g_strdup_printf ("%s", FFTWISDOM_INV);
    }

    if ((fp = my_utils_fopen_tmp (fftw_filename, "w")) != NULL) {
	fftw_export_wisdom_to_file(fp);
	fclose(fp);
    }

    fftw_forget_wisdom();
    g_free (fftw_filename);
}


/*
 * Public functions, original from piv_speed
 */

gchar *
gpiv_piv_dxdy_at_new_grid (const GpivPivData *piv_data_src,
                           GpivPivData *piv_data_dest
                           )
/*---------------------------------------------------------------------------*/
/**
 * calculates dx, dy of piv_data_dest from piv_data_src
 * by bi-linear interpolation of inner points with shifted knots
 * or extrapolation of outer lying points
 *
 *   dist_:      distance
 *   _n:         NORTH
 *   _s:         SOUTH
 *   _e:         EAST
 *   _w:         WEST
 *   _nn:        at NORTH of NORTH, etc.
 *
 *     @param[in] piv_data_src       input piv data
 *     @param[out] piv_data_dest      output piv data
 *     @return NULL on success or *err_msg on failure
 */
/*---------------------------------------------------------------------------*/
{
    char c_line[GPIV_MAX_LINES_C][GPIV_MAX_CHARS];
    char *err_msg = NULL;

    gint *index_n, *index_s, *index_e, *index_w;
    gint *index_nn, *index_ss, *index_ee, *index_ww;

    float *src_point_x = NULL, *dest_point_x = NULL;
    float *src_point_y = NULL, *dest_point_y = NULL;
    double *alpha_hor, *alpha_vert;

    double epsi = 0.01;
    enum VerticalPosition vert_pos;
    enum HorizontalPosition hor_pos;
    gint i = 0, j = 0;

    GpivPivData *gpd = NULL;


/*
 * shift the knots of the grid for higher accuracies.
 * in order not to affect piv_data_src, a new PIV dataset will be copied
 */
#ifdef DEBUG
    g_message ("gpiv_piv_dxdy_at_new_grid:: 0, src_nx = %d src_ny = %d dest_nx = %d dest_ny = %d",
               piv_data_src->nx, piv_data_src->ny,
               piv_data_dest->nx, piv_data_dest->ny);
#endif
    gpd = gpiv_cp_pivdata (piv_data_src);
 

    if ((err_msg = gpiv_piv_shift_grid (gpd)) != NULL) {
        err_msg = "gpiv_piv_dxdy_at_new_grid: failing  gpiv_piv_shift_grid";
        g_warning ("%s", err_msg);
        return err_msg;
    }


    index_n = gpiv_ivector (piv_data_dest->ny);
    index_s = gpiv_ivector (piv_data_dest->ny);
    index_e = gpiv_ivector (piv_data_dest->nx);
    index_w = gpiv_ivector (piv_data_dest->nx);
    index_nn = gpiv_ivector (piv_data_dest->ny);
    index_ss = gpiv_ivector (piv_data_dest->ny);
    index_ee = gpiv_ivector (piv_data_dest->nx);
    index_ww = gpiv_ivector (piv_data_dest->nx);

    alpha_vert = gpiv_dvector (piv_data_dest->ny);
    alpha_hor = gpiv_dvector (piv_data_dest->nx);

    src_point_x = gpiv_vector (gpd->nx);
    src_point_y = gpiv_vector (gpd->ny);
    dest_point_x = gpiv_vector (piv_data_dest->nx);
    dest_point_y = gpiv_vector (piv_data_dest->ny);

/*
 * Calculate interpolation factors
 * in Horizontal direction
 */
#ifdef DEBUG
    g_message ("gpiv_piv_dxdy_at_new_grid:: 1a, gpd_nx = %d gpd_ny = %d _ny",
               gpd->nx, gpd->ny);
#endif
    if (gpd->nx >= NMIN_TO_INTERPOLATE) {
        for (i = 0, j = 0; j < gpd->nx; j++) {
            src_point_x[j] = gpd->point_x[i][j];
       }

        for (i = 0, j = 0; j < piv_data_dest->nx; j++) {
            dest_point_x[j] = piv_data_dest->point_x[i][j];
        }

        intpol_facts (src_point_x, 
                      dest_point_x, 
                      gpd->nx,
                      piv_data_dest->nx,
                      index_w,
                      index_e,
                      index_ww,
                      index_ee,
                      alpha_hor);
    } else {
        err_msg = "gpiv_piv_dxdy_at_new_grid: Not enough points in horizontal direction";
        return err_msg;
    }

/*
 * Calculate interpolation factors
 * in Vertical direction
 */
    if (gpd->ny >= NMIN_TO_INTERPOLATE) {
        for (i = 0, j = 0; i < gpd->ny; i++) {
            src_point_y[i] = gpd->point_y[i][j];
        }

        for (i = 0, j = 0; i < piv_data_dest->ny; i++) {
            dest_point_y[i] = piv_data_dest->point_y[i][j];
        }

        intpol_facts (src_point_y, 
                      dest_point_y, 
                      gpd->ny,
                      piv_data_dest->ny,
                      index_n,
                      index_s,
                      index_nn,
                      index_ss,
                      alpha_vert);
    } else {
        err_msg = "gpiv_piv_dxdy_at_new_grid: Not enough points in horizontal direction";
        return err_msg;
    }

/*
 * Calculate new displacements by bi-lineair interpolation
 */
    for (i = 0; i < piv_data_dest->ny; i++) {
        for (j = 0; j < piv_data_dest->nx; j++) {
            piv_data_dest->dx[i][j] = bilinear_interpol
                (alpha_hor[j],
                 alpha_vert[i],
                 gpd->dx[index_n[i]][index_w[j]],
                 gpd->dx[index_n[i]][index_e[j]],
                 gpd->dx[index_s[i]][index_w[j]],
                 gpd->dx[index_s[i]][index_e[j]]);

#ifdef DEBUG2
g_message ("piv_dxdy_at_new_grid:: alpha_hor[%d]=%f alpha_vert[%d]=%f dx_nw=%f dx_ne=%f dx_sw=%f dx_se=%f => dx=%f",
              j, alpha_hor[j], i, alpha_vert[i],
              gpd->dx[index_n[i]][index_w[j]],
              gpd->dx[index_n[i]][index_e[j]],
              gpd->dx[index_s[i]][index_w[j]],
              gpd->dx[index_s[i]][index_e[j]],
              piv_data_dest->dx[i][j]
              );
#endif

            piv_data_dest->dy[i][j] = bilinear_interpol
                (alpha_hor[j],
                 alpha_vert[i],
                 gpd->dy[index_n[i]][index_w[j]],
                 gpd->dy[index_n[i]][index_e[j]],
                 gpd->dy[index_s[i]][index_w[j]],
                 gpd->dy[index_s[i]][index_e[j]]);

#ifdef DEBUG2
            g_message ("piv_dxdy_at_new_grid:: alpha_hor[%d]=%f alpha_vert[%d]=%f dy_nw=%f dy_ne=%f dy_sw=%f dy_se=%f => dy=%f",
              j, alpha_hor[j], i, alpha_vert[i],
              gpd->dy[index_n[i]][index_w[j]],
              gpd->dy[index_n[i]][index_e[j]],
              gpd->dy[index_s[i]][index_w[j]],
              gpd->dy[index_s[i]][index_e[j]],
              piv_data_dest->dy[i][j]
              );
#endif

        }
    }
    

    gpiv_free_ivector (index_n);
    gpiv_free_ivector (index_s);
    gpiv_free_ivector (index_e);
    gpiv_free_ivector (index_w);
    gpiv_free_ivector (index_nn);
    gpiv_free_ivector (index_ss);
    gpiv_free_ivector (index_ee);
    gpiv_free_ivector (index_ww);

    gpiv_free_dvector (alpha_vert);
    gpiv_free_dvector (alpha_hor);

    gpiv_free_vector (src_point_x);
    gpiv_free_vector (src_point_y);
    gpiv_free_vector (dest_point_x);
    gpiv_free_vector (dest_point_y);

    gpiv_free_pivdata (gpd);

    return err_msg;
}


gchar *
gpiv_piv_shift_grid (GpivPivData *gpd_src
                     )
/*---------------------------------------------------------------------------*/
/**
 * shifts the knots of a 2-dimensional grid containing PIV data for improved 
 * (bi-linear) interpolation
 *
 * See: T. Blu, P. Thevenaz, "Linear Interpolation Revitalized",
 * IEEE Trans. in Image Processing, vol13, no 5, May 2004
 *
 *     @param[in] piv_data_src   input piv data
 *     @return NULL on success or *err_msg on failure
 */
/*---------------------------------------------------------------------------*/
{
#define SHIFT 0.2

    char *err_msg = NULL;
    GpivPivData *h_gpd = NULL;
    GpivPivData *v_gpd = NULL;
    gfloat fact1 = -SHIFT / (1.0 - SHIFT);
    gfloat fact2 = 1.0 / (1 - SHIFT);
    gfloat **cx, **cy;
    gfloat delta = 0.0;
    gint i, j;


    delta = gpd_src->point_x[0][1] - gpd_src->point_x[0][0];

/*
 * Shift in horizontal (column-wise) direction
 */
    h_gpd = gpiv_alloc_pivdata (gpd_src->nx, gpd_src->ny);


    for (i = 0; i < gpd_src->ny; i++) {
        for (j = 0; j < gpd_src->nx; j++) {
/*
 * Shift the knot (sample points)
 */
            h_gpd->point_x[i][j] = gpd_src->point_x[i][j] + SHIFT * delta;
            h_gpd->point_y[i][j] = gpd_src->point_y[i][j];
            if (j == 0) {
                h_gpd->dx[i][j] = gpd_src->dx[i][j];
                h_gpd->dy[i][j] = gpd_src->dy[i][j];
            } else {
/*
 * Calculate value at shifted knot
 */
                h_gpd->dx[i][j] = fact1 * h_gpd->dx[i][j-1] + fact2 * 
                    gpd_src->dx[i][j];
                h_gpd->dy[i][j] = fact1 * h_gpd->dy[i][j-1] + fact2 * 
                    gpd_src->dy[i][j];
            }
        }
    }
    

/*
 * Shift in vertical (row-wise) direction by using the horizontal shifted nodes
 */
    v_gpd = gpiv_alloc_pivdata (gpd_src->nx, gpd_src->ny);


    for (i = 0; i < gpd_src->ny; i++) {
        for (j = 0; j < gpd_src->nx; j++) {
            v_gpd->point_x[i][j] = h_gpd->point_x[i][j];
            v_gpd->point_y[i][j] = h_gpd->point_y[i][j] + SHIFT * delta;
            if (i == 0) {
                v_gpd->dx[i][j] = h_gpd->dx[i][j];
                v_gpd->dy[i][j] = h_gpd->dy[i][j];
            } else {
                v_gpd->dx[i][j] = fact1 * v_gpd->dx[i-1][j] + fact2 * 
                    h_gpd->dx[i][j];
                v_gpd->dy[i][j] = fact1 * v_gpd->dy[i-1][j] + fact2 * 
                    h_gpd->dy[i][j];
            }
        }
    }
    

/*     gpiv_free_pivdata (gpd_src); */
/*     gpd_src = gpiv_cp_pivdata (v_gpd); */

    for (i = 0; i < gpd_src->ny; i++) {
        for (j = 0; j < gpd_src->nx; j++) {
            gpd_src->point_x[i][j] = v_gpd->point_x[i][j];
            gpd_src->point_y[i][j] = v_gpd->point_y[i][j];
            gpd_src->dx[i][j] = v_gpd->dx[i][j];
            gpd_src->dy[i][j] = v_gpd->dy[i][j];
            gpd_src->snr[i][j] = v_gpd->snr[i][j];
            gpd_src->peak_no[i][j] = v_gpd->peak_no[i][j];
        }
    }

/*     gpiv_write_pivdata (NULL, gpd_src, FALSE); */


    gpiv_free_pivdata (h_gpd);
    gpiv_free_pivdata (v_gpd);
    return err_msg;
#undef SHIFT
}



GpivPivData *
gpiv_piv_gridgen (const guint nx,
                  const guint ny,
                  const GpivImagePar *image_par,
                  const GpivPivPar *piv_par
                  )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Generates grid by Calculating the positions of interrogation areas
 *     Substitutes gpiv_piv_select_int_point
 *
 * INPUTS:
 *     nx               number of columns
 *     ny               number of rows
 *     @image_par:      structure of image parameters
 *     @piv_par:   structure of piv pivuation parameters
 *
 * OUTPUTS:
 *     @out_data:       output piv data from image analysis
 *
 * RETURNS:
 *     %char * NULL on success or *err_msg on failure
 *---------------------------------------------------------------------------*/
{
    GpivPivData *piv_data = NULL;
    gchar *err_msg = NULL;
    int row, column, row_1, column_1, i, j;
    int row_max, row_min, column_max, column_min;

    int ncolumns = image_par->ncolumns;
    int nrows = image_par->nrows;

    int int_geo = piv_par->int_geo;
    int row_start = piv_par->row_start;
    int row_end = piv_par->row_end;
    int col_start = piv_par->col_start;
    int col_end= piv_par->col_end;
    int int_line_col = piv_par->int_line_col;
    int int_line_col_start = piv_par->int_line_col_start;
    int int_line_col_end = piv_par->int_line_col_end;
    int int_line_row = piv_par->int_line_row;
    int int_line_row_start = piv_par->int_line_row_start;
    int int_line_row_end = piv_par->int_line_row_end;
    int int_point_col = piv_par->int_point_col;
    int int_point_row = piv_par->int_point_row;
    int int_size_f = piv_par->int_size_f;
    int int_size_i = piv_par->int_size_i;
    int int_shift = piv_par->int_shift;
    int pre_shift_row = piv_par->pre_shift_row;
    int pre_shift_col = piv_par->pre_shift_col;
				 

/*     g_return_val_if_fail (piv_data->point_x != NULL, "piv_data->point_x != NULL"); */
/*     g_return_val_if_fail (piv_data->point_y != NULL, "piv_data->point_y != NULL"); */

    row_min = gpiv_min (-int_size_f / 2 + 1, 
                        pre_shift_row - int_size_i / 2 + 1);
    column_min = gpiv_max (-int_size_f / 2 + 1, 
                           pre_shift_col - int_size_i / 2 + 1);
    row_max = gpiv_max (int_size_f / 2, pre_shift_row + int_size_i / 2);
    column_max = gpiv_max (int_size_f / 2, pre_shift_col + int_size_i / 2);


/*
 * Creates piv_data and centre points for one single interrogation area
 */
    piv_data = gpiv_alloc_pivdata (nx, ny);

    if (int_geo == GPIV_POINT) {
        piv_data->point_y[0][0] = int_point_row;
        piv_data->point_x[0][0] = int_point_col;
            

/*
 * Creates centre points for one single row
 */
    } else if (int_geo == GPIV_LINE_R) {
        if ((int_size_f - int_size_i) / 2 + pre_shift_col < 0) {
            column_1 = int_line_col_start - 
                ((int_size_f - int_size_i) / 2 +
                 pre_shift_col) + int_size_f / 2 - 1;
        } else {
            column_1 = int_line_col_start + int_size_f / 2 - 1;
        }

        for (i = 0, j = 0, row = int_line_row, column = column_1;
             j < piv_data->nx; j++, column += int_shift) {
            piv_data->point_y[i][j] = row;
            piv_data->point_x[i][j] = column;
        }


/*
 * Creates centre points for one single column
 */
    } else  if (int_geo == GPIV_LINE_C) {
        if ((int_size_f - int_size_i) / 2 + pre_shift_row < 0) {
            row_1 = int_line_row_start - 
                ((int_size_f - int_size_i) / 2 +
                 pre_shift_row) + int_size_f / 2 - 1;
        } else {
            row_1 = int_line_row_start + int_size_f / 2 - 1;
        }

        for (i = 0, j = 0, column = int_line_col, row = row_1; 
             i < piv_data->ny; i++, row += int_shift) {
            piv_data->point_y[i][j] = row;
            piv_data->point_x[i][j] = column;
        }


/*
 * Creates an array of  centre points of the Interrrogation Area's: 
 * int_ar_x and int_ar_y within the defined region of the image
 */
    } else if (int_geo == GPIV_AOI) {
	if ((int_size_f - int_size_i) / 2 + pre_shift_row < 0) {
	    row_1 =
		row_start - ((int_size_f - int_size_i) / 2 +
			     pre_shift_row) + int_size_f / 2 - 1;
	} else {
	    row_1 = row_start + int_size_f / 2 - 1;
	}

	if ((int_size_f - int_size_i) / 2 + pre_shift_col < 0) {
	    column_1 =
		col_start - ((int_size_f - int_size_i) / 2 +
			     pre_shift_col) + int_size_f / 2 - 1;
	} else {
	    column_1 = col_start + int_size_f / 2 - 1;
	}

	for (i = 0, row = row_1; i < ny; i++, row += int_shift) {
	    for (j = 0, column = column_1; j < nx;
		 j++, column += int_shift) {
		piv_data->point_y[i][j] = row;
		piv_data->point_x[i][j] = column;
	    }
	}
    } else {
        err_msg = "gpiv_piv_gridgen: should not arrive here";
        gpiv_warning ("%s", err_msg);
        return NULL;
    }


    return piv_data;
}



GpivPivData *
gpiv_piv_gridadapt (const GpivImagePar *image_par, 
                    const GpivPivPar *piv_par_src,
                    GpivPivPar *piv_par_dest,
                    const GpivPivData *piv_data,
                    const guint sweep, 
                    gboolean *grid_last
                    )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Adjust grid nodes if zero_off or adaptive interrogation 
 *     area has been used. This is performed by modifying int_shift equal 
 *     to int_shift / GPIV_SHIFT_FACTOR , until it reaches (src)
 *     int_shift. Then, grid_last is set TRUE, which will avoid
 *     changing the interrogation shift in next calls and signal the
 *     (while loop in) the calling function.
 *     
 * INPUTS:
 *     @image_par:                 image parameters
 *     @piv_par_src:               piv parameters
 *     @piv_data:                  input PIV data
 *     @sweep:                     interrogation sweep step
 *
 * OUTPUTS:
 *     @image_par:                image parameters
 *     @piv_par_dest:             modified piv parameters
 *     @grid_last:                flag if final grid refinement has been 
 *                                reached
 *
 * RETURNS:
 *     piv_data:                 modified PIV data
 *---------------------------------------------------------------------------*/
{
    GpivPivData *pd = NULL;
    gint local_int_shift, local_int_size_f, local_int_size_i;
    gint LOCAL_SHIFT_FACTOR = 2;

    guint nx, ny;


    local_int_shift = piv_par_dest->int_shift / LOCAL_SHIFT_FACTOR;
    if (local_int_shift <= piv_par_src->int_shift) {
        *grid_last = TRUE;
    }

    if (*grid_last == FALSE) {
/*
 * - renew grid of PIV dataset 
 * - calculate displacements at new grid points
 */
        piv_par_dest->int_shift = piv_par_dest->int_shift / 
            LOCAL_SHIFT_FACTOR;
        gpiv_piv_count_pivdata_fromimage (image_par, piv_par_dest, &nx, &ny);
        pd = gpiv_piv_gridgen (nx, ny, image_par, piv_par_dest);
        gpiv_piv_dxdy_at_new_grid (piv_data, pd);

    } else {
/*
 * reset int_shift (and data positions) to the originally defined 
 * parameter value.
 * For the last grid adaption, the number of interrogation area's may 
 * not have been doubled perse, as int_size may be of 
 * arbitrary quantity. 
 */

        piv_par_dest->int_shift =  piv_par_src->int_shift;
/*
 * Set int_size_f and int_size_i of piv_par_dest temporarly to the
 * original settings, so that an identic grid will be constructued as
 * during the gpiv_gridgen call.
 */
        local_int_size_f =  piv_par_dest->int_size_f;
        local_int_size_i =  piv_par_dest->int_size_i;
        piv_par_dest->int_size_f = piv_par_src->int_size_f;
        piv_par_dest->int_size_i = piv_par_src->int_size_i;
        gpiv_piv_count_pivdata_fromimage (image_par, piv_par_dest, &nx, &ny);
        pd = gpiv_piv_gridgen (nx, ny, image_par, piv_par_dest);
        piv_par_dest->int_size_f = local_int_size_f;
        piv_par_dest->int_size_i = local_int_size_i;

        gpiv_piv_dxdy_at_new_grid (piv_data, pd);
    }


    return pd;    
}


/*
 * Local functions
 */

static GpivPivData *
alloc_pivdata_gridgen (const GpivImagePar *image_par, 
                       const GpivPivPar *piv_par
                       )
/*-----------------------------------------------------------------------------
 * Determines the number of grid points, allocating memory for output 
 * data and generates the grid
 */
{
    GpivPivData *piv_data = NULL;
    gchar *err_msg = NULL;
    GpivPivPar *lo_piv_par = NULL;
    guint nx, ny;

    if ((lo_piv_par = gpiv_piv_cp_parameters (piv_par)) == NULL) {
        gpiv_error ("LIBGPIV internal error: alloc_pivdata_gridgen: failing gpiv_piv_cp_parameters");
    }

    if (piv_par->int_size_i > piv_par->int_size_f
        && piv_par->int_shift < piv_par->int_size_i / GPIV_SHIFT_FACTOR) { 
        lo_piv_par->int_shift = lo_piv_par->int_size_i / GPIV_SHIFT_FACTOR;
    }

    if ((err_msg = 
         gpiv_piv_count_pivdata_fromimage (image_par, lo_piv_par, &nx, &ny))
        != NULL) {
        g_message ("LIBGPIV internal error: alloc_pivdata_gridgen: %s", err_msg);
        return NULL;
    }

    
    if ((piv_data = 
         gpiv_piv_gridgen (nx, ny, image_par, lo_piv_par))
        == NULL) {
        g_message ("LIBGPIV internal error: alloc_pivdata_gridgen: failing gpiv_piv_gridgen");
        return NULL;
    }
    

    return piv_data;
}



static void 
report_progress (gint *progress_old,
                 gint index_y,
                 gint index_x,
                 GpivPivData *piv_data,
                 GpivPivPar *piv_par,
                 gint sweep,
                 gfloat cum_residu
                 )
/*-----------------------------------------------------------------------------
 * Printing the progress (between 0 and 100) of piv interrogation to stdout
 */
{
    gint progress  = 100 * (index_y * piv_data->nx + index_x + 1) / 
        (piv_data->nx * piv_data->ny); 

#ifdef ENABLE_MPI
    gint rank, size;
#endif


    if (progress != *progress_old) {
        *progress_old = progress;
                            
        if (index_y > 0 || index_x > 0)
            printf ("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b");

        if (piv_par->int_scheme == GPIV_ZERO_OFF_FORWARD
            || piv_par->int_scheme == GPIV_ZERO_OFF_CENTRAL
            || piv_par->int_scheme == GPIV_ZERO_OFF_CENTRAL
            || piv_par->int_scheme == GPIV_IMG_DEFORM
            || piv_par->int_size_i > piv_par->int_size_f) {
            printf
                ("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
                 "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
                 "\b\b\b\b\b\b\b\b\b\b\b"
                 "\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
                 );
#ifdef ENABLE_MPI
            MPI_Comm_rank(MPI_COMM_WORLD, &rank);
            MPI_Comm_size(MPI_COMM_WORLD, &size); 
            printf ("\b\b\b\b\b\b\b\b\b\b\b\b");
            printf ("rank %2d/%2d, ", rank,size);
#endif
            printf ("sweep #%2d, int_size = %d int_shift = %d residu = %.3f: ", 
                    sweep, piv_par->int_size_f, piv_par->int_shift, 
                    cum_residu);
        }

        printf ("%3d %%", progress);
        fflush (stdout);
    }
}



static gboolean
assign_img2intarr (gint ipoint_x,
                   gint ipoint_y,
                   guint16 **img_1,
                   guint16 **img_2,
                   gint int_size_f,
                   gint int_size_i,
                   gfloat **int_area_1,
                   gfloat **int_area_2,
                   gint pre_shift_row,
                   gint pre_shift_col,
                   gint nrows,
                   gint ncolumns,
                   gint int_size_0
                   )
/*-----------------------------------------------------------------------------
 * Assigns image data to the interrogation area arrays in a straightforward way
 */
{
    gint m, n;
    gint arg_int1_r = ipoint_y  - int_size_f / 2 + 1;
    gint arg_int1_c = ipoint_x  - int_size_f / 2 + 1;
    gint arg_int2_r = ipoint_y  - int_size_i / 2 + 1;
    gint arg_int2_c = ipoint_x  - int_size_i / 2 + 1;

    gboolean flag_valid;


    assert (img_1[0] != NULL);
    assert (img_2[0] != NULL);
    assert (int_area_1[0] != NULL);
    assert (int_area_2[0] != NULL);

/*
 * Check if Interrogation Areas are within the image boundaries.
 * Principally arg_int1_r,c don't have to be tested as 
 * int_size_i >= int_size_f, but has been kept to maintain generallity with the
 * other assign_img2intarr* functions
 */
    if ((arg_int1_r) >= 0
         && (arg_int1_r + int_size_f - 1) < nrows
         && (arg_int1_c) >= 0
         && (arg_int1_c + int_size_f - 1) < ncolumns

         && (arg_int2_r) >= 0
         && (arg_int2_r + int_size_i - 1) < nrows
         && (arg_int2_c) >= 0
         && (arg_int2_c + int_size_i - 1) < ncolumns) {
        flag_valid = TRUE;

    } else {
        flag_valid = FALSE;
    }


    if (flag_valid == TRUE) {
/*
 * reset int_area_1, int_area_2 values
 */
        memset(int_area_1[0], 0.0, (sizeof(gfloat)) * int_size_0 * int_size_0);
        memset(int_area_2[0], 0.0, (sizeof(gfloat)) * int_size_0 * int_size_0);

        for (m = 0; m < int_size_f; m++) {
            for (n = 0; n < int_size_f; n++) {
                int_area_1[m][n] =
                    (float) img_1[m + arg_int1_r][n + arg_int1_c];
            }
        }

        for (m = 0; m < int_size_i; m++) {
            for (n = 0; n < int_size_i; n++) {
                int_area_2[m][n] =
                    (float) img_2[m + arg_int2_r][n + arg_int2_c];
            }
        }
    }


    return flag_valid;
}



static gboolean
assign_img2intarr_central (gint ipoint_x,
                           gint ipoint_y,
                           guint16 **img_1,
                           guint16 **img_2,
                           gint int_size_f,
                           gint int_size_i,
                           gfloat **int_area_1,
                           gfloat **int_area_2,
                           gint pre_shift_row,
                           gint pre_shift_col,
                           gint nrows,
                           gint ncolumns,
                           gint int_size_0
                           )
/*-----------------------------------------------------------------------------
 * Assigns image data to the interrogation area arrays using the central 
 * differential scheme
 */
{
    gint m, n;
    gint idum = gpiv_max((int_size_i - int_size_f) / 2, 0);
    gint arg_int1_r = ipoint_y - int_size_f / 2 + 1 - pre_shift_row / 2 -
        pre_shift_row % 2;
    gint arg_int1_c = ipoint_x - int_size_f / 2 + 1 - pre_shift_col / 2 -
        pre_shift_col % 2;
    gint arg_int2_r = ipoint_y - int_size_i / 2 + 1 + pre_shift_row / 2;
    gint arg_int2_c = ipoint_x - int_size_i / 2 + 1 + pre_shift_col / 2;
    gboolean flag_valid;


    assert (img_1[0] != NULL);
    assert (img_2[0] != NULL);
    assert (int_area_1[0] != NULL);
    assert (int_area_2[0] != NULL);
/*
 * Check if Interrogation Areas are within the image boundaries.
 */
     if ((arg_int1_r) >= 0
         && (arg_int1_r + int_size_f - 1) < nrows
         && (arg_int1_c) >= 0
         && (arg_int1_c + int_size_f - 1) < ncolumns

         && (arg_int2_r) >= 0
         && (arg_int2_r + int_size_i - 1) < nrows
         && (arg_int2_c) >= 0
         && (arg_int2_c + int_size_i - 1) < ncolumns) {
         flag_valid = TRUE;
     } else {
         flag_valid = FALSE;
     }


    if (flag_valid == TRUE) {
/*
 * reset int_area_1, int_area_2 values
 */
        memset(int_area_1[0], 0.0, (sizeof(gfloat)) * int_size_0 * int_size_0);
        memset(int_area_2[0], 0.0, (sizeof(gfloat)) * int_size_0 * int_size_0);

        for (m = 0; m < int_size_f; m++) {
            for (n = 0; n < int_size_f; n++) {
                int_area_1[m + idum][n + idum] =
                    (float) img_1[m + arg_int1_r][n + arg_int1_c];
            }
        }


        for (m = 0; m < int_size_i; m++) {
            for (n = 0; n < int_size_i; n++) {
                int_area_2[m][n] =
                    (float) img_2[m + arg_int2_r][n + arg_int2_c];
            }
        }

    }


    return flag_valid;
}



static gboolean
assign_img2intarr_forward (gint ipoint_x,
                           gint  ipoint_y,
                           guint16 **img_1,
                           guint16 **img_2,
                           gint int_size_f,
                           gint int_size_i,
                           gfloat **int_area_1,
                           gfloat **int_area_2,
                           gint pre_shift_row,
                           gint pre_shift_col,
                           gint nrows,
                           gint ncolumns,
                           gint int_size_0
                           )
/*-----------------------------------------------------------------------------
 * Assigns image data to the interrogation area arrays for forward differential
 * scheme
 */
{
    gint m, n;
    gint idum = gpiv_max((int_size_i - int_size_f) / 2, 0);
    gint arg_int1_r = ipoint_y - int_size_f / 2 + 1 + pre_shift_row + idum;
    gint arg_int1_c = ipoint_x - int_size_f / 2 + 1 + pre_shift_col + idum;
    gint arg_int2_r = ipoint_y - int_size_i / 2 + 1 + pre_shift_row;
    gint arg_int2_c = ipoint_x - int_size_i / 2 + 1 + pre_shift_col;
    gboolean flag_valid;


    assert (img_1[0] != NULL);
    assert (img_2[0] != NULL);
    assert (int_area_1[0] != NULL);
    assert (int_area_2[0] != NULL);
/*
 * Check if Interrogation Areas are within the image boundaries.
 */
    if ((arg_int1_r) >= 0
        && (arg_int1_r + int_size_f - 1) < nrows
        && (arg_int1_c) >= 0
        && (arg_int1_c  + int_size_f - 1) < ncolumns

        && (arg_int2_r) >= 0
        && (arg_int2_r + int_size_i - 1) < nrows
        && (arg_int2_c) >= 0
        && (arg_int2_c  + int_size_i - 1) < ncolumns) {
        flag_valid = TRUE;
    } else {
        flag_valid = FALSE;
    }


    if (flag_valid == TRUE) {
/*
 * reset int_area_1, int_area_2 values
 */
        memset(int_area_1[0], 0.0,
               (sizeof(float)) * int_size_0 * int_size_0);
        memset(int_area_2[0], 0.0,
               (sizeof(float)) * int_size_0 * int_size_0);

        for (m = 0; m < int_size_f; m++) {
            for (n = 0; n < int_size_f; n++) {
                int_area_1[m + idum][n + idum] =
                    (float) img_1[m + arg_int1_r][n + arg_int1_c];
            }
        }


        for (m = 0; m < int_size_i; m++) {
            for (n = 0; n < int_size_i; n++) {
                int_area_2[m][n] =
                    (float) img_2[m + arg_int2_r][n + arg_int2_c];
            }
        }

    }


    return flag_valid;
}



static float
int_mean_old (guint16 **img,
              int int_size,
              int int_size_i,
              int ipoint_x,
              int ipoint_y
              )
/* ----------------------------------------------------------------------------
 * calculates mean image value from which image data are taken
 */
{
    int m = 0, n = 0, idum = gpiv_max((int_size_i - int_size) / 2, 0);
    int int_area_sum = 0;
    float mean;


    assert (img[0] != NULL);

    for (m = 0; m < int_size; m++) {
        for (n = 0; n < int_size; n++) {
            int_area_sum +=
                img[m + ipoint_y - int_size_i / 2 + 1 + idum]
                [n + ipoint_x - int_size_i / 2 + 1 + idum];
        }
    }

    mean = int_area_sum / (int_size * int_size);


    return mean;
}



static gfloat
int_mean (gfloat **int_area,
          gint int_size
          )
/* ----------------------------------------------------------------------------
 * calculates mean value from interrogation area intensities
 */
{
    int m = 0, n = 0;
    gfloat int_area_sum = 0;
    gfloat mean = 0.0;


    assert (int_area[0] != NULL);

    for (m = 0; m < int_size; m++) {
        for (n = 0; n < int_size; n++) {
            int_area_sum += int_area[m][n];
        }
    }

    mean = int_area_sum / (int_size * int_size);


    return mean;
}



static gfloat
int_range (gfloat **int_area,
          gint int_size
          )
/* ----------------------------------------------------------------------------
 * calculates range of values from interrogation area intensities
 */
{
    int m = 0, n = 0;
    gfloat int_area_sum = 0;
    gfloat min = 10.0e9, max = -10.0e9, range = 0.0;


    assert (int_area[0] != NULL);

    for (m = 0; m < int_size; m++) {
        for (n = 0; n < int_size; n++) {
            if (int_area[m][n] > max) max = int_area[m][n];
            if (int_area[m][n] < min) min = int_area[m][n];
        }
    }

    range = max - min;


    return range;
}



static gboolean
int_const (gfloat **int_area,
           const guint int_size
           )
/* ----------------------------------------------------------------------------
 * Tests if all intesities values with an interrogation area are equal
 */
{
    int m = 0, n = 0;
    gboolean flag = TRUE;
    gfloat val;


    assert (int_area[0] != NULL);

    val = int_area[0][0];
    for (m = 1; m < int_size; m++) {
        for (n = 1; n < int_size; n++) {
            if (int_area[m][n] != val) flag = FALSE;
        }
    }


    return flag;
}



static void
cov_min_max (GpivCov *cov
             )
/* ----------------------------------------------------------------------------
 * calculates minimum and maximum in cov
 */
{
    gfloat max = -10.0e+9, min = 10.0e+9;
    gint z_rl = cov->z_rl, z_rh = cov->z_rh, z_cl = cov->z_cl,
        z_ch = cov->z_ch;
    gint i, j;


    for (i = z_rl + 1; i < z_rh - 1; i++) {
        for (j = z_cl + 1; j < z_ch - 1; j++) {
	    if (cov->z[i][j] > max) max = cov->z[i][j];
	    if (cov->z[i][j] < min) min = cov->z[i][j];
        }
    }

    cov->min = min;
    cov->max = max;
}


static void
search_top (GpivCov *cov,
            gint peak_act,
            gint x_corr,
            gint sweep,
            gint i_skip_act,
            gint j_skip_act,
            float *z_max,
            long *i_max,
            long *j_max
            )
/* ----------------------------------------------------------------------------
 * searches top in cov. function
 */
{
    gint h, i, j;
    gint z_rl = cov->z_rl, z_rh = cov->z_rh, z_cl = cov->z_cl,
        z_ch = cov->z_ch;


    for (h = 1; h <= peak_act + 1; h++) {
	z_max[h] = -1.0;
	i_max[h] = -2;
	j_max[h] = -3;
    }

    for (h = 1; h <= peak_act; h++) {
	for (i = z_rl + 1; i < z_rh - 1; i++) {
	    for (j = z_cl + 1; j < z_ch - 1; j++) {

		if (x_corr == 1 || (sweep == 0 || (i != i_skip_act ||
						   j != j_skip_act))) {
		    if (h == 1
			|| (h == 2
			    && (i != i_max[1] || j != j_max[1]))
			|| (h == 3
			    && (i != i_max[1] || j != j_max[1])
			    && (i != i_max[2] || j != j_max[2]))) {
			if (cov->z[i][j] > z_max[h]) {
			    if ((cov->z[i][j] >= cov->z[i - 1][j]) &&
				(cov->z[i][j] >= cov->z[i + 1][j]) &&
				(cov->z[i][j] >= cov->z[i][j - 1]) &&
				(cov->z[i][j] >= cov->z[i][j + 1])) {
				z_max[h] = cov->z[i][j];
				i_max[h] = i;
				j_max[h] = j;
			    }
			}
		    }

		}
	    }
	}
    }
}



static char *
cov_subtop (float **z,
            long *i_max,
            long *j_max,
            float *epsi_x,
            float *epsi_y,
            int ifit,
            int peak_act
            )
/*-----------------------------------------------------------------------------
 * Calculates particle displacements at sub-pixel level
 */
{
    char *err_msg = NULL;
    double A_log, B_log, C_log;
    double min = 10e-3;
    gboolean flag = TRUE;


    if (ifit == GPIV_GAUSS) {
/*
 * sub-pixel estimator by gaussian fit
 */
/*         g_message ("cov_subtop:: z = %f", z[i_max[peak_act]][j_max[peak_act]]); */
        if (z[i_max[peak_act]][j_max[peak_act]] > min) {
            C_log = log((double) z[i_max[peak_act]][j_max[peak_act]]);
/*         g_message ("cov_subtop:: C_log"); */
        } else {
            flag = FALSE;
        }

        if (flag && z[i_max[peak_act] - 1][j_max[peak_act]] > min) {
            A_log = log((double) z[i_max[peak_act] - 1][j_max[peak_act]]);
/*             g_message ("cov_subtop:: A_log"); */
        } else {
            flag = FALSE;
        }

        if (flag && z[i_max[peak_act] + 1][j_max[peak_act]] > min) {
            B_log = log((double) z[i_max[peak_act] + 1][j_max[peak_act]]);
/*             g_message ("cov_subtop:: B_log"); */
        } else {
            flag = FALSE;
        }

        if (flag && (2 * (A_log + B_log - 2 * C_log)) != 0.0) {
            *epsi_y = (A_log - B_log) / (2 * (A_log + B_log - 2 * C_log));
        } else {
            *epsi_y = 0.0;
            peak_act = -1;
            err_msg = "epsi_y = 0.0";
/*             g_message ("cov_subtop:: %s", err_msg); */
            flag = FALSE;
        }


        if (flag && z[i_max[peak_act]][j_max[peak_act] - 1] != 0.0) {
            A_log = log((double) z[i_max[peak_act]][j_max[peak_act] - 1]);
        } else {
            flag = FALSE;
        }

        if (flag && z[i_max[peak_act]][j_max[peak_act] + 1] != 0.0) {
            B_log = log((double) z[i_max[peak_act]][j_max[peak_act] + 1]);
        } else {
            flag = FALSE;
        }

        if (flag && (2 * (A_log + B_log - 2 * C_log)) != 0.0) {
            *epsi_x = (A_log - B_log) / (2 * (A_log + B_log - 2 * C_log));
        } else {
            *epsi_x = 0.0;
            peak_act = -1;
            err_msg = "epsi_x = 0.0";
/*             g_message ("cov_subtop:: %s", err_msg); */
            flag = FALSE;
        }


    } else if (ifit == GPIV_POWER) {
/*
 * sub-pixel estimator by quadratic fit
 */
	*epsi_y = (z[i_max[peak_act] - 1][j_max[peak_act]] -
		   z[i_max[peak_act] + 1][j_max[peak_act]]) /
	    (2 * (z[i_max[peak_act] - 1][j_max[peak_act]] +
		  z[i_max[peak_act] + 1][j_max[peak_act]] -
		  2 * z[i_max[peak_act]][j_max[peak_act]]));

	*epsi_x = (z[i_max[peak_act]][j_max[peak_act] - 1] -
		   z[i_max[peak_act]][j_max[peak_act] + 1]) /
	    (2 * (z[i_max[peak_act]][j_max[peak_act] - 1] +
		  z[i_max[peak_act]][j_max[peak_act] + 1] -
		  2 * z[i_max[peak_act]][j_max[peak_act]]));


    } else if (ifit == GPIV_GRAVITY) {
/*
 * sub-pixel estimator by centre of gravity
 */
	*epsi_y = (z[i_max[peak_act] - 1][j_max[peak_act]] -
		   z[i_max[peak_act] + 1][j_max[peak_act]]) /
	    (z[i_max[peak_act] - 1][j_max[peak_act]] +
	     z[i_max[peak_act] + 1][j_max[peak_act]] +
	     z[i_max[peak_act]][j_max[peak_act]]);

	*epsi_x = (z[i_max[peak_act]][j_max[peak_act] - 1] -
		   z[i_max[peak_act]][j_max[peak_act] + 1]) /
	    (z[i_max[peak_act]][j_max[peak_act] - 1] +
	     z[i_max[peak_act]][j_max[peak_act] + 1] +
	     z[i_max[peak_act]][j_max[peak_act]]);


    } else {
	err_msg = "LIBGPIV internal error: cov_subtop: invalid fit parameter";
        gpiv_warning("%s", err_msg);
        return err_msg;
    }

/*     fprintf (stderr, "LIBGPIV cov_subtop:: epsi_y = %f epsi_x = %f\n", */
/*              *epsi_y, *epsi_x); */


        return err_msg;
}



static int
cov_top (GpivPivPar piv_par,
         GpivPivData * piv_data,
         int index_y,
         int index_x,
         GpivCov *cov,
         int x_corr,
         int ifit,
         int sweep,
         int last_sweep,
         int peak,
         int peak_act,
         int pre_shift_row_act,
         int pre_shift_col_act,
         int i_skip_act,
         int j_skip_act,
         gboolean *flag_subtop
         )
/* ----------------------------------------------------------------------------
 * detects location of peak and snr in correlation function
 */
{
#define INITIAL_MIN 9999.9
    char *err_msg = NULL;
    float z_min, *z_max, *z_max_next;
    int h, i, j, i_min, j_min;
    long *i_max, *j_max, *i_max_next, *j_max_next;

    int z_rl = cov->z_rl, z_rh = cov->z_rh, z_cl = cov->z_cl, z_ch = cov->z_ch;

    int ipoint_x = (int) piv_data->point_x[index_y][index_x];
    int ipoint_y = (int) piv_data->point_y[index_y][index_x];
/*     float epsi_x = 0.0, epsi_y = 0.0; */
    gboolean flag_snr = TRUE;
    gint dim = peak_act;


    i_max = gpiv_nulvector_index(1, dim + 1);
    j_max = gpiv_nulvector_index(1, dim + 1);
    z_max = gpiv_vector_index(1, dim + 1);
    i_max_next = gpiv_nulvector_index(1, dim + 2);
    j_max_next = gpiv_nulvector_index(1, dim + 2);
    z_max_next = gpiv_vector_index(1, dim + 2);

/*
 * BUGFIX: CHECK!!
 * finding a local top within the interrogation region. In case of
 * autocorrelation, exclude the first max (normally at i=0,j=0 if no
 * pre-shifting has been used), by increasing peak_act with 1 during the first
 * iteration sweep, then call it skip_act
 */

    if (sweep == 0 && x_corr == 0) {
	peak_act = peak + 1;
    } else {
	if (x_corr == 0)
	    peak_act = peak;
    }


/*     g_message("cov_top:: finding a local top 1"); */
    search_top (cov, peak_act, x_corr, sweep, i_skip_act, j_skip_act,
                z_max, i_max, j_max);

    for (h = 1; h <= peak_act + 1; h++) {
        if (z_max_next[h] == -1.0) {
            ifit = GPIV_NONE;
            flag_snr = FALSE;
/*             g_warning("cov_top:: No first top found at i = %d j = %d", */
/*                       index_y, index_x); */
        }
    }

/*
 * Define first max to be skipped if autocorr, eventually shift this
 * point with new pre-shifting values
 */


    if (x_corr == 0 && sweep == 0) {
	i_skip_act = i_max[1];
	j_skip_act = j_max[1];
    }

/* BUGFIX: don't calculate snr for the Challenge project */
/*     flag_snr = FALSE;   */

/*
 * Search next higher peak for SNR calculation
 */
    if (flag_snr) {
        search_top (cov, peak_act + 1, x_corr, sweep, i_skip_act, j_skip_act,
                    z_max_next, i_max_next, j_max_next);
    }

/*
 * Check if second top has been found
 */
    for (h = 1; h <= peak_act + 1; h++) {
        if (z_max_next[h] == -1.0) {
            flag_snr = FALSE;
        }
    }


    if (flag_snr
        && cov->z[i_max_next[peak_act + 1]][j_max_next[peak_act + 1]] != 0.0) {
        cov->snr = cov->z[i_max[peak_act]][j_max[peak_act]] - cov->min /
	(cov->z[i_max_next[peak_act + 1]][j_max_next[peak_act + 1]] - cov->min);
/*         if (last_sweep == 1) g_message("cov_top:: snr = %f",cov->snr); */
    } else {
        cov->snr = 0.0;
        piv_data->snr[index_y][index_x] = cov->snr;
/*         piv_data->peak_no[index_y][index_x] = -1; */
    }

/*
 * Searching of minimum around cov. peak_act and remove 'pedestal'
 */
    z_min = INITIAL_MIN;
    i_min = INITIAL_MIN;
    j_min = INITIAL_MIN;
    for (i = i_max[peak_act] - 1; i <= i_max[peak_act] + 1; i++) {
	for (j = j_max[peak_act] - 1; j <= j_max[peak_act] + 1; j++) {
	    if ((i >= z_rl && i <= z_rh) && (j >= z_cl && j <= z_ch)) {
		if (cov->z[i][j] < z_min) {
		    z_min = cov->z[i][j];
		    i_min = i;
		    j_min = j;
		}
	    }
	}
    }

    if (z_min <= INITIAL_MIN) {
        for (i = i_max[peak_act] - 1; i <= i_max[peak_act] + 1; i++) {
            for (j = j_max[peak_act] - 1; j <= j_max[peak_act] + 1; j++) {
/*           cov->z[i][j] = cov->z[i][j]-z_min; */
                cov->z[i][j] = cov->z[i][j] - z_min + 0.1;
            }
        }
    } else {
        ifit = GPIV_NONE;
    }

/*
 * Calculate particle displacement at integer pixel numbers or at sub-pixel
 */
    if (ifit == GPIV_NONE) {
        cov->subtop_x = 0.0;
        cov->subtop_y = 0.0;

    } else {
            if ((err_msg = cov_subtop (cov->z, i_max, j_max, &cov->subtop_x,
                                       &cov->subtop_y, ifit, peak_act))
                != NULL) {
                g_message("%s", err_msg);
                *flag_subtop = TRUE;
            }
    }

/*
 * Writing maximuma  to cov
 */
    cov->top_y = i_max[peak_act];
    cov->top_x = j_max[peak_act];

/*
 * Free memeory
 */
    gpiv_free_nulvector_index(i_max, 1, dim + 1);
    gpiv_free_nulvector_index(j_max, 1, dim + 1);
    gpiv_free_vector_index(z_max, 1, dim + 1);
    gpiv_free_nulvector_index(i_max_next, 1, dim + 2);
    gpiv_free_nulvector_index(j_max_next, 1, dim + 2);
    gpiv_free_vector_index(z_max_next, 1, dim + 2);

    return 0;
}



static
void pack_cov (float **covariance,
               GpivCov *cov,
               int int_size_0
               )
/*-----------------------------------------------------------------------------
 * Packs the unordered covariance data in an ordered sequence when returning
 * from cova_nr
 */
{
    int i, j;
    int z_rnl = cov->z_rnl, z_rnh = cov->z_rnh, z_rpl = cov->z_rpl, 
        z_rph = cov->z_rph;
    int z_cnl = cov->z_cnl, z_cnh = cov->z_cnh, z_cpl = cov->z_cpl, 
        z_cph = cov->z_rph;


    for (i = z_rnl; i < z_rnh; i++) {
	for (j = z_cnl; j < z_cnh; j++) {
	    cov->z[i - int_size_0][j - int_size_0] = covariance[i][j];
	}
	for (j = z_cpl; j < z_cph; j++) {
	    cov->z[i - int_size_0][j] = covariance[i][j];
	}
    }

    for (i = z_rpl; i < z_rph; i++) {
	for (j = z_cnl; j < z_cnh; j++) {
	    cov->z[i][j - int_size_0] = covariance[i][j];
	}
	for (j = z_cpl; j < z_cph; j++) {
	    cov->z[i][j] = covariance[i][j];
	}
    }
}



static void
weight_cov (GpivCov *cov,
            GpivCov *w_k
            )
/*-----------------------------------------------------------------------------
 * Corrects ordered covariance data with weighting kernel
 */
{
    int i, j;
    int z_rl = w_k->z_rl, z_rh = w_k->z_rh;
    int z_cl = w_k->z_cl, z_ch = w_k->z_ch;


    if (w_k == NULL) {
        g_message("LIBGPIV internal error: weight_cov: w_k = NULL");
        return;
    }

    for (i = z_rl; i < z_rh; i++) {
	for (j = z_cl; j < z_ch; j++) {
	    cov->z[i][j] = cov->z[i][j] / w_k->z[i][j];
	}
    }

}


static gchar *
filter_cov_spof (fftw_complex *a, 
                 fftw_complex *b,
                 gint m,
                 gint n
                 )
/*-----------------------------------------------------------------------------
 * Applies Symmetric Phase Only filtering on the complex arrays a and b
 *
 * REFERENCES:
 *     M.P. Wernet, "Symmetric phase only filtering: a new paradigm for DPIV 
 *     data processing", Meas. Sci. Technol, vol 16 (2005), pp 601 - 618
 */
{
    gchar *err_msg = NULL;
    gfloat amplitude_a, amplitude_b;
    gint i, j, ij;


    /*         assert (a[0] != NULL); */
    /*         assert (b[0] != NULL); */

    for (i = 0; i < m; i++) {
        for (j = 0; j < n / 2 + 1; j++) {
            ij = i * (n / 2 + 1) + j;
            amplitude_a = sqrt(a[ij][0] * a[ij][0] + a[ij][1] * a[ij][1]);
            amplitude_b = sqrt(b[ij][0] * b[ij][0] + b[ij][1] * b[ij][1]);

            if (amplitude_a == 0.0 || amplitude_b == 0.0) {
                a[ij][0] = 0.0;
                a[ij][1] = 0.0;
                b[ij][0] = 0.0;
                b[ij][1] = 0.0;
            } else {
                a[ij][0] /= sqrt(amplitude_a) * sqrt(amplitude_b);
                a[ij][1] /= sqrt(amplitude_a) * sqrt(amplitude_b);
                b[ij][0] /= sqrt(amplitude_a) * sqrt(amplitude_b);
                b[ij][1] /= sqrt(amplitude_a) * sqrt(amplitude_b);
            }
        }
    }


    return err_msg;
}



static gchar *
cova (int int_size,
      float **int_area_1,
      float **int_area_2,
      GpivCov *cov,
      gboolean spof_filter
      )
/*-----------------------------------------------------------------------------
 * Calculates the covariance function of int_area_1 and int_area_2 by
 * means of Fast Fourier Transforms.
 */
{
    gchar *err_msg = NULL;
    int M = GPIV_ZEROPAD_FACT * int_size, N = GPIV_ZEROPAD_FACT * int_size;
    float covariance_max, covariance_min;
    gint i, j;
    float **covariance;

    double **a, **b, **c;
    fftw_complex  *A, *B, **C;

    fftw_plan  plan/* , plan_a, plan_b, plan_c */;
    gdouble scale = 1.0 / (M * N);


#ifdef DEBUG
    FILE *fp = my_utils_fopen_tmp (GPIV_LOGFILE, "w");
#endif
#ifdef USE_MTRACE
    /*     printf ("LIBGPIV:: cova with mtrace\n"); */
    mtrace();
#endif

#ifdef _OPENMP
/*     gpiv_warning ("cova:: 1 ===> nthreads = %d", omp_get_thread_num()); */
    fftw_plan_with_nthreads(omp_get_thread_num());
/*     fftw_plan_with_nthreads(2); */
#endif

    g_assert (int_area_1[0] != NULL);
    g_assert (int_area_2[0] != NULL);
    g_assert (cov != NULL);

    a = gpiv_double_matrix(M, 2 * (N / 2 + 1));
    A = (fftw_complex *) &a[0][0];
    b = gpiv_double_matrix(M, 2 * (N / 2 + 1));
    B = (fftw_complex *) &b[0][0];
    c = gpiv_double_matrix(M, N);
    C = gpiv_fftw_complex_matrix(M, N /2 + 1);

    covariance = gpiv_matrix(M, N);
    /* BUGFIX: may be removed */
    memset(covariance[0], 0, (sizeof(float)) * M * N);

    /*
     * FFT both interrogation areas
     */
#ifdef DEBUG
    fprintf (fp, "New I.A:\n");
#endif
#pragma omp parallel for
    for (i = 0; i < M; ++i) {
	for (j = 0; j < N; ++j) {
	    a[i][j] = (double) int_area_1[i][j];
	    b[i][j] = (double) int_area_2[i][j];
#ifdef DEBUG
            fprintf (fp, "cova:: int_area_1[%d][%d] = %f  a[%d][%d] = %f  int_area_2[%d][%d] = %f  b[%d][%d] = %f\n", 
                     i, j, int_area_1[i][j],
                     i, j, a[i][j],
                     i, j, int_area_2[i][j],
                     i, j, b[i][j]
                     );
#endif /* DEBUG */
        }
    }

#ifdef DEBUG
    fprintf (fp, "\n\n");
#endif

    /*
     * make plans and perform FT
     */
    plan = fftw_plan_dft_r2c_2d(M, N, (double *) &a[0][0], 
                                (fftw_complex *) &a[0][0], 
                                FFTW_MEASURE);
    fftw_execute(plan);
    fftw_destroy_plan(plan);
    /*     fftw_forget_wisdom(); */


    plan = fftw_plan_dft_r2c_2d(M, N, (double *) &b[0][0], 
                                (fftw_complex *) &b[0][0], 
                                FFTW_MEASURE);
    fftw_execute(plan);
    fftw_destroy_plan(plan);

    if (spof_filter) {
        if ((err_msg = filter_cov_spof(A, B, M, N)) != NULL) {
            return (err_msg);
        }
    }

    /*
     * B * conjugate(A) result in correct sign of displacements!
     */
#pragma omp parallel for
    for (i = 0; i < M; ++i) {
	for (j = 0; j < N / 2 + 1; ++j) {
	    gint ij = i * (N / 2 + 1) + j;
	    C[i][j][0] = (B[ij][0] * A[ij][0] + B[ij][1] * A[ij][1]) * scale;
	    C[i][j][1] = (B[ij][1] * A[ij][0] - B[ij][0] * A[ij][1]) * scale;
#ifdef DEBUG
            fprintf (fp, "cova:: A[%d]_re = %f A[%d]_im = %f  B[%d]_re = %f B[%d]_im = %f\n",
                     ij, A[ij][0], ij, A[ij][1],
                     ij, B[ij][0], ij, B[ij][1]
                     );
#endif
	}
    }
#ifdef DEBUG
    fprintf (fp, "\n\n");
#endif

    /*
     * inverse transform to get c, the covariance of a and b;
     * this has the side effect of overwriting C
     */
    plan = fftw_plan_dft_c2r_2d(M, N, (fftw_complex *) &C[0][0], 
                                (double *) &c[0][0],
                                FFTW_MEASURE);
    fftw_execute(plan);
    fftw_destroy_plan(plan);

    /*
     * Put the data back in a 2-dim array covariance[][]
     */
#pragma omp parallel for
    for (i = 0; i < M; i++) {
	for (j = 0; j < N; j++) {
	    covariance[i][j] = (float) c[i][j];
            /*             fprintf (fp, "cova:: c[%d][%d] = %f\n", i, j, c[i][j]); */
	}
    }
#ifdef DEBUG
    fprintf (fp, "\n\n");
#endif

    /*
     * normalisation => correlation function
     */
    covariance_max = -10.0e+9;
    covariance_min = 10.0e+9;
#pragma omp parallel for
    for (i = 0; i < M; i++) {
	for (j = 0; j < N; j++) {
	    if (covariance[i][j] > covariance_max)
		covariance_max = covariance[i][j];
	    if (covariance[i][j] < covariance_min)
		covariance_min = covariance[i][j];
	}
    }

#pragma omp parallel for
    for (i = 0; i < M; i++) {
	for (j = 0; j < N; j++) {
	    covariance[i][j] = covariance[i][j] / covariance_max;
	}
    }


    /*
     * Packing the unordered covariance data into the ordered array of
     * Covariance structure
     */
    pack_cov(covariance, cov, M);
    /* BUGFIX: may be changed */
    cov_min_max(cov);
    /*     cov->min = covariance_min; */
    /*     cov->max = covariance_max; */


    /*
     * free mems
     */
    gpiv_free_matrix (covariance);
    gpiv_free_fftw_complex_matrix (C);
    gpiv_free_double_matrix (c);
    gpiv_free_double_matrix (b);
    gpiv_free_double_matrix (a);
    A = NULL;
    B = NULL;

    /*
     * BUGFIX: fftw_cleanup really slows down!
     */
    /*     fftw_forget_wisdom(); */
    /*     fftw_cleanup(); */


#ifdef DEBUG
    fclose(fp);
#endif
#ifdef USE_MTRACE
    muntrace();
#endif
    return err_msg;
}



static gchar *
ia_weight_gauss (gint int_size, 
                 float **int_area
                 )
/**----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Weights Interrogation Area's
 */
{
    gchar *err_msg = NULL;
    gint i = 0, j = 0;
    gdouble weight;


    assert (int_area[0] != NULL);

    for (i = 0; i < int_size; i++) {
        for (j = 0; j < int_size; j++) {
            weight = exp( -(((double)i - (double)int_size / 2.0) 
                                    * ((double)i - (double)int_size / 2.0)
                                    + ((double)j - (double)int_size / 2.0) 
                                    * ((double)j - (double)int_size / 2.0))
                          / (2.0 * (double)int_size * (double)int_size));
/*             g_message("weight_ia:: 2a int_size = %d i = %d j = %d weight = %f", */
/*                       int_size, i, j, weight); */
            int_area[i][j] *= weight;
        }
    }


    return err_msg;
}


/*
 * From piv_speed
 */
static void
nearest_point (gint i,
               gfloat x, 
               gfloat point_x, 
               gfloat *min, 
               gint *index, 
               gboolean *exist
               )
/*-----------------------------------------------------------------------------
 * Test if current point_x is nearest from x
 */
{
    gfloat dif = abs (x - point_x);

    if (dif < *min) {
        *exist = TRUE;
        *min = dif;
        *index = i;
    }

}


static gboolean
nearest_index (enum Position pos,
               gint vlength,
               gfloat *src_point, 
               gfloat dest_point,
               gint *index
               )
/*-----------------------------------------------------------------------------
 * Search nearest index from piv_data belonging to point (x, y)
 * in horizontal direction
 * pos_x/y index left/right, top/bottom of point
 */
{
    gint i, j;
    gfloat min = 10.0e4;
    gboolean exist = FALSE;

    *index = 0;
/* g_message ("nearest_index::  dest_point=%f", dest_point); */
    for (i = 0; i < vlength; i++) {
/* g_message ("nearest_index::  src_point[%d]=%f", i, src_point[i]); */

        if (pos ==  LOWER
            && src_point[i] <= dest_point) {
            nearest_point (i, dest_point, src_point[i], &min, 
                           index, &exist);
/* g_message ("nearest_index::  index_l=%d", *index); */

        } else if (pos == NEXT_LOWER
                   &&  i > 0
                   && src_point[i - 1] < dest_point) {
 
            nearest_point (i - 1, dest_point, src_point[i - 1], &min, 
                           index, &exist);
/* g_message("nearest_index::  src_point[%d]=%f index_ll=%d", */
/*           i-1, src_point[i - 1], *index); */

        } else if (pos == HIGHER
                   && src_point[i] > dest_point) {
            nearest_point (i, dest_point, src_point[i], &min, index, &exist);
/* g_message("nearest_index::  index_h=%d", *index); */
            
        } else if (pos ==  NEXT_HIGHER
                   && i <  vlength - 1 
                   && src_point[i + 1] > dest_point) {
            nearest_point (i + 1, dest_point, src_point[i + 1], 
                           &min, 
                           index, &exist);
/* g_message ("nearest_index::  src_point[%d]=%f index_hh=%d", */
/*           i+1, src_point[i + 1], *index); */
            
        }
        
    }

    return exist;
}



static gdouble
bilinear_interpol (gdouble alpha_hor,
                   gdouble alpha_vert,
                   gdouble src_dx_nw,
                   gdouble src_dx_ne,
                   gdouble src_dx_sw,
                   gdouble src_dx_se
                   )
/*-----------------------------------------------------------------------------
 * Bilinear interpolation of src_dx_*
 * _ne: NORTH_EAST
 * _nw: NORTH_WEST
 * _se: SOUTH_EAST
 * _SW: SOUTH_WEST
 */
{
    gdouble dx, dx_n, dx_s;
    dx_n = (1.0 - alpha_hor) * src_dx_nw + alpha_hor * src_dx_ne;
    dx_s = (1.0 - alpha_hor) * src_dx_sw + alpha_hor * src_dx_se;
    dx =  (1.0 - alpha_vert) * dx_n + alpha_vert * dx_s;

/*     g_message ("bilinear_interpol:: alpha_hor=%f alpha_vert=%f, dx_n = %f dx_s = %f => dx = %f",  */
/*               alpha_hor, alpha_vert, dx_n, dx_s, dx); */
    return dx;
}



static void *
intpol_facts (gfloat *src_point, 
              gfloat *dest_point, 
              gint src_vlength,
              gint dest_vlength,
              gint *index_l,
              gint *index_h,
              gint *index_ll,
              gint *index_hh,
              double *alpha
              )
/*-----------------------------------------------------------------------------
 * calculates normalized interpolation factors for piv_data_src
 * Think of:
 *          _l (LOWER) is used for _w (WEST) or _n (NORTH),
 *          _h (HIGHER) is used for _e (EAST) or _s (SOUTH)
 *          _ll (NEXT_LOWER) is used for _ww (WEST_WEST) or _nn (NORTH_NORTH),
 *          _hh (NEXT_HIGHER) is used for _ee (EAST_EAST) or _ss (SOUTH_SOUTH)
 */
{
    gboolean *exist_l, *exist_h, *exist_ll, *exist_hh;
    double *dist_l, *dist_h, *dist_ll, *dist_hh;
    enum Position pos;
    gint i;

    exist_l = gpiv_gbolvector (dest_vlength);
    exist_h = gpiv_gbolvector (dest_vlength);
    exist_ll = gpiv_gbolvector (dest_vlength);
    exist_hh = gpiv_gbolvector (dest_vlength);

    dist_l = gpiv_dvector (dest_vlength);
    dist_h = gpiv_dvector (dest_vlength);
    dist_ll = gpiv_dvector (dest_vlength);
    dist_hh = gpiv_dvector (dest_vlength);

/*     g_warning ("intpol_facts:: 1, src_vlengthth=%d dest_vlength=%d",  */
/*               src_vlength, dest_vlength); */
/*
 * Searching adjacent and next to adjacent points of dest_point in src_point
 * data array
 */
    for (i = 0; i < dest_vlength; i++) {
/* g_message ("intpol_facts::   (next) adjacent point for dest_point[%d]=%f",  */
/*           i, dest_point[i]); */
        pos = LOWER;
        exist_l[i] = FALSE;
        if (exist_l[i] = 
            nearest_index (pos, 
                           src_vlength, 
                           src_point,
                           dest_point[i],
                           &index_l[i])) {
/*             g_message ("intpol_facts:: index_l[%d]=%d", i, index_l[i]); */
            dist_l[i] = dest_point[i] - src_point[index_l[i]];
        } else {
/*
 * To be used for extrapolation in negative direction 
 * by applying higher and next to higher points of src data
 */
            pos = NEXT_HIGHER;
            exist_hh[i] = FALSE;
            if (exist_hh[i] = 
                nearest_index (pos, 
                               src_vlength, 
                               src_point,
                               dest_point[i],
                               &index_hh[i])) {
/*                 g_message ("intpol_facts:: index_hh[%d]=%d", i, index_hh[i]); */
                dist_hh[i] = dest_point[i] - src_point[index_hh[i]];
            }
        }
        


        pos = HIGHER;
        exist_h[i] = FALSE;
        if (exist_h[i] = 
            nearest_index (pos, 
                           src_vlength, 
                           src_point,
                           dest_point[i],
                           &index_h[i])) {
/*                 g_message ("intpol_facts:: index_h[%d]=%d", i, index_h[i]); */
            dist_h[i] = dest_point[i] - src_point[index_h[i]];
        } else {

/*
 * To be used for extrapolation in positive direction 
 * by applying lower and next to lower points of src data
 */
            pos = NEXT_LOWER;
            exist_ll[i] = FALSE;
            index_ll[i] = 0;
            if (exist_ll[i] = 
                nearest_index (pos, 
                               src_vlength, 
                               src_point,
                               dest_point[i],
                               &index_ll[i])) {
/*                 g_message ("intpol_facts:: index_ll[%d]=%d, index_l[%d]=%d",  */
/*                           i, index_ll[i], i, index_l[i]); */
                dist_ll[i] = dest_point[i] - src_point[index_ll[i]];
            }
        }

/*
 * calculating of weight factors for inter- or extrapolation
 */

/* g_message ("intpol_facts:: weight factors for dest_point[%d]=%f",  */
/*           i, dest_point[i]); */
        if (exist_l[i] && exist_h[i]) {
/* g_message ("intpol_facts:: index_l[%d]=%d index_h[%d]=%d src_point_l=%f src_point_h=%f",  */
/*           i, index_l[i], i, index_h[i],  */
/*           src_point[index_l[i]], src_point[index_h[i]]); */
/*
 * Inner point: bilinear interpolation
 */
            if (src_point[index_l[i]] != src_point[index_h[i]]) {
                alpha[i] = dist_l[i] / 
                    (src_point[index_h[i]] - src_point[index_l[i]]);
            } else {
                alpha[i] = 1.0;
            }
            

        } else if (exist_l[i] && exist_ll[i] && !exist_h[i]) {
/* 
 * extrapolation from two lower values
 */
/* g_message ("intpol_facts:: index_l[%d]=%d index_ll[%d]=%d src_point_l=%f src_point_ll=%f",  */
/*           i, index_l[i], i, index_ll[i],  */
/*           src_point[index_l[i]], src_point[index_ll[i]]); */

            if (src_point[index_ll[i]] != src_point[index_l[i]]) {
                alpha[i] = dist_ll[i] / 
                    (src_point[index_l[i]] - src_point[index_ll[i]]);
                index_h[i] = index_l[i];
                index_l[i] = index_ll[i];
            } else {
                alpha[i] = 1.0;
            }


        } else if (!exist_l[i] && exist_h[i] && exist_hh[i]) {
/* 
 * extrapolation from two higher values
 */
/* g_message ("intpol_facts:: index_h=%d index_hh=%d src_point_h=%f src_point_hh=%f",  */
/*           index_h[i], index_hh[i], src_point[index_h[i]],  */
/*           src_point[index_hh[i]]); */

            if (src_point[index_hh[i]] != src_point[index_h[i]]) {
                alpha[i] = dist_h[i] / 
                    (src_point[index_hh[i]] - src_point[index_h[i]]);
                index_l[i] = index_h[i];
                index_h[i] = index_hh[i];
            } else {
                alpha[i] = 1.0;
            }
            

        } else {
            alpha[i] = 1.0;
        }
    }


    gpiv_free_gbolvector (exist_l);
    gpiv_free_gbolvector (exist_h);
    gpiv_free_gbolvector (exist_ll);
    gpiv_free_gbolvector (exist_hh);
    
    gpiv_free_dvector (dist_l);
    gpiv_free_dvector (dist_h);
    gpiv_free_dvector (dist_ll);
    gpiv_free_dvector (dist_hh);
 
}


static void
dxdy_at_new_grid_block (const GpivPivData *piv_data_src, 
                        GpivPivData *piv_data_dest,
                        gint expansion_factor,
                        gint smooth_window
                        )
/*-----------------------------------------------------------------------------
 * Calculates displacements from old to new grid, that has been expanded by
 * factor 2 and avarages with smoothing window. Works only correct if all neighbours 
 * at equal distances
 */
{
    int i, j, k, l, ef = expansion_factor, sw = smooth_window;
    int count = 0;
    GpivPivData *pd = NULL;

    pd = gpiv_alloc_pivdata (piv_data_dest->nx, piv_data_dest->ny);

/*
 * Copy blocks of 2x2 input data to pd 
 */
    for (i = 0; i < piv_data_src->ny; i++) {
        for (j = 0; j < piv_data_src->nx; j++) {
            for (k = 0; k < 2; k++) {
                if (ef * i+k < pd->ny) {
                    for (l = 0; l < 2; l++) {
                        if (ef * j+l < pd->nx) {
                            pd->dx[ef * i+k][ef * j+l] = piv_data_src->dx[i][j];
                            pd->dy[ef * i+k][ef * j+l] = piv_data_src->dy[i][j];
                        }
                    }
                }
            }   
        }
    }

/*
 * smooth the data
 */
    for (i = 0; i < piv_data_src->ny; i++) {
        for (j = 0; j < piv_data_src->nx; j++) {
            count = 0;
            for (k = -sw + 1; k < sw; k++) {
                if (i + k > 0 && i + k < pd->ny) {
                    for (l = -sw + 1; l < sw; l++) {
                        if (j + l > 0 && j + l < pd->ny) {
                            count++;
                            piv_data_dest->dx[i][j] += pd->dx[i+k][j+l];
                            piv_data_dest->dy[i][j] += pd->dy[i+k][j+l];
                        }
                    }
                }
            }
            piv_data_dest->dx[i][j] = piv_data_dest->dx[i][j] / (float)count;
            piv_data_dest->dy[i][j] = piv_data_dest->dx[i][j] / (float)count;
        }
    }

    gpiv_free_pivdata (pd);
}



static gchar *
update_pivdata_imgdeform_zoff (const GpivImage *image, 
                               GpivImage *lo_image, 
                               const GpivPivPar *piv_par, 
                               const GpivValidPar *valid_par, 
                               GpivPivData *piv_data, 
                               GpivPivData *lo_piv_data, 
                               gfloat *cum_residu, 
                               gboolean *cum_residu_reached,
                               gfloat *sum_dxdy, 
                               gfloat *sum_dxdy_old,
                               gboolean isi_last,
                               gboolean grid_last,
                               gboolean sweep_last,
                               gboolean verbose
                               )
/*-----------------------------------------------------------------------------
 * Validates and updates / renews pivdata and some other variables when image 
 * deformation or zero-offset interrogation scheme is used
 */
{

    gchar *err_msg = NULL;


    /*
     * Test on outliers
     */
    if ((err_msg = 
         gpiv_valid_errvec (lo_piv_data, 
                            image, 
                            piv_par,
                            valid_par, 
                            TRUE))
        != NULL) {
        return err_msg;
    }
            

    if (piv_par->int_scheme == GPIV_IMG_DEFORM) {

        /*
         * Update PIV estimators with those from the last interrogation
         * Resetting local PIV estimators for eventual next interrogation
         */
        if ((err_msg = gpiv_add_dxdy_pivdata (lo_piv_data, piv_data))
            != NULL) {
            return err_msg;
        }

        if ((err_msg = gpiv_0_pivdata (lo_piv_data))
            != NULL) {
            return err_msg;
        }

        /*
         * Deform image with updated PIV estimators.
         * First, copy local from original image.
         * Deform with newly updated PIV estimators.
         * Eventually write deformed image.
         */

        if ((err_msg = gpiv_cp_img_data (image, lo_image))
            != NULL) {
            return err_msg;
        }

        if ((err_msg = gpiv_imgproc_deform (lo_image, piv_data))
            != NULL) {
            return err_msg;
        }
/* #define DEBUG */
#ifdef DEBUG
        if (sweep_last && verbose) {
            printf ("\n");
            if ((err_msg = 
                 my_utils_write_tmp_image (lo_image, GPIV_DEFORMED_IMG_NAME,
                                           "Writing deformed image to:"))
                != NULL) {
                return err_msg;
            }
        }
#endif /* DEBUG */
/* #undef DEBUG */

    } else {

        /*
         * Renew PIV estimators with those from the last interrogation
         */
        if ((err_msg = gpiv_0_pivdata (piv_data))
            != NULL) {
            return err_msg;
        }
        if ((err_msg = gpiv_add_dxdy_pivdata (lo_piv_data, piv_data))
            != NULL) {
            return err_msg;
        }
    }

    /*
     * Checking the relative cumulative residu for convergence
     * if final residu has been reached, cum_residu_reached will be set.
     */
    if (isi_last && grid_last) {
        *sum_dxdy_old = *sum_dxdy;
        *sum_dxdy = 0.0;
        gpiv_sum_dxdy_pivdata (piv_data, sum_dxdy);
        *cum_residu = fabsf ((*sum_dxdy - *sum_dxdy_old) / 
                            ((gfloat)piv_data->nx * (gfloat)piv_data->ny));
        if (*cum_residu < GPIV_CUM_RESIDU_MIN) {
            *cum_residu_reached = TRUE;
        }
    }


    return err_msg;
}



#undef NMIN_TO_INTERPOLATE


#ifdef ENABLE_MPI
static GpivPivData *
piv_interrogate_img__scatterv_pivdata(GpivPivData *piv_data)
/*---------------------------------------------------------------------------------------
 * Scatter the piv_data equally over its rows.
 *
 * The number of rows in piv_data need not be a multiple of nprocs. 
 * Therefore, the first (piv_data->ny)%nprocs  processes get 
 * ceil(piv_data->ny/np/nprocs) rows, and the remaining processes get
 * floor(piv_data->ny)/nprocs) rows.
 */
{
    GpivPivData *pd = NULL, *mpi_piv_data = NULL;
    gint i, nprocs, rank;
    gint *counts = NULL, *displs = NULL;


    MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

#ifdef DEBUG
    if (rank ==0) g_message ("gpiv_piv_interrogate_img:: nx=%d ny=%d nprocs = %d", 
                             piv_data->nx, piv_data->ny, nprocs);
#endif

    counts = gpiv_piv_mpi_compute_counts(piv_data->nx, piv_data->ny);
    displs = gpiv_piv_mpi_compute_displs(counts, piv_data->nx, piv_data->ny);
    mpi_piv_data = gpiv_cp_pivdata (piv_data);
    gpiv_free_pivdata (piv_data);

    for (i = 0; i < nprocs; i++) {
        if (rank == i) pd = gpiv_alloc_pivdata(mpi_piv_data->nx, counts[i] / mpi_piv_data->nx);
    }

    gpiv_piv_mpi_scatterv_pivdata (mpi_piv_data, pd, counts, displs);


    gpiv_free_pivdata (mpi_piv_data);
    gpiv_free_ivector (counts);
    gpiv_free_ivector (displs);


    return pd;
}



static GpivPivData *
piv_interrogate_img__gatherv_pivdata(GpivPivData *lo_piv_data, 
                                     GpivPivData *piv_data)
/*---------------------------------------------------------------------------------------
 * Gathers the piv_data equally over its rows. 
 * Counterpart of piv_interrogate_img__scatterv_pivdata
 *
 * The number of rows in piv_data need not be a multiple of nprocs. 
 * Therefore, the first (piv_data->ny)%nprocs  processes get 
 * ceil(piv_data->ny/np/nprocs) rows, and the remaining processes get
 * floor(piv_data->ny)/nprocs) rows.
 */
{
    GpivPivData *pd = NULL, *mpi_piv_data = NULL;
    gint *counts = NULL, *displs = NULL;


    counts = gpiv_piv_mpi_compute_counts(piv_data->nx, piv_data->ny);
    displs = gpiv_piv_mpi_compute_displs(counts, piv_data->nx, piv_data->ny);
    mpi_piv_data = gpiv_alloc_pivdata(piv_data->nx, piv_data->ny);
    gpiv_piv_mpi_gatherv_pivdata (lo_piv_data, mpi_piv_data, counts, displs);
    gpiv_free_ivector (counts);
    gpiv_free_ivector (displs);
    gpiv_free_pivdata (lo_piv_data);
    pd = gpiv_cp_pivdata (mpi_piv_data);
    gpiv_free_pivdata (mpi_piv_data);


    return pd;
}




guint
substr_noremain(guint n, 
                guint m)
/*-------------------------------------------------------------------
 * Substracts 1 while remainder of n not equal to zero
 */
{
    while (fmod((double) n, (double) m) != 0) {
        n--;
    }


    return (guint) n;
}



#endif /* ENABLE_MPI */