/* -*- 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:                valid.c
LIBRARY:                 libgpiv
EXTERNAL FUNCTIONS:      gpiv_valid_residu
                         gpiv_valid_peaklck
                         gpiv_valid_residu_stats
                         gpiv_valid_errvec
			 gpiv_valid_gradient
                         gpiv_valid_threshold
                         gpiv_cumhisto_eqdatbin_gnuplot

LAST MODIFICATION DATE: $Id: valid.c,v 1.25 2008-09-25 13:19:53 gerber Exp $
--------------------------------------------------------------------- */
#include <gpiv.h>

#define SNR_ERR 99.0
#define MIN_VECTORS4MEDIAN 3    /* Minimum number of valid vectors needed 
                                   to calculate median */
#define RESIDU_EPSI 0.1

/*
 * Local functions
 */

static int
compare_float (const void * a,
               const void * b)
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Compares two float numbers
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     a:              first float number
 *     b:              second float number
 *
 * OUTPUTS:
 *
 * RETURNS:
 *     int:            -1, 0 or +1 for a < b, a = b or a > b respectively
 *---------------------------------------------------------------------------*/
{
    float *la = (float *) a, *lb = (float *) b;

    if (*la > *lb)
       return 1;
    else if (*la < *lb)
       return -1;
    else
       return 0;
}


