/* $Id: hawki_distortion.c,v 1.33 2013-03-25 11:35:07 cgarcia Exp $
 *
 * This file is part of the HAWKI Pipeline
 * Copyright (C) 2002,2003 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

/*
 * $Author: cgarcia $
 * $Date: 2013-03-25 11:35:07 $
 * $Revision: 1.33 $
 * $Name: not supported by cvs2svn $
 */

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

//Minimization algorithm hard-coded constants
#define HAWKI_DISTORTION_MAX_ITER 10000
#define HAWKI_DISTORTION_TOLERANCE 0.001
#define HAWKI_DISTORTION_MAX_ITER2 100000
#define HAWKI_DISTORTION_TOLERANCE2 0.0001

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

#include <math.h>
#include <cpl.h>
#include <cxdeque.h>
#ifdef HAVE_LIBGSL
#include <gsl/gsl_multimin.h>
#endif

#include "hawki_distortion.h"
#include "hawki_dfs_legacy.h"
#include "hawki_utils_legacy.h"
#include "hawki_load.h"



/*----------------------------------------------------------------------------*/
/**
 * @defgroup hawki_distortion       Distortion correction
 */
/*----------------------------------------------------------------------------*/

/**@{*/

/**
 * This private structure allows to pass parameters to the minimization
 * function used to compute the distortion solution
 * @see hawki_distortion_compute_solution 
 */
struct _hawki_distortion_obj_function_args_
{
    const cpl_table   ** ref_catalogues;
    const cpl_table    * matching_sets;
    cpl_bivector       * offsets;
    hawki_distortion   * distortion;
    int                  ncats;
};
    

//Private functions

int hawki_distortion_interpolate_distortion
(const hawki_distortion * distortion, 
 double                   x_pos,
 double                   y_pos,
 double                 * x_dist, 
 double                 * y_dist);

double hawki_distortion_compute_rms
(const cpl_table    ** ref_catalogues,
 const cpl_bivector  * cat_offsets,
 const cpl_table     * matching_sets,
 int                   ncats,
 hawki_distortion    * distortion);

#ifdef HAVE_LIBGSL
double hawki_distortion_gsl_obj_function
(const gsl_vector * dist_param,
 void             * args);

int hawki_distortion_update_solution_from_param
(hawki_distortion * distortion,
 const gsl_vector * dist_param);

int hawki_distortion_update_offsets_from_param
(cpl_bivector      * offsets,
 const gsl_vector  * dist_param);

int hawki_distortion_update_param_from_solution
(gsl_vector             * dist_param,
 const hawki_distortion * distortion);

int hawki_distortion_update_param_from_offsets
(gsl_vector              * dist_param,
 const cpl_bivector      * offsets);
#endif

void hawki_distortion_get_flag_vars
(double * x_val, double * y_val, int *pos_flag,
 int nvals, int *nvalid, double *var_x, double *var_y);


/*----------------------------------------------------------------------------*/
/**
  @brief    Allocate a new distortion solution
  @param    detector_nx  The dimensions in X of a HAWK-I detector
  @param    detector_nx  The dimensions in Y of a HAWK-I detector
  @param    grid_size    The number of points in the distortion grid
  @return   The allocated distortion if everything is ok , NULL otherwise
 */
