/* $Id: cpl_geom_img.c,v 1.7 2007/07/20 09:24:29 llundin Exp $
 *
 * This file is part of the ESO Common Pipeline Library
 * Copyright (C) 2001-2004 European Southern Observatory
 *
 * This program 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 of the License, 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
 */

/*
 * $Author: llundin $
 * $Date: 2007/07/20 09:24:29 $
 * $Revision: 1.7 $
 * $Name:  $
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

/*-----------------------------------------------------------------------------
                                Includes
 -----------------------------------------------------------------------------*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <assert.h>

#include <cpl_memory.h>
#include <cpl_image.h>
#include <cpl_imagelist.h>
#include <cpl_stats.h>

#include <cpl_tools.h>
#include <cpl_vector.h>
#include <cpl_error.h>
#include <cpl_errorstate.h>

#include "cpl_apertures.h"
#include "cpl_geom_img.h"

/*-----------------------------------------------------------------------------
                                   Define
 -----------------------------------------------------------------------------*/

#define CPL_GEOM_IMA_OFFSET_SAA             0 
#define CPL_GEOM_IMA_OFFSET_XCORR           1

#define CPL_CLASS_DOUBLE                    1
#define CPL_CLASS_FLOAT                     2

/*-----------------------------------------------------------------------------
                            Functions prototypes
 -----------------------------------------------------------------------------*/

static double cpl_geom_ima_offset_xcorr(const cpl_image *, const cpl_image *,
        const cpl_bivector *, int, int, int, int, double *, double *) ;
static double cpl_geom_ima_offset_xcorr_subw(const cpl_image *, 
        const cpl_image *, int, int, int, int, int, int, int, int, double *, 
        double *) ;

/*----------------------------------------------------------------------------*/
/**
 * @defgroup cpl_geom_img   High level functions for geometric transformations
 *
 * This module contains functions to compute the shift-and-add operation
 * on an image list.
 *
 * @par Synopsis:
 * @code
 *   #include "cpl_geom_img.h"
 * @endcode
 */
/*----------------------------------------------------------------------------*/
/**@{*/

/*-----------------------------------------------------------------------------
                            Function codes
 -----------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Get the offsets by correlating the images
  @param    ilist           Input image list
  @param    estimates       First-guess estimation of the offsets
  @param    anchors         List of anchor points
  @param    s_hx            Half-width of search area
  @param    s_hy            Half-height of search area
  @param    m_hx            Half-width of measurement area
  @param    m_hy            Half-height of measurement area
  @param    correl          List of cross-correlation quality factors
  @return   List of offsets or NULL in error case

  The matching is performed using a 2d cross-correlation, using a minimal 
  squared differences criterion. One measurement is performed per input anchor 
  point, and the median offset is returned together with a measure of 
  similarity for each plane.
 
  The images in the input list must only differ from a shift. In order
  from the correlation to work, they must have the same level (check the
  average values of your input images if the correlation does not work).

  The supported types are CPL_TYPE_DOUBLE and CPL_TYPE_FLOAT.
  The bad pixel maps are ignored by this function.
  
  The ith offset (offsx, offsy) in the returned offsets is the one that have 
  to be used to shift the ith image to align it on the reference image (the 
  first one).
  
  If not NULL, the returned cpl_bivector must be deallocated with
  cpl_bivector_delete().
  
  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT if (one of) the input pointer(s) is NULL
  - CPL_ERROR_ILLEGAL_INPUT if ilist is not valid
 */