static float
median_residu (const guint i, 
               const guint j, 
               const guint neighbors, 
               const gboolean incl_point,
               const gboolean norm,
               const GpivPivData *data, 
               guint *i_median_x, 
               guint *j_median_x,
               guint *i_median_y, 
               guint *j_median_y
              )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     If incl_point. calculates residu value of point i, j from a NxN data 
 *     array out of data[i][j] and the indices of the median velocity
 *     If not incl_point, returns the median residu from NxN data array, 
 *     calculated from dx(i) - dx(median), i,= 1, ..,8
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     i:              horizontal index of data to be investigated
 *     j:              vertical index of data to be investigated
 *     neighbors:      number of neighboring PIV data (typically 3x3, 5x5)
 *     data:           piv dataset
 *     incl_point:     flag to include data point under investigation
 *     norm:           flag to return residu for normalizing
 *
 * OUTPUTS:
 *    i_median_x:      i-index of median for x displacement
 *    j_median_x:      j-index of median for x displacement
 *    i_median_y:      i-index of median for y displacement
 *    j_median_y:      j-index of median for y displacement
 *
 * RETURNS:
 *    median residu
 *---------------------------------------------------------------------------*/
{
    int k, l, m = 0;
    gint vlength = 0, median_index;
    const gint N = neighbors - 2;
    float *dx_m, *dy_m, *dx_org, *dy_org;
    float *r_m;
    float residu;

/*
 * Obtain length of array
 */
#pragma parallel for shared(data, incl_point, vlength) private(k, l)
    for (k = i - N; k <= i + N; k++) {
	if ((k >= 0) && (k < data->ny)) {
	    for (l = j - N; l <= j + N; l++) {
		if ((l >= 0) && (l < data->nx)) {
                    if ((incl_point
                         && data->peak_no[k][l] != -1)
                        || (!incl_point 
                            && (k != i || l != j)
                            && data->peak_no[k][l] != -1) ) {
			vlength++;
		    }
		}
	    }
	}
    }

    if (data->peak_no[i][j] != -1 && vlength >= MIN_VECTORS4MEDIAN ) {
        dx_m = gpiv_vector(vlength);
        dy_m = gpiv_vector(vlength);
        dx_org = gpiv_vector(vlength);
        dy_org = gpiv_vector(vlength);
        if (norm) {
            r_m = gpiv_vector(vlength);
        }
/*
 * Write the absolute neighbouring velocities and its components to a 
 * 1-dimensional array 
 */
        m = 0;
        for (k = i - N; k <= i + N; k++) {
            if ((k >= 0) && (k < data->ny)) {
                for (l = j - N; l <= j + N; l++) {
                    if ((l >= 0) && (l < data->nx)) {
                        if ((incl_point
                             && data->peak_no[k][l] != -1)
                            || (!incl_point 
                                && (k != i || l != j)
                                && data->peak_no[k][l] != -1) ) {
                            dx_m[m] = data->dx[k][l];
                            dx_org[m] = data->dx[k][l];
                            dy_m[m] = data->dy[k][l];
                            dy_org[m] = data->dy[k][l];
                            m++;
                        }
                    }
                }
            }
        }
        
/*
 * Sorting dx_m and dy_m arrays and searching median index and value
 */
#pragma omp sections
        {
#pragma omp section
            {
            qsort(dx_m, vlength, sizeof(float), compare_float);
            }
#pragma omp section
            {
            qsort(dy_m, vlength, sizeof(float), compare_float);
            }
        }

        if (incl_point) {
            median_index = (int) (vlength - 1) / 2;
        } else {
            median_index = (int) (vlength) / 2;
        }        

        if (norm) {
/*
 * Obtain all residus from surrounding data, sorting and picking the median
 */
            for (k = 0; k < vlength; k++) {
                r_m[k] = sqrt((dx_m[k] - dx_m[median_index]) *
                            (dx_m[k] - dx_m[median_index]) +
                            (dy_m[k] - dy_m[median_index]) *
                            (dy_m[k] - dy_m[median_index]));
            }
            qsort(r_m, vlength, sizeof(float), compare_float);
            residu = r_m[median_index];
        } else {
/*
 * Obtain residu from difference between current displacement at (i,j) 
 * and median displacement from the surroundings.
 */
            residu = sqrt((data->dx[i][j] - dx_m[median_index]) *
                          (data->dx[i][j] - dx_m[median_index]) +
                          (data->dy[i][j] - dy_m[median_index]) *
                          (data->dy[i][j] - dy_m[median_index]));
        }

/*
 * Search the indexes for the 2-dim arrays dx and dy belonging 
 * to mediaan_index
 */
        m = 0;
        for (k = i - N; k <= i + N; k++) {
            if ((k >= 0) && (k < data->ny)) {
                for (l = j - N; l <= j + N; l++) {
                    if ((l >= 0) && (l < data->nx)) {
                        if ((incl_point
                             && data->peak_no[k][l] != -1)
                            || (!incl_point 
                                && (k != i || l != j)
                                && data->peak_no[k][l] != -1) ) {
                            if (dx_org[m] == dx_m[median_index]) {
                                *i_median_x = k - i;
                                *j_median_x = l - j;
                            }
                            if (dy_org[m] == dy_m[median_index]) {
                                *i_median_y = k - i;
                                *j_median_y = l - j;
                            }
                            m++;
                        }
                    }
                }
            }
        }

        gpiv_free_vector (dx_m);
        gpiv_free_vector (dx_org);
        gpiv_free_vector (dy_m);
        gpiv_free_vector (dy_org);
        if (norm) {
            gpiv_free_vector (r_m);
        }
        
    } else {
        residu = SNR_ERR;
    }
    
    return residu;
}



gchar *
gpiv_valid_residu (GpivPivData *piv_data, 
                   const GpivValidPar *valid_par,
                   const gboolean incl_point
                   )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Calculates residu values (at the inner points) of a PIV data set and 
 *     applies to snr member of out_data
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     piv_data:        piv dataset
 *     valid_par:  validation parameters
 *     incl_point:     flag to include data point under investigation
 *
 * OUTPUTS:
 *     out_data:       piv dataset containing residu values in snr
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    gchar *err_msg = NULL;
    gint i, j;