/*----------------------------------------------------------------------------*/
hawki_distortion * hawki_distortion_grid_new
(int detector_nx, 
 int detector_ny, 
 int grid_size)
{
    hawki_distortion * distortion;
    
    //Allocate the structure
    distortion = cpl_malloc(sizeof(hawki_distortion));
    
    //Allocate the images
    distortion->dist_x = cpl_image_new
        (grid_size, grid_size, CPL_TYPE_FLOAT);
    distortion->dist_y = cpl_image_new
        (grid_size, grid_size, CPL_TYPE_FLOAT);
    
    //Create the transformation between distortion images and the detector
    distortion->x_cdelt = detector_nx / (double)grid_size; 
    distortion->y_cdelt = detector_ny / (double)grid_size;
    distortion->x_crval  = 0.5 + 0.5 * distortion->x_cdelt;
    distortion->y_crval  = 0.5 + 0.5 * distortion->y_cdelt;
    
    return distortion;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Deallocates a hawki_distortion structure
  @param    distortion  The distortion to allocate
 */
/*----------------------------------------------------------------------------*/
void hawki_distortion_delete
(hawki_distortion * distortion)
{
    if(distortion == NULL)
        return;
    cpl_image_delete(distortion->dist_x);
    cpl_image_delete(distortion->dist_y);
    cpl_free(distortion);
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Loads the distortion solution for one detector
  @param    dist_x  the image with distortion in X 
  @param    dist_y  the image with distortion in Y 
  @param    idet    the detector to read.
  @return   The allocated distortion if everything is ok , NULL otherwise
 */
/*----------------------------------------------------------------------------*/
hawki_distortion * hawki_distortion_load
(const cpl_frame * dist_x,
 const cpl_frame * dist_y,
 int               idet)
{
    const char       * file_dist_x;
    const char       * file_dist_y;
    hawki_distortion * distortion;
    int                iext;
    cpl_propertylist * plist;
    
    //Allocate the structure
    distortion = cpl_malloc(sizeof(hawki_distortion));
    
    //Read the images
    file_dist_x = cpl_frame_get_filename(dist_x);
    file_dist_y = cpl_frame_get_filename(dist_y);
    distortion->dist_x = hawki_load_frame_detector
        (dist_x, idet, CPL_TYPE_FLOAT);
    distortion->dist_y = hawki_load_frame_detector
        (dist_y, idet, CPL_TYPE_FLOAT);
    
    //Read the WCS keywords
    iext = hawki_get_ext_from_detector(file_dist_x, idet);
    plist = cpl_propertylist_load(file_dist_x, iext);
    distortion->x_crval = cpl_propertylist_get_double(plist, "CRVAL1");
    distortion->x_cdelt = cpl_propertylist_get_double(plist, "CDELT1");
    distortion->y_crval = cpl_propertylist_get_double(plist, "CRVAL2");
    distortion->y_cdelt = cpl_propertylist_get_double(plist, "CDELT2");
    if(cpl_propertylist_get_double(plist, "CRPIX1") != 1 ||
       cpl_propertylist_get_double(plist, "CRPIX2") != 1)
    {
        cpl_error_set_message_macro(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
                                    __FILE__, __LINE__,"Wrong CRPIX? keywords");
        cpl_image_delete(distortion->dist_x);
        cpl_image_delete(distortion->dist_y);
        cpl_propertylist_delete(plist);
        cpl_free(distortion);
        return NULL;
    }
    cpl_propertylist_delete(plist);
    //Check that the keywords in X and Y are compatibles;
    plist = cpl_propertylist_load(file_dist_y, iext);
    if(distortion->x_crval != cpl_propertylist_get_double(plist, "CRVAL1") ||
       distortion->x_cdelt != cpl_propertylist_get_double(plist, "CDELT1") ||
       distortion->y_crval != cpl_propertylist_get_double(plist, "CRVAL2") ||
       distortion->y_cdelt != cpl_propertylist_get_double(plist, "CDELT2") ||
       cpl_propertylist_get_double(plist, "CRPIX1") != 1 ||
       cpl_propertylist_get_double(plist, "CRPIX2") != 1)
    {
        cpl_error_set_message_macro(cpl_func, CPL_ERROR_ILLEGAL_INPUT, __FILE__,
                __LINE__,"WCS keywords mismatch in X and Y distortions");
        cpl_image_delete(distortion->dist_x);
        cpl_image_delete(distortion->dist_y);
        cpl_propertylist_delete(plist);
        cpl_free(distortion);
        return NULL;
    }
    cpl_propertylist_delete(plist);
    
    return distortion;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Gives the dimension in X of the distortion solution 
  @param    dist    the distortion solution
  @return   The dimension in X
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_get_size_x
(const hawki_distortion * distortion)
{
    if(distortion == NULL)
    {
        cpl_error_set(__func__,CPL_ERROR_ILLEGAL_INPUT);
    }
    return cpl_image_get_size_x(distortion->dist_x);
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Gives the dimension in Y of the distortion solution 
  @param    dist    the distortion solution
  @return   The dimension in Y
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_get_size_y
(const hawki_distortion * distortion)
{
    if(distortion == NULL)
    {
        cpl_error_set(__func__,CPL_ERROR_ILLEGAL_INPUT);
    }
    return cpl_image_get_size_y(distortion->dist_x);
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Apply the distortion correction
  @param    ilist   the input image list (chips 1 2 3 4)
  @param    dist    the distortion file
  @return   0 if everything is ok, -1 otherwise
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_correct_alldetectors
(cpl_image        ** ilist,
 const cpl_frame   * frame_dist_x,
 const cpl_frame   * frame_dist_y)
{
    cpl_image        *   corr[HAWKI_NB_DETECTORS];
    hawki_distortion *   distortion;
    cpl_image        *   dist_x;
    cpl_image        *   dist_y;
    int                  idet, j ;

    /* Test entries */
    if (ilist == NULL) return -1 ;
    if (frame_dist_x == NULL) return -1 ;
    if (frame_dist_y == NULL) return -1 ;

    /* Loop on the 4 chips */
    for (idet=0 ; idet<HAWKI_NB_DETECTORS ; idet++) 
    {
        int nx;
        int ny;
        
        /* Get the image size */
        nx = cpl_image_get_size_x(ilist[idet]);
        ny = cpl_image_get_size_y(ilist[idet]);

        /* Load the distortion */
        if ((distortion = hawki_distortion_load
                 (frame_dist_x, frame_dist_y, idet + 1)) == NULL) 
        {
            cpl_msg_error(__func__, "Cannot load the distortion for chip %d", 
                    idet+1) ;
            for (j=0 ; j<idet ; j++) cpl_image_delete(corr[j]) ;
            return -1 ;
        }

        /* Create the offsets images */
        dist_x = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
        dist_y = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
        if (hawki_distortion_create_maps_detector
                (distortion, dist_x, dist_y))
        {
            cpl_msg_error(__func__, "Cannot create the distortion maps") ;
            cpl_image_delete(dist_x);
            cpl_image_delete(dist_y);
            for (j=0 ; j<idet ; j++) cpl_image_delete(corr[j]) ;
            return -1;
        }

        /* Correct this image */
        corr[idet] = hawki_distortion_correct_detector(ilist[idet], dist_x, dist_y);
        if(corr[idet] == NULL)
        {
            cpl_msg_error(__func__, "Cannot correct the distortion") ;
            hawki_distortion_delete(distortion);
            cpl_image_delete(dist_x);
            cpl_image_delete(dist_y);
            for (j=0 ; j<idet; j++) cpl_image_delete(corr[j]) ;
            return -1 ;
        }
        hawki_distortion_delete(distortion);
        cpl_image_delete(dist_x) ;
        cpl_image_delete(dist_y);
    }

    /* Store the results */
    for (idet=0 ; idet<HAWKI_NB_DETECTORS ; idet++)
    {
        cpl_image_delete(ilist[idet]) ;
        ilist[idet] = corr[idet] ;
    }
    
    /* Return */
    return 0 ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Apply the distortion correction to one chip
  @param    image   the input image
  @param    dist    the distortion table
  @return   0 if everything is ok, -1 otherwise
 */
/*----------------------------------------------------------------------------*/
cpl_image *  hawki_distortion_correct_detector
(cpl_image       *  image,
 cpl_image       *  dist_x,
 cpl_image       *  dist_y)
{
    cpl_image       *   corr;
    cpl_vector      *   profile ;
    
    /* Test entries */
    if (image  == NULL) return NULL;
    if (dist_x == NULL) return NULL;
    if (dist_y == NULL) return NULL;

    /* Create the output image */
    corr = cpl_image_new(cpl_image_get_size_x(image),
                         cpl_image_get_size_y(image), CPL_TYPE_FLOAT) ;

    /* Create the interpolation profile */
    profile = cpl_vector_new(CPL_KERNEL_DEF_SAMPLES) ;
    cpl_vector_fill_kernel_profile(profile, CPL_KERNEL_DEFAULT,
                                   CPL_KERNEL_DEF_WIDTH) ;

    /* Apply the distortion */
    if (cpl_image_warp(corr, image, dist_x, dist_y, profile, 
                       CPL_KERNEL_DEF_WIDTH, profile, 
                       CPL_KERNEL_DEF_WIDTH) != CPL_ERROR_NONE) 
    {
        cpl_msg_error(__func__, "Cannot warp the image") ;
        cpl_image_delete(corr) ;
        cpl_vector_delete(profile) ;
        return NULL;
     }
    cpl_vector_delete(profile) ;

    /* Return */
    return corr;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Apply the distortion correction to one detector position 
  @param    distortion  the distortion solution
  @param    x_pos       the X position in detector that we want to correct.
  @param    y_pos       the Y position in detector that we want to correct.
  @param    x_pos_distcorr  the X position in detector corrected from distortion
  @param    y_pos_distcorr  the Y position in detector corrected from distortion
  @return   0 if everything is ok, -1 otherwise
  
   This function will interpolate the distortion solution (given in 
   variable distortion, @see hawki_distortion_new)
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_correct_coords
(const hawki_distortion * distortion, 
 double                   x_pos,
 double                   y_pos,
 double                 * x_pos_distcorr, 
 double                 * y_pos_distcorr)
{
    double x_dist;
    double y_dist;
    
    if(distortion == NULL)
    {
        cpl_error_set("hawki_distortion_correct_coords", CPL_ERROR_ILLEGAL_INPUT);
        return -1;
    }
    
    hawki_distortion_interpolate_distortion
        (distortion, x_pos, y_pos, &x_dist, &y_dist);

    *x_pos_distcorr = x_pos - x_dist;
    *y_pos_distcorr = y_pos - y_dist;
    
    return 0;
}

    
/*----------------------------------------------------------------------------*/
/**
  @brief   Apply the inverse distortion correction to one detector position 
  @param   distortion  the distortion solution
  @param   x_pos       the X position in detector that we want to decorrect.
  @param   y_pos       the Y position in detector that we want to decorrect.
  @param   x_pos_distcorr the X position in detector decorrected from distortion
  @param   y_pos_distcorr the Y position in detector decorrected from distortion
  @return  0 if everything is ok, -1 otherwise
  
  This function is the inverse function of hawki_distortion_correct_coords.
  WARNING: Although this is the "inverse" of hawki_distortion_correct_coords,
  applying this function to positions delivered by 
  hawki_distortion_correct_coords will not provide the original positions back. 
  We are simply not using the same position to interpolate the distortion table.
  
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_inverse_correct_coords
(const hawki_distortion * distortion, 
 double                   x_pos,
 double                   y_pos,
 double                 * x_pos_distinvcorr, 
 double                 * y_pos_distinvcorr)
{
    double x_dist = 0;
    double y_dist = 0;
    int    i;
    int    niter = 3;
    
    if(distortion == NULL)
    {
        cpl_error_set("hawki_distortion_inverse_correct_coords", CPL_ERROR_ILLEGAL_INPUT);
        return -1;
    }
    for(i = 0; i < niter; ++i)
    {
        hawki_distortion_interpolate_distortion
            (distortion, x_pos + x_dist, y_pos + y_dist, &x_dist, &y_dist);
    }

    
    /* Apply the correction in the inverse direction */
    *x_pos_distinvcorr = x_pos + x_dist;
    *y_pos_distinvcorr = y_pos + y_dist;
    
    return 0;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Interpolates the distortion solution at a given position 
  @param    distortion  the distortion solution
  @param    x_pos       the X position in detector that we want to correct.
  @param    y_pos       the Y position in detector that we want to correct.
  @param    x_dist      the distortion in X at position x_pos, y_pos
  @param    y_dist      the distortion in Y at position x_pos, y_pos
  @return   0 if everything is ok, -1 otherwise
  
  This private function uses the four reference points found in distortion 
  closer to x_pos, y_pos. Then it weights each point with the 3rd power of the 
  distance to the target pixel and uses a linear interpolation.
  
  If this function is used in a multithread environment with a CPL version not 
  thread-safe, there is no risk as long as these two premises are hold:
  -The caller must check if distortion->dist_x and distortion->dist_y are not
   null before calling this function, because this is check by
   cpl_image_get_size_x and cpl_image_get.
  -The caller must check in advance that the mask has all the points valid, 
   because cpl_image_get checks for the validity of the pixel in the mask,
   if it is not valid, enters the error handling of CPL, which is not 
   thread-safe, at least before CPL 5.2.
  The rest of the calls are read-only, so it should be safe.  
    
  
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_interpolate_distortion
(const hawki_distortion * distortion, 
 double                   x_pos,
 double                   y_pos,
 double                 * x_dist, 
 double                 * y_dist)
{
    int             ix1;
    int             ix2;
    int             iy1;
    int             iy2;
    int             nx;
    int             ny;
    double          x1_pos;
    double          x2_pos;
    double          y1_pos;
    double          y2_pos;
    double          dx11;
    double          dx12;
    double          dx21;
    double          dx22;
    double          dy11;
    double          dy12;
    double          dy21;
    double          dy22;
    int             isnull;

    /* Get the size of the distortion images */
    nx = cpl_image_get_size_x(distortion->dist_x);
    ny = cpl_image_get_size_y(distortion->dist_x);

    //This uses bilinear interpolation
    //Get lower left corner. This assumes CRPIX? =1 and ix, iy start with 0
    ix1 = (int)floor((x_pos - distortion->x_crval)/distortion->x_cdelt);
    iy1 = (int)floor((y_pos - distortion->y_crval)/distortion->y_cdelt);
    //Handle extrapolation
    if(ix1 < 0)
        ix1 = 0;
    if(ix1 >= nx - 1)
        ix1 = nx - 2;
    if(iy1 < 0)
        iy1 = 0;
    if(iy1 >= ny - 1)
        iy1 = ny - 2;
    //Get upper right corner
    ix2 = ix1 + 1;
    iy2 = iy1 + 1;
    
    //Get the position values
    //This implies that CRPIX? = 1 and that ix, iy start at 0.
    x1_pos = ix1 * distortion->x_cdelt + distortion->x_crval; 
    x2_pos = ix2 * distortion->x_cdelt + distortion->x_crval; 
    y1_pos = iy1 * distortion->y_cdelt + distortion->y_crval; 
    y2_pos = iy2 * distortion->y_cdelt + distortion->y_crval; 
    
    //Get the values used to interpolate
    //The +1 is because cpl_image_get uses FITS convention
    dx11 = cpl_image_get(distortion->dist_x, ix1 + 1, iy1 + 1, &isnull);  
    dx21 = cpl_image_get(distortion->dist_x, ix2 + 1, iy1 + 1, &isnull);  
    dx12 = cpl_image_get(distortion->dist_x, ix1 + 1, iy2 + 1, &isnull);  
    dx22 = cpl_image_get(distortion->dist_x, ix2 + 1, iy2 + 1, &isnull);  
    dy11 = cpl_image_get(distortion->dist_y, ix1 + 1, iy1 + 1, &isnull);  
    dy21 = cpl_image_get(distortion->dist_y, ix2 + 1, iy1 + 1, &isnull);  
    dy12 = cpl_image_get(distortion->dist_y, ix1 + 1, iy2 + 1, &isnull);  
    dy22 = cpl_image_get(distortion->dist_y, ix2 + 1, iy2 + 1, &isnull);  
    
    
    //Compute the final values
    *x_dist = dx11 * (x2_pos - x_pos) * (y2_pos - y_pos) +
              dx21 * (x_pos - x1_pos) * (y2_pos - y_pos) +
              dx12 * (x2_pos - x_pos) * (y_pos - y1_pos) +
              dx22 * (x_pos - x1_pos) * (y_pos - y1_pos);
    *x_dist = *x_dist / (x2_pos -x1_pos) / (y2_pos -y1_pos);
    *y_dist = dy11 * (x2_pos - x_pos) * (y2_pos - y_pos) +
              dy21 * (x_pos - x1_pos) * (y2_pos - y_pos) +
              dy12 * (x2_pos - x_pos) * (y_pos - y1_pos) +
              dy22 * (x_pos - x1_pos) * (y_pos - y1_pos);
    *y_dist = *y_dist / (x2_pos -x1_pos) / (y2_pos -y1_pos);

    return 0;

}

/*----------------------------------------------------------------------------*/
/**
  @brief    Apply the distortion correction
  @param    ilist   the input image list (chips 1 2 3 4)
  @param    dist    the distortion file
  @return   0 if everything is ok, -1 otherwise
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_apply_maps
(cpl_imagelist   *   ilist, 
 cpl_image       **  dist_x,
 cpl_image       **  dist_y)
{
    cpl_image       *   corr[HAWKI_NB_DETECTORS] ;
    int                 i, j ;

    /* Test entries */
    if (ilist == NULL) return -1 ;
    if (dist_x == NULL) return -1 ;
    if (dist_y == NULL) return -1 ;

    /* Loop on the 4 chips */
    for (i=0 ; i<HAWKI_NB_DETECTORS ; i++)
    {
        cpl_image * cur_image;

        /* Get the current image */
        cur_image = cpl_imagelist_get(ilist, i);
        
        /* Correct this image */
        corr[i] = hawki_distortion_correct_detector(cur_image,dist_x[i],dist_y[i]);
        if(corr[i] == NULL)
        {
            cpl_msg_error(__func__, "Cannot correct the distortion") ;
            for (j=0 ; j<i ; j++) cpl_image_delete(corr[j]) ;
            return -1 ;
        }
    }

    /* Store the results */
    for (i=0 ; i<HAWKI_NB_DETECTORS ; i++) 
        cpl_imagelist_set(ilist, corr[i], i);
    
    /* Return */
    return 0 ;
}

/**@}*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Build the delta x and y maps from the table
  @param    dist_tab    the distortion table
  @param    dist_x      the distortion image in x
  @param    dist_y      the distortion image in y
  @return   0 if everything is ok, -1 otherwise
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_create_maps_detector
(const hawki_distortion * distortion,
 cpl_image              * dist_x,
 cpl_image              * dist_y)
{
    double              *   pdx;
    double              *   pdy;
    int                     nx, ny;
    int                     pos;
    int                     i, j;

    /* Test entries */
    if (distortion == NULL) return -1 ;
    if (dist_x == NULL) return -1 ;
    if (dist_y == NULL) return -1 ;

    /* Initialise */
    nx = cpl_image_get_size_x(dist_x) ;
    ny = cpl_image_get_size_y(dist_x) ;
    if (cpl_image_get_size_x(dist_y) != nx) return -1 ;
    if (cpl_image_get_size_y(dist_y) != ny) return -1 ;
  
    /* Access to the data */
    pdx = cpl_image_get_data_double(dist_x) ;
    pdy = cpl_image_get_data_double(dist_y) ;

    /* Loop on the pixels */
    for (j=0 ; j<ny ; j++) {
        for (i=0 ; i<nx ; i++) {
            double x_dist;
            double y_dist;

            pos = i+j*nx ;

            hawki_distortion_interpolate_distortion
                (distortion, (double)i, (double)j, &x_dist, &y_dist);
            
            pdx[pos] = x_dist;
            pdy[pos] = y_dist;
        }
    }

    return 0 ; 
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Compute the distortion solution using an autocalibration method
  @param    ref_catalogues  the catalogues containing stars to use
                            for autocalibration 
  @param    cat_offsets     the approximate offsets for each of the catalogues
  @param    matching_sets   a table containing the matchings between catalog
                            sources
  dist_y      the distortion image in y
  @return   0 if everything is ok, -1 otherwise
  
   This function asumes that the input catalogues (ncats in number) contain 
   columns named X_POS, Y_POS, which are the positions of the stars in each
   of the ncats images.
   The matching_sets table contains in the column "MATCHING_SETS" an array of 
   ncats length. Each array represents one matching. The position in the array
   corresponds to each catalogue, and the value corresponds to the object in 
   the catalogue. If the value is -1 then there is no match for that catalogue.
   The function uses an iterative approach trying to minimize the distortion
   corrected residuals for each match.
   
   TODO: The function has to fit the offsets as well.
  
 */
/*----------------------------------------------------------------------------*/
hawki_distortion *  hawki_distortion_compute_solution
(const cpl_table       ** ref_catalogues,
 const cpl_bivector     * initial_offsets,
 const cpl_table        * matching_sets,
 int                      ncats,
 int                      detector_nx,
 int                      detector_ny,
 int                      grid_size,
 const hawki_distortion * initial_guess,
 double                 * rms)
{
#ifdef HAVE_LIBGSL
    
    hawki_distortion                          * distortion;
    cpl_bivector                              * offsets;
    gsl_multimin_fminimizer                   * minimizer;
    gsl_vector                                * step_size;
    gsl_vector                                * init_param;
    gsl_multimin_function                       minimize_function;
    
    int                                         nfitparam = 0;
    int                                         iparam;
    double                                      tolerance = HAWKI_DISTORTION_TOLERANCE;
    double                                      tolerance2 = HAWKI_DISTORTION_TOLERANCE2;
    int                                         minimizer_status;
    int                                         iter = 0;
    struct _hawki_distortion_obj_function_args_ args;

    /* Initialize the distortion solution */
    if(initial_guess == NULL)
    {
        distortion = hawki_distortion_grid_new
            (detector_nx, detector_ny, grid_size);
    }
    else
    {
        distortion = cpl_malloc(sizeof(hawki_distortion));
        distortion->dist_x  = cpl_image_duplicate(initial_guess->dist_x);
        distortion->dist_y  = cpl_image_duplicate(initial_guess->dist_y);
        distortion->x_cdelt = initial_guess->x_cdelt;
        distortion->x_crval = initial_guess->x_crval;
        distortion->y_cdelt = initial_guess->y_cdelt;
        distortion->y_crval = initial_guess->y_crval;
        //We have to fit all the distortion coefficients plus
        //the offsets of the catalogues
    }
    //We have to fit all the distortion coefficients plus
    //the offsets of the catalogues
    nfitparam = grid_size * grid_size * 2 + ncats * 2;
    offsets = cpl_bivector_duplicate(initial_offsets);
    if(cpl_table_get_nrow(matching_sets) * 2 < nfitparam)
    {
        cpl_msg_error(__func__,"Too few matches to compute distortion (< %d)",
                      nfitparam);
        hawki_distortion_delete(distortion);
        return NULL;
    }

    /* Setup function to minimize */
    args.ref_catalogues = ref_catalogues;
    args.matching_sets  = matching_sets;
    args.distortion     = distortion;
    args.offsets        = offsets;
    args.ncats          = ncats;
    
    minimize_function.f      = hawki_distortion_gsl_obj_function;
    minimize_function.n      = nfitparam;
    minimize_function.params = &args;
    
    /* Setup minimizer */
    minimizer = gsl_multimin_fminimizer_alloc
        (gsl_multimin_fminimizer_nmsimplex, nfitparam);
    step_size = gsl_vector_alloc(nfitparam);
    init_param = gsl_vector_alloc(nfitparam);
    for(iparam = 0; iparam < grid_size * grid_size * 2; ++iparam)
        gsl_vector_set(step_size, iparam, 5);
    for(iparam = grid_size * grid_size * 2;
        iparam < nfitparam; ++iparam)
        gsl_vector_set(step_size, iparam, 1);
    hawki_distortion_update_param_from_solution(init_param, distortion);
    hawki_distortion_update_param_from_offsets(init_param, offsets);
    gsl_multimin_fminimizer_set(minimizer, &minimize_function,
                                init_param, step_size);

    do
    {
        ++iter;
        minimizer_status = gsl_multimin_fminimizer_iterate (minimizer);
        if(minimizer_status)
            break;
        minimizer_status = gsl_multimin_test_size
            (gsl_multimin_fminimizer_size(minimizer), tolerance);
        cpl_msg_debug(__func__,"Iteration %d Minimum: %g",
                      iter, gsl_multimin_fminimizer_minimum(minimizer));
    }
    while (minimizer_status == GSL_CONTINUE && iter < HAWKI_DISTORTION_MAX_ITER);

    cpl_msg_warning(__func__, "rms before computing %f", hawki_distortion_compute_rms(ref_catalogues, offsets, 
                                        matching_sets, ncats, distortion));

    
    //Do it again to avoid local minimum
    gsl_multimin_fminimizer_set(minimizer, &minimize_function,
                                gsl_multimin_fminimizer_x(minimizer), step_size);
    iter = 0;
    do
    {
        ++iter;
        minimizer_status = gsl_multimin_fminimizer_iterate (minimizer);
        if(minimizer_status)
            break;
        minimizer_status = gsl_multimin_test_size
            (gsl_multimin_fminimizer_size(minimizer), tolerance2);
        cpl_msg_debug(__func__,"2nd run Iteration %d Minimum: %g",
                      iter, gsl_multimin_fminimizer_minimum(minimizer));
    }
    while (minimizer_status == GSL_CONTINUE && iter < HAWKI_DISTORTION_MAX_ITER2);

    /* Update the distortion solution */
    hawki_distortion_update_solution_from_param
        (distortion, gsl_multimin_fminimizer_x(minimizer));
    hawki_distortion_update_offsets_from_param
        (offsets, gsl_multimin_fminimizer_x(minimizer));
    
    *rms = hawki_distortion_compute_rms(ref_catalogues, offsets, 
                                        matching_sets, ncats, distortion);
    
    /* Free and return */
    gsl_multimin_fminimizer_free(minimizer);
    gsl_vector_free(init_param);
    gsl_vector_free(step_size);
    cpl_bivector_delete(offsets);

    return distortion;
#else
    cpl_msg_error(__func__,"Not compiled with GSL support.");
    return NULL;
#endif
}

#ifdef HAVE_LIBGSL
/*----------------------------------------------------------------------------*/
/**
  @brief    This private function computes the rms, which is the objective
            function of the minimization algorithm. This function
            has the needed interface for gsl, so it is a wrapper of
            @see hawki_distortion_compute_rms
            @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
double hawki_distortion_gsl_obj_function
(const gsl_vector * dist_param,
 void             * args)
{
    struct _hawki_distortion_obj_function_args_  args_struct;
    const cpl_table                           ** ref_catalogues;
    const cpl_table                            * matching_sets;
    hawki_distortion                           * distortion;
    cpl_bivector                               * offsets;
    int                                          ncats;
    double                                       rms;
    double                                       objective_function;

    
    
    args_struct    = *(struct _hawki_distortion_obj_function_args_ * )args;
    ref_catalogues = args_struct.ref_catalogues; 
    matching_sets  = args_struct.matching_sets; 
    distortion     = args_struct.distortion;
    offsets        = args_struct.offsets;
    ncats          = args_struct.ncats; 
    
    hawki_distortion_update_solution_from_param(distortion, dist_param);
    hawki_distortion_update_offsets_from_param(offsets, dist_param);
    
    rms = hawki_distortion_compute_rms(ref_catalogues, offsets, 
                                       matching_sets, ncats, distortion);
    
    objective_function = rms; 
    
    
    cpl_msg_debug(__func__,"Objective function: %g", objective_function);
    return objective_function;
}
#endif

/*----------------------------------------------------------------------------*/
/**
  @brief    This private function computes the rms, which is the objective
            function of the minimization algorithm. 
            @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
double hawki_distortion_compute_rms
(const cpl_table   ** ref_catalogues,
 const cpl_bivector * cat_offsets,
 const cpl_table    * matching_sets,
 int                  ncats,
 hawki_distortion   * distortion)
{
    int                icat;
    int                imatch;
    int                nmatch;
    double             rms = 0;
    const double     * x_cat_offsets;
    const double     * y_cat_offsets;
    const double    ** x_cat_cols; 
    const double    ** y_cat_cols;
    const cpl_array ** match_arrays;
    double          ** x_pos_values;
    double          ** y_pos_values;
    int             ** pos_flag;

    
    
    nmatch = cpl_table_get_nrow(matching_sets);

    x_cat_offsets = cpl_vector_get_data_const
        (cpl_bivector_get_x_const(cat_offsets));
    y_cat_offsets = cpl_vector_get_data_const
        (cpl_bivector_get_y_const(cat_offsets));

    x_cat_cols = cpl_malloc(sizeof(double *) * ncats);
    y_cat_cols = cpl_malloc(sizeof(double *) * ncats);
    for(icat = 0; icat < ncats; ++icat)
    {
        x_cat_cols[icat] = cpl_table_get_data_double_const(ref_catalogues[icat],
                                                           HAWKI_COL_OBJ_POSX);
        y_cat_cols[icat] = cpl_table_get_data_double_const(ref_catalogues[icat],
                                                           HAWKI_COL_OBJ_POSY);
    }

    match_arrays = cpl_malloc(sizeof(cpl_array *) * nmatch);
    x_pos_values = cpl_malloc(sizeof(double *) * nmatch);
    y_pos_values = cpl_malloc(sizeof(double *) * nmatch);
    pos_flag     = cpl_malloc(sizeof(int *) * nmatch);
    for(imatch = 0; imatch < nmatch; ++imatch)
    {
        match_arrays[imatch] = cpl_table_get_array(matching_sets, 
                HAWKI_COL_MATCHING_SETS, imatch);
        x_pos_values[imatch] = cpl_malloc(sizeof(double) * ncats);
        y_pos_values[imatch] = cpl_malloc(sizeof(double) * ncats);
        pos_flag[imatch]     = cpl_malloc(sizeof(int) * ncats);
    }

#ifdef _OPENMP    
#pragma omp parallel for  private(icat,imatch) reduction(+:rms)
#endif    
    for(imatch = 0; imatch < nmatch; ++imatch)
    {
        int                 nstddev;
        double              var_x;
        double              var_y;
                
        for(icat = 0; icat < ncats; ++icat)
        {
            int    iobj;
            double x_cat_offset;
            double y_cat_offset;
            
            x_cat_offset = x_cat_offsets[icat];
            y_cat_offset = y_cat_offsets[icat];            

            if((iobj = cpl_array_get(match_arrays[imatch], icat, NULL)) != -1)
            {
                double x_cat;
                double y_cat;
                double x_dist_corr;
                double y_dist_corr;
                double x_dist;
                double y_dist;
                double x_glob;
                double y_glob;

                x_cat = x_cat_cols[icat][iobj];
                y_cat = y_cat_cols[icat][iobj];


                //These 4 lines of code are from hawki_distortion_correct_coords.
                //The are repeated here to avoid a cpl call, which is not thread-safe
                //Two checks to ensure thread-safety:
                //We have ensured outside the loop that distortion->dist_x and
                //distortion->dist_y are not null. 
                //We have checked outside the loop the mask has all the points valid
                hawki_distortion_interpolate_distortion
                    (distortion, x_cat, y_cat, &x_dist, &y_dist);
                x_dist_corr = x_cat - x_dist;
                y_dist_corr = y_cat - y_dist;


                x_glob = x_dist_corr + x_cat_offset;
                y_glob = y_dist_corr + y_cat_offset;
                x_pos_values[imatch][icat] = x_glob;
                y_pos_values[imatch][icat] = y_glob;
                pos_flag[imatch][icat] = 1;
           }
           else
               pos_flag[imatch][icat] = 0;
        }

        hawki_distortion_get_flag_vars(x_pos_values[imatch], 
                                       y_pos_values[imatch], pos_flag[imatch],
                                       ncats, &nstddev, &var_x, &var_y);
        
        //The rms is counted as many times as this star is the list of catalogs.
        rms += sqrt(var_x + var_y) * nstddev;
        
    }
    cpl_free(x_cat_cols);
    cpl_free(y_cat_cols);
    for(imatch = 0; imatch < nmatch; ++imatch)
    {
        cpl_free(x_pos_values[imatch]);
        cpl_free(y_pos_values[imatch]);
        cpl_free(pos_flag[imatch]);
    }
    cpl_free(x_pos_values);
    cpl_free(y_pos_values);
    cpl_free(pos_flag);
    cpl_free(match_arrays);
    
    return rms;
}

#ifdef HAVE_LIBGSL
/*----------------------------------------------------------------------------*/
/**
  @brief    This private function updates the distortion solution given
            the gsl structure with the parameters.
            @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_update_solution_from_param
(hawki_distortion  * distortion,
 const gsl_vector  * dist_param)
{
    int     ipoint;
    int     ix;
    int     iy;
    int     nx;
    int     ny;
    double  x_dist_mean; 
    double  y_dist_mean; 
    
    nx  = cpl_image_get_size_x(distortion->dist_x);
    ny  = cpl_image_get_size_y(distortion->dist_x);
    for(ix = 0; ix < nx; ++ix)
        for(iy = 0; iy < ny; ++iy)
        {
            ipoint = ix + iy * nx;
            cpl_image_set(distortion->dist_x, ix+1, iy+1, 
                          gsl_vector_get(dist_param, ipoint * 2));
            cpl_image_set(distortion->dist_y, ix+1, iy+1, 
                          gsl_vector_get(dist_param, ipoint * 2 + 1));
        }
    
    /* Normalize to mean(distorsion) = 0 */
    x_dist_mean = cpl_image_get_mean(distortion->dist_x);
    y_dist_mean = cpl_image_get_mean(distortion->dist_y);
    cpl_image_subtract_scalar(distortion->dist_x, x_dist_mean);
    cpl_image_subtract_scalar(distortion->dist_y, y_dist_mean);

    return 0;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    This private function updates the offsets given
            the gsl structure with the parameters.
            @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_update_offsets_from_param
(cpl_bivector      * offsets,
 const gsl_vector  * dist_param)
{
    int     i;
    int     ncats;
    int     nparam;
    
    ncats  = cpl_bivector_get_size(offsets);
    nparam = dist_param->size;
    for(i = 0; i < ncats; ++i)
    {
        cpl_vector_set(cpl_bivector_get_x(offsets), i,  
                       gsl_vector_get(dist_param, nparam - 2 * ncats + 2 * i));
        cpl_vector_set(cpl_bivector_get_y(offsets), i,  
                       gsl_vector_get(dist_param, nparam - 2 * ncats + 2 * i + 1));
    }
    
    return 0;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    This private function updates the gsl structure with the parameters
            to fit given the distortion solution.
            @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_update_param_from_solution
(gsl_vector              * dist_param,
 const hawki_distortion  * distortion)
{
    int  ipoint;
    int  ix;
    int  iy;
    int  nx;
    int  ny;
    int  isnull;
    
    nx  = cpl_image_get_size_x(distortion->dist_x);
    ny  = cpl_image_get_size_y(distortion->dist_y);
    for(ix = 0; ix < nx; ++ix)
        for(iy = 0; iy < ny; ++iy)
        {
            ipoint = ix + iy * nx;
            gsl_vector_set(dist_param, ipoint * 2, 
                           cpl_image_get(distortion->dist_x, ix+1, iy+1, &isnull));
            gsl_vector_set(dist_param, ipoint * 2 + 1, 
                           cpl_image_get(distortion->dist_y, ix+1, iy+1, &isnull));
    }
    
    return 0;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    This private function updates the gsl structure with the parameters
            to fit given the offsets. @see hawki_distortion_compute_solution
 */
/*----------------------------------------------------------------------------*/
int hawki_distortion_update_param_from_offsets
(gsl_vector              * dist_param,
 const cpl_bivector      * offsets)
{
    int     i;
    int     ncats;
    int     nparam;
    
    ncats  = cpl_bivector_get_size(offsets);
    nparam = dist_param->size;
    for(i = 0; i < ncats; ++i)
    {
        gsl_vector_set(dist_param, nparam - 2 * ncats + 2 * i, 
                       cpl_vector_get(cpl_bivector_get_x_const(offsets), i));
        gsl_vector_set(dist_param, nparam - 2 * ncats + 2 * i + 1, 
                       cpl_vector_get(cpl_bivector_get_y_const(offsets), i));
    }

    return 0;
}
#endif

/*----------------------------------------------------------------------------*/
/**
  @brief    Compute the standard deviation of two vectors, not using flagged
            values            
  @return   The standard deviation
 */
/*----------------------------------------------------------------------------*/
void hawki_distortion_get_flag_vars
(double * x_val, double * y_val, int *pos_flag,
 int nvals, int *nvalid, double *var_x, double *var_y)
{
    double varsum_x = 0.0;
    double varsum_y = 0.0;
    double mean_x = 0.0;
    double mean_y = 0.0;
    int i;

    *nvalid = 0;
    for (i=0; i < nvals; i++)
    {
        if(pos_flag[i] == 1)
        {
            const double delta_x = (double)x_val[i] - mean_x;
            const double delta_y = (double)y_val[i] - mean_y;

            varsum_x += *nvalid * delta_x * delta_x / (*nvalid + 1.0);
            varsum_y += *nvalid * delta_y * delta_y / (*nvalid + 1.0);
            mean_x   += delta_x / (*nvalid + 1.0);
            mean_y   += delta_y / (*nvalid + 1.0);
            (*nvalid)++;
        }
    }

    /* Compute the bias-corrected standard deviation.
       - With the recurrence relation rounding can likely not cause
       the variance to become negative, but check just to be safe */
    *var_x = varsum_x / (double) (*nvalid - 1);
    *var_y = varsum_y / (double) (*nvalid - 1);
}