/*----------------------------------------------------------------------------*/
cpl_bivector * cpl_geom_img_offset_fine(
        const cpl_imagelist *   ilist,
        const cpl_bivector  *   estimates,
        const cpl_bivector  *   anchors,
        int                     s_hx,
        int                     s_hy,
        int                     m_hx,
        int                     m_hy,
        cpl_vector          *   correl)
{
    cpl_image           *   im1 ;
    cpl_image           *   im2 ;
    cpl_matrix          *   kernel ;
    double              *   correl_data ;
    const double        *   estimates_x_data ;
    const double        *   estimates_y_data ;
    cpl_bivector        *   offsets ;
    double              *   offsets_x_data ;
    double              *   offsets_y_data ;
    double                  offsx, offsy ;
    int                     i ;
    
    /* Check entries */
    cpl_ensure(ilist,      CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(estimates, CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(anchors,   CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(correl,   CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(cpl_imagelist_is_uniform(ilist)==0, CPL_ERROR_ILLEGAL_INPUT,
            NULL) ;

    /* Create the kernel for the filtering operations */
    kernel = cpl_matrix_new(3, 3) ;
    cpl_matrix_fill(kernel, 1.0) ;
    
    /* Filter the first image */
    im1 = cpl_image_filter_median(cpl_imagelist_get_const(ilist, 0), kernel);
    
    /* Create the offsets container */
    offsets = cpl_bivector_new(cpl_imagelist_get_size(ilist)) ;
    offsets_x_data = cpl_bivector_get_x_data(offsets) ;
    offsets_y_data = cpl_bivector_get_y_data(offsets) ;
    correl_data = cpl_vector_get_data(correl) ;
    estimates_x_data = cpl_bivector_get_x_data_const(estimates) ;
    estimates_y_data = cpl_bivector_get_y_data_const(estimates) ;

    offsets_x_data[0] = 0.00 ;
    offsets_y_data[0] = 0.00 ;
    correl_data[0] = 0.00 ;
    for (i=1 ; i<cpl_imagelist_get_size(ilist) ; i++) {
        cpl_errorstate prevstate;
        cpl_msg_progress(cpl_func, i, cpl_imagelist_get_size(ilist), 
                "Cross-correlate images") ;
        /* Filter the current image */
        im2 = cpl_image_filter_median(cpl_imagelist_get_const(ilist, i), kernel);

        /* Set the estimates */
        offsx = estimates_x_data[i] ;
        offsy = estimates_y_data[i] ;
        
        /* Perform cross-correlation */
        prevstate = cpl_errorstate_get();
        correl_data[i] = cpl_geom_ima_offset_xcorr(im1, im2, anchors, 
                s_hx, s_hy, m_hx, m_hy, &offsx, &offsy) ;
        cpl_errorstate_set(prevstate);
        cpl_image_delete(im2) ;

        if (correl_data[i] < -0.5) {
            /* Nothing was found. */
            /* declare the offset as invalid: null offsets and dist=-1 */
            offsets_x_data[i] = 0.00 ;
            offsets_y_data[i] = 0.00 ;
        } else {
            /* Something was found. */
            /* One standard failure case is when the returned offset is */
            /* located on the border of the search zone. Identify such */
            /* cases and flag the frame as not registrable by setting */
            /* the offset vector to nil and the distance to -1. */
            if ((fabs(fabs(estimates_x_data[i]-offsx)-(double)s_hx)<1) ||
                (fabs(fabs(estimates_y_data[i]-offsy)-(double)s_hy)<1)) {
                offsets_x_data[i] = 0.00 ;
                offsets_y_data[i] = 0.00 ;
                correl_data[i] = -1.00 ;
            } else {
                /* The returned offset is correct */
                offsets_x_data[i] = offsx ;
                offsets_y_data[i] = offsy ;
            }
        }
        if (correl_data[i] < -0.5) {
            cpl_msg_debug(cpl_func, "Frame %d does not correlate", i+1) ;
        }
    }
    
    /* Free and return */
    cpl_matrix_delete(kernel) ;
    cpl_image_delete(im1) ;
    return offsets ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Images list recombination
  @param    ilist       Input imagelist - may be empty on return
  @param    offs        List of offsets estimates in x and y or NULL if unknown
  @param    refine      Flag to refine the estimated offsets or not
  @param    aperts      List of correlation apertures or NULL if unknown
  @param    sigmas      Positive, decreasing sigmas to apply
  @param    pisigma     Index of the sigma that was used or undefined on error
  @param    s_hx        Search area half-width.
  @param    s_hy        Search area half-height.
  @param    m_hx        Measurement area half-width.
  @param    m_hy        Measurement area half-height.
  @param    min_rej     number of low values to reject in stacking
  @param    max_rej     number of high values to reject in stacking
  @param    union_flag  Combination mode (CPL_GEOM_UNION or CPL_GEOM_INTERSECT)
  @return   Pointer to newly allocated images array, or NULL on error.
  @see cpl_apertures_extract()
  @see cpl_geom_img_offset_fine()

  This function detects cross correlation points in the first image (if not 
  provided by the user), use them to refine the provided offsets with a cross 
  correlation method, and then apply the shift and add to recombine the images 
  together.
  If offs is NULL, the function tries to find offsets estimates itself by 
  subsampling the images and correlate them together with the first one.
 
  The input image list is purged from bad correlating frames.
  
  The supported types are CPL_TYPE_DOUBLE, CPL_TYPE_FLOAT.
 
  The number of provided offsets shall be equal to the number of inputs images.
  The ith offset (offs_x, offs_y) is the offset that has to be used to shift 
  the ith image to align it on the first one.
  
  The bad pixel map of the input images list is ignored, and the bad pixel map 
  of the output image is empty.

  The returned image array contains 2 images:
  - the combined image
  - the contribution map
  
  If not NULL, the returned cpl_image array arr must be deallocated like:
  @code
  if (arr[0] != NULL) cpl_image_delete(arr[0]) ;
  if (arr[1] != NULL) cpl_image_delete(arr[1]) ;
  cpl_free(arr) ;
  @endcode
  
  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT if ilist or offs is NULL
  - CPL_ERROR_ILLEGAL_INPUT if ilist is not uniform or if ilist and offs
    have different sizes
  - CPL_ERROR_ILLEGAL_OUTPUT if the images cannot be combined
 */
/*----------------------------------------------------------------------------*/
cpl_image ** cpl_geom_img_offset_combine(
        cpl_imagelist       *   ilist,
        const cpl_bivector  *   offs,
        int                     refine,
        const cpl_bivector  *   aperts,
        const cpl_vector    *   sigmas,
        int                 *   pisigma,
        int                     s_hx,
        int                     s_hy,
        int                     m_hx,
        int                     m_hy,
        int                     min_rej,
        int                     max_rej,
        cpl_geom_combine        union_flag)
{
    cpl_bivector    *   offs_est ;
    cpl_apertures   *   apertures ;
    cpl_bivector    *   anchors ;
    double          *   anchors_x ;
    double          *   anchors_y ;
    cpl_bivector    *   offs_ref ;
    double          *   offs_ref_x ;
    double          *   offs_ref_y ;
    cpl_vector      *   correl ;
    double          *   correl_data ;
    cpl_bivector    *   offs_ref_purged ;
    double          *   offs_ref_pur_x ;
    double          *   offs_ref_pur_y ;
    int                 ngood ;
    cpl_image       **  combined ;
    int                 nima ;
    int                 i, j ;

    /* Initialise */
    anchors = NULL ;

    /* Check inputs */
    cpl_ensure(ilist != NULL, CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(cpl_imagelist_is_uniform(ilist)==0, CPL_ERROR_ILLEGAL_INPUT, 
            NULL);
    cpl_ensure(offs != NULL, CPL_ERROR_NULL_INPUT, NULL) ;

    nima = cpl_imagelist_get_size(ilist) ;
    cpl_ensure(cpl_bivector_get_size(offs)==nima, CPL_ERROR_ILLEGAL_INPUT,
            NULL) ;

    /* If the offsets refining is not requested */
    if (!refine) {
        /* Apply the shift & add on nodded */
        if ((combined=cpl_geom_img_offset_saa(ilist, offs, CPL_KERNEL_DEFAULT,
                        min_rej, max_rej, union_flag)) == NULL) {
            cpl_msg_error(cpl_func, "Cannot apply the shift and add") ;
            cpl_ensure(0, CPL_ERROR_ILLEGAL_OUTPUT, NULL) ;
        }
        return combined ;
    }
    
    offs_est = cpl_bivector_duplicate((cpl_bivector*)offs) ;
    cpl_ensure(offs_est, CPL_ERROR_ILLEGAL_OUTPUT, NULL) ; 

    /* If the apertures are not provided, detect a cross correlation point */
    if (aperts == NULL) {
        cpl_msg_info(cpl_func, "Get a cross-correlation point") ;
        cpl_msg_indent_more() ;

        apertures = cpl_apertures_extract(cpl_imagelist_get(ilist, 0), sigmas,
                                          pisigma);

        if (apertures != NULL) {
            cpl_apertures_sort_by_npix(apertures) ;
            anchors = cpl_bivector_new(1) ;
            anchors_x = cpl_bivector_get_x_data(anchors) ;
            anchors_y = cpl_bivector_get_y_data(anchors) ;
            anchors_x[0] = cpl_apertures_get_max_x(apertures, 1) ;
            anchors_y[0] = cpl_apertures_get_max_y(apertures, 1) ;
            cpl_apertures_delete(apertures) ;
            cpl_msg_info(cpl_func, "Correlation point: %g %g\n", anchors_x[0],
                    anchors_y[0]) ;
        }
        cpl_msg_indent_less() ;
    } else {
        anchors = cpl_bivector_duplicate((cpl_bivector*)aperts) ;
    }

    /* Check if anchor points are there */
    if (anchors == NULL) {
        cpl_msg_error(cpl_func, "Cannot get the correlation point(s)") ;
        cpl_bivector_delete(offs_est) ;
        cpl_ensure(0, CPL_ERROR_ILLEGAL_OUTPUT, NULL) ;
    }

    /* Refine the offsets */
    cpl_msg_info(cpl_func, "Refine the offsets") ;
    cpl_msg_indent_more() ;
    correl = cpl_vector_new(nima) ;
    if ((offs_ref = cpl_geom_img_offset_fine(ilist, offs_est, anchors, 
                    s_hx, s_hy, m_hx, m_hy, correl)) == NULL) {
        cpl_msg_warning(cpl_func, "Cannot refine the offsets - use estimates");
        offs_ref = cpl_bivector_duplicate(offs_est) ;
    }
    cpl_bivector_delete(anchors) ;
    cpl_bivector_delete(offs_est) ;
    
    /* Display the results */
    offs_ref_x = cpl_bivector_get_x_data(offs_ref) ;
    offs_ref_y = cpl_bivector_get_y_data(offs_ref) ;
    correl_data = cpl_vector_get_data(correl) ;
    cpl_msg_info(cpl_func, "Refined offsets [correlation factor]") ;
    ngood = 0 ;
    for (i=0 ; i<nima ; i++) {
        cpl_msg_info(cpl_func, "#%02d: %8.2f %8.2f [%12.2f]",
                i+1, offs_ref_x[i], offs_ref_y[i], correl_data[i]) ;
        if (correl_data[i] > -0.5) ngood++ ;
    }
    if (ngood == 0) {
        cpl_bivector_delete(offs_ref) ;
        cpl_vector_delete(correl) ;
        cpl_msg_error(cpl_func, "No frame correctly correlated") ;
        cpl_ensure(0, CPL_ERROR_ILLEGAL_OUTPUT, NULL) ;
    }
    cpl_msg_indent_less() ;

    /* Purge the bad correlated planes */
    cpl_msg_info(cpl_func, "Purge the badly correlated planes") ;
    cpl_imagelist_erase(ilist, correl) ;
    offs_ref_purged = cpl_bivector_new(ngood) ;
    offs_ref_pur_x = cpl_bivector_get_x_data(offs_ref_purged) ;
    offs_ref_pur_y = cpl_bivector_get_y_data(offs_ref_purged) ;
    j = 0 ;
    for (i=0 ; i<nima ; i++) {
        if (correl_data[i] > -0.5) {
            offs_ref_pur_x[j] = offs_ref_x[i] ;
            offs_ref_pur_y[j] = offs_ref_y[i] ;
            j++ ;
        }
    }
    cpl_bivector_delete(offs_ref) ;
    cpl_vector_delete(correl) ;

    /* Apply the shift & add on nodded */
    if ((combined=cpl_geom_img_offset_saa(ilist, offs_ref_purged,
                    CPL_KERNEL_DEFAULT, min_rej, max_rej, union_flag))==NULL) {
        cpl_msg_error(cpl_func, "Cannot apply the shift and add") ;
        cpl_bivector_delete(offs_ref_purged) ;
        cpl_ensure(0, CPL_ERROR_ILLEGAL_OUTPUT, NULL) ;
    }

    /* Free and return */
    cpl_bivector_delete(offs_ref_purged) ;
    return combined ;
}

#define CPL_OPERATION CPL_GEOM_IMA_OFFSET_SAA
/*----------------------------------------------------------------------------*/
/**
  @brief    Shift and add an images list to a single image 
  @param    ilist       Input image list
  @param    offs        List of offsets in x and y
  @param    kernel      Interpolation kernel to use for resampling
  @param    rejmin      Number of minimum value pixels to reject in stacking
  @param    rejmax      Number of maximum value pixels to reject in stacking
  @param    union_flag  Combination mode (CPL_GEOM_UNION or CPL_GEOM_INTERSECT)
  @return   Pointer to newly allocated images array, or NULL on error.

  The supported types are CPL_TYPE_DOUBLE, CPL_TYPE_FLOAT.
 
  The number of provided offsets shall be equal to the number of inputs images.
  The ith offset (offs_x, offs_y) is the offset that has to be used to shift 
  the ith image to align it on the first one.
  
  Provide the name of the kernel you want to generate. Supported kernel
  types are:
  - CPL_KERNEL_DEFAULT: default kernel, currently CPL_KERNEL_TANH
  - CPL_KERNEL_TANH: Hyperbolic tangent
  - CPL_KERNEL_SINC: Sinus cardinal
  - CPL_KERNEL_SINC2: Square sinus cardinal
  - CPL_KERNEL_LANCZOS: Lanczos2 kernel
  - CPL_KERNEL_HAMMING: Hamming kernel
  - CPL_KERNEL_HANN: Hann kernel

  If the number of input images is lower or equal to 3, the rejection 
  parameters are ignored.
  If the number of input images is lower or equal to 2*(rejmin+rejmax), the 
  rejection parameters are ignored.
  
  The bad pixel map of the input images list is ignored, and the bad pixel map 
  of the output image is empty.

  If not NULL, the returned cpl_image array arr must be deallocated like:
  @code
  if (arr[0] != NULL) cpl_image_delete(arr[0]) ;
  if (arr[1] != NULL) cpl_image_delete(arr[1]) ;
  cpl_free(arr) ;
  @endcode
  
  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT if (one of) the input pointer(s) is NULL
  - CPL_ERROR_ILLEGAL_INPUT if ilist is invalid or if rejmin or rejmax is
    negative
  - CPL_ERROR_ILLEGAL_OUTPUT if no valid correlation points are found
  - CPL_ERROR_TYPE_MISMATCH if the passed image list type is not supported
  - CPL_ERROR_UNSUPPORTED_MODE if the union_flag is not one of the supported
    combination modes, which are @em CPL_GEOM_INTERSECT, @em CPL_GEOM_UNION,
    @em CPL_GEOM_FIRST.
 */
/*----------------------------------------------------------------------------*/
cpl_image ** cpl_geom_img_offset_saa(
        const cpl_imagelist *   ilist,
        const cpl_bivector  *   offs,
        cpl_kernel              kernel,
        int                     rejmin,
        int                     rejmax,
        cpl_geom_combine        union_flag)
{
    int                     rmin, rmax, rtot ;
    int                     nima ;
    const double        *   offs_x ;
    const double        *   offs_y ;
    cpl_vector          *   ox_tmp ;
    cpl_vector          *   oy_tmp ;
    double              *   ox_tmp_data ;
    double              *   oy_tmp_data ;
    double                  ox_min, oy_min, ox_max, oy_max ;
    int                     nx, ny, sizex, sizey ;
    cpl_type                type ;
    int                     start_x, start_y ;

    cpl_vector          *   xyprofile = NULL;
    /* FIXME */
    /* double                  confidence = -1; */
    double              *   interp_kernel ;
    int                     leaps[16] ;
    double                  neighbors[16] ;
    cpl_image           *   final ;
    double              *   acc ;
    cpl_image           *   contrib ;
    int                 *   pcontrib ;
    double                  interp ;
    int                     ncontrib ;
    double                  x, y ;
    int                     px, py ;
    const int               tabsperpix = CPL_KERNEL_TABSPERPIX;
    int                     tabx, taby ;
    double                  rsc[8] ;
    double                  sumrs ;
    double                  finpix ;
    int                     pos ;
    cpl_image           **  out_arr ;
    int                     i, j, k, p ;

    /* Check inputs */
    cpl_ensure(ilist && offs, CPL_ERROR_NULL_INPUT, NULL) ;
    cpl_ensure(cpl_imagelist_is_uniform(ilist)==0, CPL_ERROR_ILLEGAL_INPUT,
               NULL);
    nima = cpl_imagelist_get_size(ilist) ;
    sizex = cpl_image_get_size_x(cpl_imagelist_get_const(ilist, 0)) ;
    sizey = cpl_image_get_size_y(cpl_imagelist_get_const(ilist, 0)) ;
    type = cpl_image_get_type(cpl_imagelist_get_const(ilist, 0)) ;
    cpl_ensure(cpl_bivector_get_size(offs)==nima, CPL_ERROR_INCOMPATIBLE_INPUT,
               NULL);
    cpl_ensure(rejmin>=0 && rejmax>=0, CPL_ERROR_ILLEGAL_INPUT, NULL) ;
    
    /* Test rejection parameters */
    if ((nima <= 3) || (nima <= 2*(rejmin+rejmax))) rmin = rmax = 0 ;
    else {
        rmin = rejmin ;
        rmax = rejmax ;
    }
    rtot = rmin + rmax ;
   
    /* Compute output image size for union/intersection */
    offs_x = cpl_bivector_get_x_data_const(offs) ;
    offs_y = cpl_bivector_get_y_data_const(offs) ;

    ox_tmp = cpl_vector_duplicate(cpl_bivector_get_x_const(offs)) ;
    cpl_vector_sort(ox_tmp, 1);
    oy_tmp = cpl_vector_duplicate(cpl_bivector_get_y_const(offs)) ;
    cpl_vector_sort(oy_tmp, 1);
    ox_tmp_data = cpl_vector_get_data(ox_tmp) ;
    oy_tmp_data = cpl_vector_get_data(oy_tmp) ;
    if (union_flag == CPL_GEOM_UNION) {
        /* Union image */
        ox_min = ox_tmp_data[rtot] ;
        ox_max = ox_tmp_data[nima - rtot - 1] ;
        oy_min = oy_tmp_data[rtot] ;
        oy_max = oy_tmp_data[nima - rtot - 1] ;
        nx = (int)(sizex + ox_max - ox_min) ;
        ny = (int)(sizey + oy_max - oy_min) ;
        start_x = ox_min ;
        start_y = oy_min ;
    } else if (union_flag == CPL_GEOM_INTERSECT) {
        /* Intersection image */
        ox_min = ox_tmp_data[0] ;
        ox_max = ox_tmp_data[nima - 1] ;
        oy_min = oy_tmp_data[0] ;
        oy_max = oy_tmp_data[nima - 1] ;
        nx = (int)(sizex - ox_max + ox_min) ;
        ny = (int)(sizey - oy_max + oy_min) ;
        start_x = ox_max ;
        start_y = oy_max ;
    } else if  (union_flag == CPL_GEOM_FIRST) {
        /* First image as reference */
        nx = (int)(sizex) ;
        ny = (int)(sizey) ;
        start_x = 0 ;
        start_y = 0 ;
    } else {
        start_x = start_y = nx = ny = -1;
    }
    cpl_vector_delete(ox_tmp) ;
    cpl_vector_delete(oy_tmp) ;

    /* union_flag is not one of the three supported modes */
    cpl_ensure(nx != -1, CPL_ERROR_UNSUPPORTED_MODE, NULL);

    xyprofile = cpl_vector_new(CPL_KERNEL_DEF_SAMPLES);
    cpl_ensure(xyprofile != NULL, cpl_error_get_code(), NULL);

    if (cpl_vector_fill_kernel_profile(xyprofile, kernel,
                                       CPL_KERNEL_DEF_WIDTH)) {
        cpl_vector_delete(xyprofile);
        cpl_ensure(0, cpl_error_get_code(), NULL);
    }

    interp_kernel = cpl_vector_get_data(xyprofile);


    /* Pre-compute leaps for 16 closest neighbor positions */
    leaps[0] = -1 - sizex ;
    leaps[1] =    - sizex ;
    leaps[2] =  1 - sizex ;
    leaps[3] =  2 - sizex ;

    leaps[4] = -1 ;
    leaps[5] =  0 ;
    leaps[6] =  1 ;
    leaps[7] =  2 ;

    leaps[8] = -1 + sizex ;
    leaps[9] =      sizex ;
    leaps[10]=  1 + sizex ;
    leaps[11]=  2 + sizex ;

    leaps[12]= -1 + 2*sizex ;
    leaps[13]=      2*sizex ;
    leaps[14]=  1 + 2*sizex ;
    leaps[15]=  2 + 2*sizex ;

    /* Create output contribution image */
    contrib = cpl_image_new(nx, ny, CPL_TYPE_INT) ;
    pcontrib = cpl_image_get_data_int(contrib) ;

    /* Create acc array */
    acc = cpl_malloc(nima * sizeof(double)) ;
    
    /* Switch on the data type */
    switch (type) {
#define CPL_CLASS CPL_CLASS_DOUBLE
#include "cpl_geom_img_body.h"
#undef CPL_CLASS

#define CPL_CLASS CPL_CLASS_FLOAT
#include "cpl_geom_img_body.h"
#undef CPL_CLASS
        default:
            cpl_free(acc) ;
            cpl_vector_delete(xyprofile);
            cpl_image_delete(contrib) ;
            cpl_ensure(0, CPL_ERROR_TYPE_MISMATCH, NULL) ;
    }

    /* Free and return */
    cpl_vector_delete(xyprofile);
    cpl_free(acc) ;
    out_arr = cpl_malloc(2*sizeof(cpl_image *)) ;
    out_arr[0] = final ;
    out_arr[1] = contrib ;
    return out_arr ;
}
#undef CPL_OPERATION
  
/**@}*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Compute the median offset between 2 images
  @param    im1         First input image
  @param    im2         Second input image
  @param    anchors     List of cross-correlation points in the first image
  @param    s_hx        Search area half-width.
  @param    s_hy        Search area half-height.
  @param    m_hx        Measurement area half-width.
  @param    m_hy        Measurement area half-height.
  @param    offs_x      X offset estimate - updated with the computed offset.
  @param    offs_y      Y offset estimate - updated with the computed offset.
  @return   The minimal squared difference factor or -1.0 in error case
 
  This function compares two images using a 2d cross-correlation. The input 
  images are expected to be more or less the same, up to a shift in x and y. 
 
  (offs_x, offs_y) has to be provided by the user. It is used as a first 
  estimate, and updated with the computed subpixel precision offsets.
  The input/returned offsets (offs_x, offs_y) is the a priori offset that has 
  to be used to shift im2 to align it on im1.
 
  The computed offset has a subpixel precision.
  
  The cross-correlation is carried out for each anchor point given in the list.
  It is Ok to provide only one anchor point.
 
  For each point, a cross-correlation criterion will be computed on a grid of 
  2m_hx+1 by 2m_hy+1, on the search area defined by 2s_hx+1 by 2s_hy+1. The 
  total number of pixel operations is quite high: number of anchor points 
  times number of search area pixels times number of measurement area pixels.
  
  The supported types are CPL_TYPE_DOUBLE and CPL_TYPE_FLOAT and the two 
  input images have to be of the same type.
  The bad pixel maps are ignored by this function.
  
  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT
  - CPL_ERROR_ILLEGAL_INPUT
 */
/*----------------------------------------------------------------------------*/
static double cpl_geom_ima_offset_xcorr(
        const cpl_image         *   im1,
        const cpl_image         *   im2,
        const cpl_bivector      *   anchors,
        int                         s_hx,
        int                         s_hy,
        int                         m_hx,
        int                         m_hy,
        double                  *   offs_x,
        double                  *   offs_y)
{
    int                 nx, ny ;
    const double    *   anchor_x ;
    const double    *   anchor_y ;
    cpl_bivector    *   delta ;
    double          *   delta_x ;
    double          *   delta_y ;
    cpl_vector      *   correl ;
    double          *   correl_data ;
    int                 nanchors ;
    cpl_bivector  *   delta_ok ;
    double          *   delta_ok_x ;
    double          *   delta_ok_y ;
    cpl_vector      *   correl_ok ;
    double          *   correl_ok_data ;
    int                 nvalid ;
    int                 x1, y1, x2, y2 ;
    double              median_dx, median_dy ;
    int                 best_rank ;
    double              diff, min_diff ;
    double              best_xcorr ; 
    int                 i, j ;

    /* Check entries */
    cpl_ensure(im1 && im2 && anchors, CPL_ERROR_NULL_INPUT, -1.00) ;
    
    /* Initialise */
    nx = cpl_image_get_size_x(im2) ;
    ny = cpl_image_get_size_y(im2) ;
    nvalid = 0 ;
    nanchors = cpl_bivector_get_size(anchors) ;
    anchor_x = cpl_bivector_get_x_data_const(anchors) ;
    anchor_y = cpl_bivector_get_y_data_const(anchors) ;

    /* Loop on all correlating points */
    correl = cpl_vector_new(nanchors) ;
    correl_data = cpl_vector_get_data(correl) ;
    delta  = cpl_bivector_new(nanchors) ; 
    delta_x = cpl_bivector_get_x_data(delta) ;
    delta_y = cpl_bivector_get_y_data(delta) ;
    
    for (i=0 ; i<nanchors ; i++) {
        x1 = (int)(anchor_x[i]) ;
        y1 = (int)(anchor_y[i]) ;
        x2 = x1 - (int)(*offs_x) ;
        y2 = y1 - (int)(*offs_y) ;

        if ((x2 < s_hx+m_hx) || (x2 >= (nx-s_hx-m_hx)) ||
            (y2 < s_hy+m_hy) || (y2 >= (ny-s_hy-m_hy))) {
            /* This point is declared invalid in the current image */
            delta_x[i]     =  0.0 ;
            delta_y[i]     =  0.0 ;
            correl_data[i] = -1.0 ;
        } else {
            if ((correl_data[i] = cpl_geom_ima_offset_xcorr_subw(im1, im2, x1, 
                            y1, x2, y2, s_hx, s_hy, m_hx, m_hy, &(delta_x[i]), 
                            &(delta_y[i]))) >= 0) nvalid ++ ;
        }
    }

    /* Test if there are valid points */
    if (nvalid < 1) {
        cpl_bivector_delete(delta) ;
        cpl_vector_delete(correl) ;
        cpl_ensure(0, CPL_ERROR_ILLEGAL_OUTPUT, -1.00) ;
    }

    /* If there was a single valid measurement point, return the offset */
    if (nvalid == 1) {
        i=0 ;
        while (correl_data[i] < 0.0) i++ ;
        *offs_x = x1 - (x2 - delta_x[i]) ;
        *offs_y = y1 - (y2 - delta_y[i]) ;
        best_xcorr = correl_data[i] ;
        cpl_bivector_delete(delta) ;
        cpl_vector_delete(correl) ;
        return best_xcorr;
    }

    /* Squeeze the input list to keep only valid points if necessary */
    if (nvalid < nanchors) {
        correl_ok = cpl_vector_new(nvalid) ;
        correl_ok_data = cpl_vector_get_data(correl_ok) ;
        delta_ok  = cpl_bivector_new(nvalid) ; 
        delta_ok_x = cpl_bivector_get_x_data(delta_ok) ;
        delta_ok_y = cpl_bivector_get_y_data(delta_ok) ;
        i=0 ; j=0 ;
        for (i=0 ; i<nanchors ; i++) {
            if (correl_data[i] >= 0.0) {
                delta_ok_x[j] = delta_x[i] ;
                delta_ok_y[j] = delta_y[i] ;
                correl_ok_data[j] = correl_data[i] ;
                j++ ;
            }
        }
        cpl_bivector_delete(delta) ;
        cpl_vector_delete(correl) ;
        delta = delta_ok ;
        correl = correl_ok ;
        correl_data = cpl_vector_get_data(correl) ;
        delta_x = cpl_bivector_get_x_data(delta) ;
        delta_y = cpl_bivector_get_y_data(delta) ;
    }
    

    /* From all correlation measurement, compute a median offset */
    /* FIXME: Call cpl_vector_get_median() instead */
    median_dx = cpl_vector_get_median_const(cpl_bivector_get_x(delta)) ;
    median_dy = cpl_vector_get_median_const(cpl_bivector_get_y(delta)) ;

    /* Find the offset measurement which is the closest to this median */
    best_rank = 0 ;
    min_diff = fabs(delta_x[0]-median_dx) + fabs(delta_y[0]-median_dy) ;

    for (i=0 ; i<nvalid ; i++) {
        diff = fabs(delta_x[i]-median_dx) + fabs(delta_y[i]-median_dy) ;
        if (diff<min_diff) best_rank = i ;
    }

    /* Free and return */
    *offs_x = x1 - (x2 - delta_x[best_rank]) ;
    *offs_y = y1 - (y2 - delta_y[best_rank]) ;
    best_xcorr = correl_data[best_rank] ;
    cpl_bivector_delete(delta) ;
    cpl_vector_delete(correl) ;
    return best_xcorr ;
}

#define CPL_OPERATION CPL_GEOM_IMA_OFFSET_XCORR
/*----------------------------------------------------------------------------*/
/**
  @brief    Correlate two images subwindow 
  @param    im1         First input image
  @param    im2         Second input image
  @param    im1_posx    X-coordinate of the subwindow center in im1
  @param    im1_posy    Y-coordinate of the subwindow center in im1
  @param    im2_posx    X-coordinate of the subwindow center in im2
  @param    im2_posy    Y-coordinate of the subwindow center in im2
  @param    s_hx        Search area half-width.
  @param    s_hy        Search area half-height.
  @param    m_hx        Measurement area half-width.
  @param    m_hy        Measurement area half-height.
  @param    offs_x      Returned apodised position in x.
  @param    offs_y      Returned apodised position in y.
  @return   The minimal squared difference factor or -1.0 in error case
 
  The computed offset has a subpixel precision.
 
  The double returned by the function measures the lowest squared difference 
  between the two input images subwindows (in the search area and over the 
  measurement area as requested by the caller). It is often a good indicator 
  of how well the cross-correlation performed.

  (offs_x, offs_y) has to be subtracted to (im2_posx, im2_posy) to retrieve 
  the exact position of the point in im2 corresponding to the point at position
  (im1_posx, im1_posy).
  
  The supported types are CPL_TYPE_DOUBLE and CPL_TYPE_FLOAT and the two 
  input images have to be of the same type.
  The bad pixel maps are ignored by this function.
  
  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT
  - CPL_ERROR_ILLEGAL_INPUT
  - CPL_ERROR_TYPE_MISMATCH
 */
/*----------------------------------------------------------------------------*/
static double cpl_geom_ima_offset_xcorr_subw(
        const cpl_image *   im1,
        const cpl_image *   im2,
        int                 im1_posx,
        int                 im1_posy,
        int                 im2_posx,
        int                 im2_posy,
        int                 s_hx,
        int                 s_hy,
        int                 m_hx,
        int                 m_hy,
        double          *   offs_x,
        double          *   offs_y)
{
    int                     nx1, nx2, ny1, ny2 ;
    cpl_type                type ;
    double                  sum_min ;
    int                     k_min, l_min ;
    double              *   correl ;
    double                  inv_surface ;
    int                     inc1, inc2 ;
    int                     i, j, k, l ;
    register double         value, sum ;
    double                  inc_x, inc_y ;
    int                     pos_min ;
    double                  best_correl ;
    
    /* Initialise */
    nx1 = cpl_image_get_size_x(im1) ;
    ny1 = cpl_image_get_size_y(im1) ;
    nx2 = cpl_image_get_size_x(im2) ;
    ny2 = cpl_image_get_size_y(im2) ;
    type = cpl_image_get_type(im1) ;
    sum_min = DBL_MAX;
    k_min = l_min = 0 ;
    inv_surface = 1.0 / ((double)(2*m_hx+1)*(double)(2*m_hy+1)) ;
    
    /* Check entries */
    cpl_ensure(im1 && im2, CPL_ERROR_NULL_INPUT, -1.00) ;
    cpl_ensure(cpl_image_get_type(im1)==cpl_image_get_type(im2), 
            CPL_ERROR_ILLEGAL_INPUT, -1.00) ;
    cpl_ensure((im1_posx>=0) && (im1_posx<nx1), CPL_ERROR_ILLEGAL_INPUT,
            -1.00) ;
    cpl_ensure((im2_posx>=0) && (im2_posx<nx2), CPL_ERROR_ILLEGAL_INPUT,
            -1.00) ;
    cpl_ensure((im1_posx>s_hx+m_hx) && (im1_posy>s_hy+m_hy) &&
            (im1_posx<nx1-s_hx-m_hx) && (im1_posy<ny1-s_hy-m_hy),
            CPL_ERROR_ILLEGAL_INPUT, -1.00) ;
    cpl_ensure((im2_posx>s_hx+m_hx) && (im2_posy>s_hy+m_hy) &&
            (im2_posx<nx2-s_hx-m_hx) && (im2_posy<ny2-s_hy-m_hy),
            CPL_ERROR_ILLEGAL_INPUT, -1.00) ;

    /* Allocate correl array */
    correl = cpl_malloc((2*s_hx+1)*(2*s_hy+1)*sizeof(double)) ;
 
    /* Switch on the data type */
    switch (type) {
#define CPL_CLASS CPL_CLASS_DOUBLE
#include "cpl_geom_img_body.h"
#undef CPL_CLASS

#define CPL_CLASS CPL_CLASS_FLOAT
#include "cpl_geom_img_body.h"
#undef CPL_CLASS
        default:
            cpl_free(correl) ;
            cpl_ensure(0, CPL_ERROR_TYPE_MISMATCH, -1.00) ;
    }

    /* Store results (pixel precision) */
    pos_min = s_hx + k_min + (2*s_hx+1) * (s_hy+l_min) ;
    best_correl = correl[pos_min] ;

    /* Compute inc_x and inc_y (sub-pixel precision) */
    if ((k_min == -s_hx) || (k_min == s_hx)) inc_x = 0.0 ;
    else inc_x = 0.5*((correl[pos_min-1]-correl[pos_min+1])/
            (correl[pos_min-1]-(2.0*correl[pos_min])+correl[pos_min+1])) ; 
    if ((l_min == -s_hy) || (l_min == s_hy)) inc_y = 0.0 ;
    else inc_y = 0.5*((correl[pos_min-(2*s_hx+1)]-correl[pos_min+(2*s_hx+1)])/
            (correl[pos_min-(2*s_hx+1)]-(2.0*correl[pos_min])+
             correl[pos_min+(2*s_hx+1)])) ;

    /* Return the sub-pixel precision offsets */
    *offs_x = (double)k_min + inc_x ;
    *offs_y = (double)l_min + inc_y ;

    /* Free and return */
    cpl_free(correl) ;
    return best_correl ;
}
#undef CPL_OPERATION