/*     out_data = gpiv_cp_pivdata (piv_data); */

    if (valid_par->residu_type == GPIV_VALID_RESIDUTYPE__SNR) {
/* Nothing to do here */

    } else if (valid_par->residu_type == GPIV_VALID_RESIDUTYPE__MEDIAN) {

        for (i = 1; i < piv_data->ny - 1; i++) {
            for (j = 1; j < piv_data->nx - 1; j++) {
                guint i_mx, j_mx, i_my, j_my;

                piv_data->snr[i][j] = 
                    median_residu (i, j, valid_par->neighbors, 
                                   incl_point, FALSE, piv_data, 
                                   &i_mx, &j_mx, &i_my, &j_my);
                if (piv_data->snr[i][j] == SNR_ERR) 
                    piv_data->peak_no[i][j] = -1;
            }
        }

    } else if (valid_par->residu_type == GPIV_VALID_RESIDUTYPE__NORMMEDIAN) {
        gfloat residu_from_median_dxdy = 1.111, residu_norm = 5.555;

        for (i = 0; i < piv_data->ny; i++) {
            for (j = 0; j < piv_data->nx; j++) {
                guint i_mx, j_mx, i_my, j_my;

                 residu_norm = 
                     median_residu (i, j, valid_par->neighbors, 
                                    FALSE, TRUE, piv_data, 
                                    &i_mx, &j_mx, &i_my, &j_my);
                 residu_from_median_dxdy = 
                     median_residu (i, j, valid_par->neighbors, 
                                    FALSE, FALSE, piv_data, 
                                    &i_mx, &j_mx, &i_my, &j_my);
                 if (residu_norm + RESIDU_EPSI != 0.0) {
                     piv_data->snr[i][j] = 
                         residu_from_median_dxdy / 
                         (residu_norm + RESIDU_EPSI);
                 } else {
                     piv_data->snr[i][j] = SNR_ERR;
                     piv_data->peak_no[i][j] = -1;
                 }

            }
        }

    } else {
        gpiv_warning("gpiv valid residu: should no arrive here");
    }


    return err_msg;
}



static void
interr_reg_nhpeak(const guint index_y, 
                  const guint index_x,
                  const GpivImage *image, 
                  const GpivPivPar *piv_par,
                  GpivPivData *piv_data
                  )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Interrogates the image(pair) at a single region at the next higher
 *     correlation peak from a previous analysis
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     index_y:        vertical array index (row)
 *     index_x:        horizontal array index (column)
 *     image_par:      image parameters
 *     image:          image
 *     piv_par:        PIV evaluation parameters
 *
 * OUTPUTS:
 *     piv_data:       piv dataset containing particle image displacements
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    char c_line[GPIV_MAX_LINES_C][GPIV_MAX_CHARS];
    int nc_lines = 0;

    float **intreg1;
    float **intreg2;
    int int_size_0;

    GpivPivPar *lo_piv_par = NULL;
    GpivCov *cov = NULL;
    int sweep = 1, sweep_last = 1;
    int return_val;
    int cmpr = 1, cmpr_fact = 1;

/*
 * Checking for memory allocation of input variables
 */
    g_assert (image->frame1[0] != NULL);
    g_assert (image->frame2[0] != NULL);

/*
 * Local (actualized) parameters
 */
    lo_piv_par = gpiv_piv_cp_parameters (piv_par);
    lo_piv_par->peak = piv_data->peak_no[index_y][index_x] + 1;

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

/*
 * Memory allocation of interrogation area's and packed interrogation area 
 * arrays. Define weight kernel values
 */
    int_size_0 = 2 * lo_piv_par->int_size_i;
    intreg1 = gpiv_matrix(int_size_0, int_size_0);
    intreg2 = gpiv_matrix(int_size_0, int_size_0);
    memset(intreg1[0], 0, (sizeof(float)) * int_size_0 * int_size_0);
    memset(intreg2[0], 0, (sizeof(float)) * int_size_0 * int_size_0);
    cov = gpiv_alloc_cov (int_size_0, image->header->x_corr);


/*
 * Interrogate at a single point
 */
    gpiv_piv_interrogate_ia (index_y, index_x, image, lo_piv_par, sweep, 
                             sweep_last, intreg1, intreg2, cov, piv_data);

/*
 * Freeing memory
 */
    gpiv_free_matrix (intreg1);
    gpiv_free_matrix (intreg2);
    gpiv_free_cov (cov);

/*
 * Writes existing fftw wisdom
 */
/*     gpiv_fwrite_fftw_wisdom(1); */
/*     gpiv_fwrite_fftw_wisdom(-1); */

}


static gboolean
subst_vector (GpivPivData *piv_data, 
              const GpivImage *image, 
              const GpivPivPar *piv_par, 
              const GpivValidPar *valid_par
              )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Substitutes data with residu values higher than threshold
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     piv_data:        piv dataset containing particle image displacements
 *     image:          image struct containing parameters and image frames
 *     piv_par:        PIV evaluation parameters
 *     valid_par:      validation parameters
 *
 * OUTPUTS:
 *     l_data:       piv dataset containing particle image displacements
 *
 * RETURNS: flag set TRUE if any data of the input set has been substituted
 *---------------------------------------------------------------------------*/
{
    gboolean outlier_found = FALSE;
    gint i, j;
    guint k, l;
    const gint N = valid_par->neighbors - 2;
    char line[GPIV_MAX_CHARS], command[GPIV_MAX_CHARS];
    guint peak_no;
    gfloat row = 0, col = 0;
    gfloat dx, dy, snr;
    FILE *fp;

    guint count = 0;
    gfloat dx_sum = 0, dy_sum = 0;


/* 
 * Create a local PIV data set, so input data will not be modified halfway
 * during substitution. Else, this might affect PIV displacements
 * calculated from (eventually previously substituted) surrounding values.
 */
    GpivPivData *l_data = NULL; 


    l_data = gpiv_cp_pivdata (piv_data);

    if (valid_par->subst_type == 
        GPIV_VALID_SUBSTYPE__NONE) {
/*
 * no substitution, only sets peak_no to 0
 */

#pragma omp parallel for shared(piv_data, l_data, outlier_found) private(i, j)
        for (i = 0; i < piv_data->ny; i++) {
            for (j = 0; j < piv_data->nx; j++) {

                if (piv_data->peak_no[i][j] != -1 
                    && piv_data->snr[i][j] > valid_par->residu_max) {
                    outlier_found = TRUE;

                    l_data->dx[i][j] = piv_data->dx[i][j];
                    l_data->dy[i][j] = piv_data->dy[i][j];
                    l_data->snr[i][j] = piv_data->snr[i][j];
                    if (l_data->snr[i][j] == SNR_ERR) {
                        l_data->peak_no[i][j] = -1;
                    } else {
                        l_data->peak_no[i][j] = 0;
                    }
                }
            }
        }


    } else if (valid_par->subst_type == 
               GPIV_VALID_SUBSTYPE__L_MEAN) {

#pragma omp parallel for shared(piv_data) private(i, j, k, l, count, \
                         dx_sum, dy_sum)
        for (i = 0; i < piv_data->ny; i++) {
            for (j = 0; j < piv_data->nx; j++) {
                count = 0;
                dx_sum = 0.0;
                dy_sum = 0.0;

                if (piv_data->peak_no[i][j] != -1 
                    && piv_data->snr[i][j] > valid_par->residu_max) {
                    outlier_found = TRUE;

                    for (k = i - N; k <= i + N; k++) {
                        if ((k >= 0) && (k < piv_data->ny)) {
                            for (l = j - N; l <= j + N; l++) {
                                if ((l >= 0) && (l < piv_data->nx)) {
/*
 * Exclude the point under investigation for calculating the mean
 */
                                    if ((k != 0) || (l != 0)) {
                                        dx_sum += piv_data->dx[k][l];
                                        dy_sum += piv_data->dy[k][l];
                                        count++;
                                    }
                                }
                            }
                        }
                    }
                    l_data->dx[i][j] = dx_sum / count;
                    l_data->dy[i][j] = dy_sum / count;
                    l_data->snr[i][j] = piv_data->snr[i][j];
                    l_data->peak_no[i][j] = 0;
                }
            }
        }


    } else if (valid_par->subst_type == 
               GPIV_VALID_SUBSTYPE__MEDIAN) {
/*
 * Substitution with  median particle displacements
 */
        for (i = 0; i < piv_data->ny; i++) {
            for (j = 0; j < piv_data->nx; j++) {
   		guint i_mx, j_mx, i_my, j_my;
                
                if (piv_data->peak_no[i][j] != -1 
                    && piv_data->snr[i][j] > valid_par->residu_max) {
                    outlier_found = TRUE;
      
                    l_data->snr[i][j] = 
                        median_residu (i, j, valid_par->neighbors, 
                                       FALSE, FALSE, piv_data, 
                                       &i_mx, &j_mx, &i_my, &j_my);
                    l_data->dx[i][j] = piv_data->dx[i + i_mx][j + j_mx];
                    l_data->dy[i][j] = piv_data->dy[i + i_my][j + j_my];
                    if (l_data->snr[i][j] == SNR_ERR) {
                        l_data->peak_no[i][j] = -1;
                    } else {
                        l_data->peak_no[i][j] = 0;
                    }

                }
            }
        }
            

    } else if (valid_par->subst_type == 
               GPIV_VALID_SUBSTYPE__COR_PEAK) {
/*
 * substitution with image analyse of next higher correlation
 * peak (or higher)
 */

        for (i = 0; i < piv_data->ny; i++) {
            for (j = 0; j < piv_data->nx; j++) {
                if (piv_data->peak_no[i][j] != -1 
                    && piv_data->snr[i][j] > valid_par->residu_max) {
                    outlier_found = TRUE;

                    interr_reg_nhpeak (i, j, image, piv_par, l_data);
                }
            }
        }

    }


/*
 * Copy back to original values and de-allocate local PIV data set
 */
    gpiv_ovwrt_pivdata (l_data, piv_data);
    gpiv_free_pivdata (l_data);
    return outlier_found;  
}


static void 
cumhisto_eqdatbin (const GpivPivData *data,
                   GpivBinData *klass
                   )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Calculating cumulative histogram from GpivPivData (NOT from GpivScalarData!) 
 *     with an equal number of date per bin of klass
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     data:           piv dataset
 *
 * OUTPUTS:
 *     klass:          histogram dataset
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    gint i, j, k, nresidus = 0;
    gint nx = data->nx, ny = data->ny, **peak_no = data->peak_no;
    gfloat **snr = data->snr;
    gfloat delta, *residu;
    gfloat *bound = klass->bound, *centre = klass->centre;
    gdouble fract, yval;
    gint *count = klass->count, nbins = klass->nbins;
    gint total_ndata = 0, nresidus_bin = 0;

    g_assert (data->point_x != NULL);
    g_assert (data->point_y != NULL);
    g_assert (data->dx != NULL);
    g_assert (data->dy != NULL);
    g_assert (data->snr != NULL);
    g_assert (data->peak_no != NULL);
    
    g_assert (klass->count != NULL);
    g_assert (klass->bound != NULL);
    g_assert (klass->centre != NULL);
  

   for (i = 0, k = 0; i < ny; i++) {
        for (j = 0; j < nx; j++) {
            if (peak_no[i][j] != -1 && snr[i][j] != 0) k++;
       }
    }
    nresidus = k;
    nresidus_bin = nresidus / nbins;

    residu = gpiv_vector(nresidus);
    for (i = 0, k = 0; i < ny; i++) {
        for (j = 0; j < nx; j++) {
            if (peak_no[i][j] != -1 && snr[i][j] != 0) {
                  residu[k] = snr[i][j];
                  k++;
            }
        }
    }


/*
 * sorting snr data
 */
    qsort(residu, nresidus, sizeof(float), compare_float);

/*
 * find lower boundaries of bins
 */

    for (i = 0; i < nbins; i++) {
        for (j = 0; j < nresidus_bin; j++) {
             if (j == 0) {
                klass->bound[i] = log (1.0/(1.0 - (double) i / (double) nbins));
/*                 klass->bound[i] = (double) i / (double) nbins; */
                klass->centre[i] = residu[(i+1) * nresidus_bin];
            }
        }
    }

    klass->min = bound[0];
    klass->max = bound[nbins - 1];
    gpiv_free_vector(residu);
}



GpivBinData *
gpiv_valid_peaklck (const GpivPivData *piv_data,
                    const guint nbins
                    )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Calculating histogram of sub-pixel displacements to check on peaklocking
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *
 * OUTPUTS:
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    GpivBinData *klass = NULL;

    gchar *err_msg = NULL;
    gint i, j, k;
    gfloat delta, fract;
    gfloat *bound, *centre;
    gint *count;


    if ((err_msg = gpiv_check_alloc_pivdata (piv_data)) != NULL) {
        gpiv_warning ("%s", err_msg);
        return NULL;
    }

    if ((klass = gpiv_alloc_bindata (nbins)) == NULL) {
        gpiv_warning ("gpiv_valid_peaklck: failing gpiv_alloc_bindata");
        return NULL;
    }

    count = klass->count;
    bound = klass->bound; 
    centre = klass->centre;
    delta = 1. / nbins;

    for (i = 0; i < nbins; i++) {
	centre[i] = (float) i *delta;
	bound[i] = -delta / 2.0 + (float) i *delta;
	count[i] = 0;
    }

/*
 * Subdividing fractional particle displacements in bins
 */
#pragma omp parallel for shared(piv_data, bound, count) \
                         private(i, j, k, fract)
    for (i = 0; i < piv_data->ny; i++) {
	for (j = 0; j < piv_data->nx; j++) {
	    fract = fabs(piv_data->dx[i][j] - (int) piv_data->dx[i][j]);
	    for (k = 0; k < nbins; k++) {
		if ((fract >= bound[k]) && (fract < bound[k + 1])) {
		    count[k] = count[k] + 1;
		}
	    }
	    fract = fabs(piv_data->dy[i][j] - (int) piv_data->dy[i][j]);
	    for (k = 0; k < nbins; k++) {
		if ((fract >= bound[k]) && (fract < bound[k + 1])) {
		    count[k] = count[k] + 1;
		}
	    }
	}
    }


    return klass;
}



GpivBinData *
gpiv_valid_residu_stats (const GpivPivData *piv_data, 
                         const guint nbins,
                         GpivLinRegData *linreg
                         )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Calculates cumulative histogram of residus
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     linreg:         linear regression data structure
 *     klass:          histogram data
 *
 * OUTPUTS:
 *     piv_data:       piv dataset containing residu values in snr
 *
 * RETURNS:
 *     GpivBinData containing histogram
 *---------------------------------------------------------------------------*/
{
    GpivBinData *klass = NULL;
    gchar *err_msg = NULL;
    int i, return_val;
    double *x, *y;
            

    if ((klass = gpiv_alloc_bindata (nbins)) == NULL) {
        gpiv_warning ("gpiv_valid_residu_stats: failing gpiv_alloc_bindata(%d)", nbins);
    }

    cumhisto_eqdatbin (piv_data, klass);
    x = gpiv_dvector (klass->nbins);
    y = gpiv_dvector (klass->nbins);

    for (i = 0; i < klass->nbins; i++) {
        x[i] = (double) klass->bound[i];
        y[i] = (double) klass->centre[i];
    }

    if (return_val = 
        gsl_fit_linear (x, 1, y, 1, klass->nbins, &linreg->c0, 
                        &linreg->c1, &linreg->cov00, &linreg->cov01, 
                        &linreg->cov11, &linreg->sumsq) == 1) {
        err_msg = "gpiv_valid_residu_stats: error from gsl_fit_linear";
        g_warning("%s", err_msg);
        return NULL;
    }
 

    gpiv_free_dvector(x);
    gpiv_free_dvector(y);
    return klass;
}   



gchar *
gpiv_valid_errvec (GpivPivData *piv_data, 
                   const GpivImage *image,
                   const GpivPivPar *piv_par,
                   const GpivValidPar *valid_par,
                   const gboolean interrogate_valid
                   )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Searches the erroneous vectors in a PIV data set and
 *     substitutes with new values, if possible
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     image:          image struct
 *     piv_par:        PIV evaluation parameters
 *     valid_par:      validation parameters
 *     piv_data:        PIV dataset containing particle image displacements
 *     interrogate_valid: validation during (iterative) interrogation process
 *
 * RETURNS:
 *     GpivPivData on success or NULL on failure
 *---------------------------------------------------------------------------*/
{
    GpivPivData *l_data = NULL;
    gchar *err_msg = NULL;

    gint i, j;
    gboolean outlier_found = TRUE;
    gint count = 0;
    enum SubstitutionType l_subst_type = valid_par->subst_type;
    gint l_peak = piv_par->peak;
    GpivPivPar *l_piv_par = NULL;
    GpivValidPar *l_valid_par = NULL;


/*
 * Checking input pivdata
 */
#ifdef DEBUG
    g_message ("gpiv_valid_errvec:: piv_data nx = %d ny = %d",
               piv_data->nx, piv_data->ny);
#endif

    if (piv_data->point_x == NULL
        || piv_data->point_y == NULL
        || piv_data->dx == NULL
        || piv_data->dy == NULL
        || piv_data->snr == NULL
        || piv_data->snr == NULL
        || piv_data->peak_no == NULL) {
        err_msg = "gpiv_valid_errvec: piv_data->* == NULL";
        gpiv_warning ("%s", err_msg);
        return err_msg;
    }


    l_piv_par = gpiv_piv_cp_parameters (piv_par);
    l_valid_par = gpiv_valid_cp_parameters (valid_par);
    

    while (outlier_found && count < GPIV_VALID_MAX_SWEEP) {
            
/*
 * Calculates and substitutes snr data with residu values higher than threshold
 */
        l_data = gpiv_cp_pivdata (piv_data);
        gpiv_valid_residu (l_data, l_valid_par, TRUE);
        if (l_valid_par->subst_type != GPIV_VALID_SUBSTYPE__NONE
            && interrogate_valid) {
/*
 * Test data with different types of substitutions if used during
 * (iterative) image interrogation
 */
            if (count <= 1) {
                l_valid_par->subst_type = GPIV_VALID_SUBSTYPE__COR_PEAK;
                l_piv_par->peak = count + 2;
            } else  {
                l_valid_par->subst_type = l_subst_type;
                l_piv_par->peak = l_peak;
            }
        }

        outlier_found = subst_vector (l_data, image, l_piv_par, l_valid_par);
        
        if (l_valid_par->subst_type == GPIV_VALID_SUBSTYPE__NONE) {
            outlier_found = FALSE;
        }
            
/*
 * piv_data are updated with corrected values 
 * l_data will be made free for an eventually next loop
 * BUGFIX: possible memleak? Comment: when outside if {} causes crashing.
 */
        if (outlier_found) {
            count++;
            gpiv_ovwrt_pivdata (l_data, piv_data);
        }            
            gpiv_free_pivdata (l_data);
    }


    return err_msg;
}



void
gpiv_valid_gradient (const GpivPivPar *piv_par,
                     GpivPivData *piv_data
                     )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Searches vectors in a PIV data set that exceed the maximum gradient 
 *     (dU x dt/int_size > GPIV_GRADIENT_THRESHOLD)
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *     piv_par:    PIV evaluation parameters
 *     valid_par:  validation parameters
 *
 * OUTPUTS:
 *     piv_data:       piv dataset containing peak_no = -1 for exceeded maxima
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    int i, j, diff_order = 1;
    double grad_dx, delta_dx, grad_dy, delta_dy;

    g_return_if_fail (piv_data->point_x != NULL);
    g_return_if_fail (piv_data->point_y != NULL);
    g_return_if_fail (piv_data->dx != NULL);
    g_return_if_fail (piv_data->dy != NULL);
    g_return_if_fail (piv_data->snr != NULL);
    g_return_if_fail (piv_data->peak_no != NULL);

/* BUGFIX: test op patch. Gerber */
    for (i = diff_order; i < piv_data->ny - diff_order; i++) {
	for (j = diff_order; j < piv_data->nx - diff_order; j++) {

            if(piv_data->peak_no[i][j] != -1 
/*                && piv_data->peak_no[i-1][j] != -1 && */
/*                piv_data->peak_no[i][j-1] != -1 && */
/*                piv_data->peak_no[i][j+1] != -1 */
               ) {
                grad_dx = (piv_data->dx[i+1][j] - piv_data->dx[i-1][j]) / 
                    (2 * piv_par->int_shift);
                delta_dx = fabs(grad_dx) * piv_par->int_size_f;
                
                piv_data->snr[i][j] = delta_dx;
                grad_dy = (piv_data->dy[i][j+1] - piv_data->dy[i][j-1]) / 
                    (2 * piv_par->int_shift); 
                delta_dy = fabs(grad_dy) * piv_par->int_size_f;
                if (delta_dx > GPIV_GRADIENT_THRESHOLD || 
                    delta_dy > GPIV_GRADIENT_THRESHOLD) 
                    piv_data->peak_no[i][j] = -1;
            }
        }
    }



/*
 * exclude all data near the boundaries of the dataset
 */
     for (i=0; i < diff_order; i++) {
	  for (j=0; j < piv_data->nx; j++) {
              piv_data->peak_no[i][j] = 0;
	  }
     }

     for (i=0; i < piv_data->ny; i++) {
	  for (j=0; j < diff_order; j++) {
              piv_data->peak_no[i][j] = 0;
	  }
     }

     for (i=piv_data->ny - diff_order; i < piv_data->ny; i++) {
	  for (j=0; j < piv_data->nx; j++) {
              piv_data->peak_no[i][j] = 0;
	  }
     }

     for (i=0; i < piv_data->ny; i++) {
	  for (j=piv_data->nx - diff_order; j < piv_data->nx; j++) {
              piv_data->peak_no[i][j] = 0;
	  }
     }


}





void
gpiv_cumhisto_eqdatbin_gnuplot (const gchar *fname_out, 
                                const gchar *title, 
                                const gchar *GNUPLOT_DISPLAY_COLOR,
                                const gint GNUPLOT_DISPLAY_SIZE,
                                const GpivLinRegData *linreg
                               )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Plots data on screen with gnuplot
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *
 * OUTPUTS:
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/

{
  FILE *fp_cmd;
  gchar *fname_cmd="/tmp/gpiv_gnuplot.cmd";
  gchar *function_name="gpiv_histo_gnuplot";
  gchar command[GPIV_MAX_CHARS];

  if((fp_cmd=fopen(fname_cmd,"w"))==NULL) { 
    fprintf (stderr,"\n%s:%s error: Failure opening %s for output\n",
	     LIBNAME, function_name, fname_cmd); 
    exit(1);
  }

  fprintf (fp_cmd,"\nset xlabel \"-ln(1-i/nbins)\"");
  fprintf (fp_cmd,"\nset ylabel \"residu (pixels)\"");
  fprintf (fp_cmd,"\nplot \"%s\" title \"%s\" with boxes, %f + %f * x", /* with boxes */
	   fname_out, title, linreg->c0, linreg->c1);
  fprintf (fp_cmd,"\npause -1 \"Hit return to exit\"");
  fprintf (fp_cmd,"\nquit");

  fclose (fp_cmd);
  
  
  snprintf(command, GPIV_MAX_CHARS, "gnuplot -bg %s -geometry %dx%d %s",
	   GNUPLOT_DISPLAY_COLOR, GNUPLOT_DISPLAY_SIZE, 
	   GNUPLOT_DISPLAY_SIZE, fname_cmd);

  if (system (command) != 0) {
    fprintf (stderr,"\n%s:%s could not exec shell command\n", 
	     LIBNAME, function_name);
    exit(1);
  }


}


gfloat 
gpiv_valid_threshold (const GpivPivPar *piv_par,
                      const GpivValidPar *valid_par,
                      const GpivLinRegData *linreg
                      )
/*-----------------------------------------------------------------------------
 * DESCRIPTION:
 *     Calculates threshold value (residu_max) from residus. 
 *     Will need the int_size_f from the GpivPivPar struct!
 *
 * PROTOTYPE LOCATATION:
 *     valid.h
 *
 * INPUTS:
 *
 * OUTPUTS:
 *
 * RETURNS:
 *---------------------------------------------------------------------------*/
{
    gfloat search_radius = (float) piv_par->int_size_f / 4.0;
    gfloat residu_max = - linreg->c1 * log((1.0 - valid_par->data_yield) / 
                                        valid_par->data_yield * 
                                         linreg->c1 / search_radius);
    return residu_max;
}

#undef SNR_ERR
#undef MIN_VECTORS4MEDIAN
#undef RESIDU_EPSI