/* $Id: xsh_fit.c,v 1.9 2011-12-02 14:15:28 amodigli Exp $
 *
 * This file is part of the irplib package
 * 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 St, Fifth Floor, Boston, MA  02111-1307  USA
 */

/*
 * $Author: amodigli $
 * $Date: 2011-12-02 14:15:28 $
 * $Revision: 1.9 $
 * $Name: not supported by cvs2svn $
 */

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

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

#include <math.h>
#include <assert.h>

#include "xsh_fit.h"

/*---------------------------------------------------------------------------*/
/**
 * @defgroup xsh_fit   Fitting functions
 * @ingroup xsh_tools
 *
 */
/*---------------------------------------------------------------------------*/
/**@{*/

/*-----------------------------------------------------------------------------
                        Private function prototypes
 -----------------------------------------------------------------------------*/

static double irplib_tools_ipow(double, int);

static cpl_vector * irplib_vector_transform_mean(const cpl_vector *, double *);

static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix *);

static cpl_error_code irplib_matrix_product_transpose(cpl_matrix *,
                                                      const cpl_matrix *,
                                                      const cpl_matrix *);

static cpl_error_code irplib_matrix_solve_chol_transpose(const cpl_matrix *,
                                                         cpl_matrix *);

static void irplib_fit_imagelist_polynomial_double(cpl_imagelist *,
                                                   const cpl_matrix *,
                                                   const cpl_matrix *,
                                                   const cpl_vector *,
                                                   const cpl_imagelist *,
                                                   const cpl_vector *,
                                                   double, int, int,
                                                   cpl_image *);

static void irplib_fit_imagelist_polynomial_float(cpl_imagelist *,
                                                  const cpl_matrix *,
                                                  const cpl_matrix *,
                                                  const cpl_vector *,
                                                  const cpl_imagelist *,
                                                  const cpl_vector *,
                                                  double, int, int,
                                                  cpl_image *);

static void irplib_fit_imagelist_polynomial_int(cpl_imagelist *,
                                                const cpl_matrix *,
                                                const cpl_matrix *,
                                                const cpl_vector *,
                                                const cpl_imagelist *,
                                                const cpl_vector *,
                                                double, int, int,
                                                cpl_image *);

static void irplib_fit_imagelist_residual_double(cpl_image *, int,
                                                 const cpl_vector *,
                                                 const cpl_vector *,
                                                 const cpl_matrix *,
                                                 const cpl_matrix *);

static void irplib_fit_imagelist_residual_float(cpl_image *, int,
                                                const cpl_vector *,
                                                const cpl_vector *,
                                                const cpl_matrix *,
                                                const cpl_matrix *);

static void irplib_fit_imagelist_residual_int(cpl_image *, int,
                                              const cpl_vector *,
                                              const cpl_vector *,
                                              const cpl_matrix *,
                                              const cpl_matrix *);

static void irplib_polynomial_shift_double(double *, int, double);


/*----------------------------------------------------------------------------*/
/**
  @brief  Fit a polynomial to each pixel in a list of images
  @param  x_pos      The vector of positions to fit
  @param  values     The list of images with values to fit
  @param  mindeg     The smallest degree with a non-zero coefficient
  @param  maxdeg     The polynomial degree of the fit, at least mindeg
  @param  is_eqdist  True iff the x_pos values are known to be equidistant
  @param  fiterror   When non-NULL, the error of the fit
  @note   values and x_pos must have the same number of elements.
  @note   The created imagelist must be deallocated with cpl_imagelist_delete().
  @note   x_pos must have at least 1 + (maxdeg - mindeg) distinct values.
  @return The image list of the fitted polynomial coefficients or NULL on error.
  @see cpl_polynomial_fit_1d_create()

  For each pixel, a polynomial representing the relation value = P(x) is
  computed where:
  P(x) = x^{mindeg} * (a_0 + a_1 * x + ... + a_{nc-1} * x^{nc-1}),
  where mindeg >= 0 and maxdeg >= mindeg, and nc is the number of
  polynomial coefficients to determine, nc = 1 + (maxdeg - mindeg).

  The returned image list thus contains nc coefficient images,
  a_0, a_1, ..., a_{nc-1}.

  np is the number of sample points, i.e. the number of elements in x_pos
  and number of images in the image list.

  is_eqdist is ignored if mindeg is nonzero, otherwise
  is_eqdist may to be set to CPL_TRUE if and only if the values in x_pos are
  known a-priori to be equidistant when sorted, eg. (1,2,3,4) and (1,3,2,4),
  but not (1,2,4,6). Setting is_eqdist to CPL_TRUE is faster and eliminates
  certain round-off errors.

  Even though it is not an error, it is hardly useful to use an image of pixel
  type integer for the fitting error. An image of pixel type float should on
  the other hand be sufficient for most fitting errors.

  The call requires the following number of FLOPs, where
  nz is the number of pixels in any one image in the imagelist:

  2 * nz * nc * (nc + np) + np * nc^2 + nc^3/3 + O(nc * (nc + np)).

  If mindeg is zero an additional nz * nc^2 FLOPs are required.

  If fiterror is non-NULL an additional 2 * nz * nc * np FLOPs are required.

  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT if an input const pointer is NULL
  - CPL_ERROR_ILLEGAL_INPUT if mindeg is negative or maxdeg is less than mindeg.
  - CPL_ERROR_INCOMPATIBLE_INPUT if x_pos and values have different lengths,
      or if fiterror is non-NULL with a different size than that of values,
      or if the input images do not all have the same dimensions and pixel type.
  - CPL_ERROR_DATA_NOT_FOUND if x_pos contains less than nc values.
  - CPL_ERROR_SINGULAR_MATRIX if x_pos contains less than nc distinct values.
 */
/*----------------------------------------------------------------------------*/
cpl_imagelist * xsh_fit_imagelist_polynomial(const cpl_vector    * x_pos,
                                                const cpl_imagelist * values,
                                                int                   mindeg,
                                                int                   maxdeg,
                                                cpl_boolean           is_eqdist,
                                                cpl_image           * fiterror)
{

    cpl_imagelist   * self = NULL; /* Avoid (false) uninit warning */
    cpl_matrix      * mv;   /* The transpose of the Vandermonde matrix, V' */
    cpl_matrix      * mh;   /* Upper triangular part of SPD Hankel matrix,
                               H = V' * V */
    cpl_vector      * xhat;

    const cpl_image * value = cpl_imagelist_get_const(values, 0);
    const double    * dx;
    double            xmean;

    cpl_error_code   error;

    /* Number of unknowns to determine */
    const int         nc = 1 + maxdeg - mindeg;
    const int         np = cpl_vector_get_size(x_pos);
    const int         nx = cpl_image_get_size_x(value);
    const int         ny = cpl_image_get_size_y(value);

    const cpl_boolean is_eqzero = is_eqdist && mindeg == 0;

    int               i, j, k;


    cpl_ensure(x_pos  != NULL, CPL_ERROR_NULL_INPUT, NULL);
    cpl_ensure(values != NULL, CPL_ERROR_NULL_INPUT, NULL);

    cpl_ensure(mindeg >= 0,      CPL_ERROR_ILLEGAL_INPUT, NULL);
    cpl_ensure(maxdeg >= mindeg, CPL_ERROR_ILLEGAL_INPUT, NULL);

    assert( nc > 0);

    cpl_ensure(np == cpl_imagelist_get_size(values),
               CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

    cpl_ensure(cpl_imagelist_is_uniform(values)==0,
               CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

    if (fiterror != NULL) {
        cpl_ensure(cpl_image_get_size_x(fiterror) == nx &&
                   cpl_image_get_size_y(fiterror) == ny,
                   CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
    }

    if (mindeg == 0) {
        /* Transform: xhat = x - mean(x) */
        xhat = irplib_vector_transform_mean(x_pos, &xmean);
        assert( xhat != NULL );

        /* Ensure that the center element of xhat is zero */
        if (is_eqdist && (np & 1)) cpl_vector_set(xhat, np>>1, 0.0);

    } else {
        xhat = (cpl_vector*)x_pos; /* xhat is not modified */
        xmean = 0.0;
    }

    dx = cpl_vector_get_data(xhat);

    /* Create matrices */
    mv = cpl_matrix_wrap(nc, np,
                         cpl_malloc(nc * np * sizeof(double)));

    /* Fill Vandermonde matrix */
    for (j=0; j < np; j++) {
        double f_prod = irplib_tools_ipow(dx[j], mindeg);
        cpl_matrix_set(mv,  0, j, f_prod);
        for (k=1; k < nc; k++) {
            f_prod *= dx[j];
            cpl_matrix_set(mv, k, j, f_prod);
        }
    }

    if (xhat != x_pos) cpl_vector_delete(xhat);

    /* Form upper triangular part of the matrix of the normal equations,
       H = V' * V.
       As in cpl_polynomial_fit_1d_create() this could be done in
       O(nc * np) flops, rather than 2 * nc^2 * np, but this is
       negligible for any practical image size and is not done since
       mv still has to be formed in order to block-optimize the formation
       of the right-hand-size */
    mh = irplib_matrix_product_normal_create(mv);

    if (is_eqzero) {

        /* Ensure that the Hankel matrix has zeros on all odd skew diagonals
           - above the (non-skew) main diagonal */

        double * dmh = cpl_matrix_get_data(mh);

        for (i = 0; i < nc; i++) {
            for (j = i + 1; j < nc; j += 2) {
                dmh[nc * i + j] = 0.0;
            }
        }
    }

    error = cpl_matrix_decomp_chol(mh);

    if (!error) {

        cpl_vector * xpow = NULL;

        /* Should not be able to fail at this point */

        /* Allocate nc images to store the results */
        self = cpl_imagelist_new();
        for (i=0; i < nc; i++) {
            cpl_imagelist_set(self, cpl_image_new(nx, ny, CPL_TYPE_DOUBLE),
                              i);
        }

        if (mindeg > 0) {
            const double * d_pos = cpl_vector_get_data_const(x_pos);
            double       * ppow  = cpl_malloc(np * sizeof(double));

            xpow  = cpl_vector_wrap(np, ppow);

            for (i = 0; i < np; i++) {
                ppow[i] = irplib_tools_ipow(d_pos[i], mindeg);
            }
        }

        switch (cpl_image_get_type(value)) {
        case CPL_TYPE_DOUBLE:
            irplib_fit_imagelist_polynomial_double(self, mh, mv, x_pos, values,
                                                   xpow, -xmean, np, nc,
                                                   fiterror);
            break;
        case CPL_TYPE_FLOAT:
            irplib_fit_imagelist_polynomial_float(self, mh, mv, x_pos, values,
                                                  xpow, -xmean, np, nc,
                                                  fiterror);
            break;
        case CPL_TYPE_INT:
            irplib_fit_imagelist_polynomial_int(self, mh, mv, x_pos, values,
                                                xpow, -xmean, np, nc, 
                                                fiterror);
            break;
        default:
            /* It is an error in CPL to reach this point */
            assert( 0 );
        }

        cpl_vector_unwrap(xpow);

    }

    cpl_matrix_delete(mh);
    cpl_matrix_delete(mv);

    /* Propagate the error, if any */
    cpl_ensure(!error, error, NULL);

    return self;

}

/*----------------------------------------------------------------------------*/
/**
  @brief   Compute x to the power of p
  @param   x  The base
  @param   p  The non-negative power
  @return  x to the power of p

  Apart from a possible difference in round-off the result equals pow(x, p).

 */
/*----------------------------------------------------------------------------*/
static double irplib_tools_ipow(double x, int p)
{

    double result;
    double pow2 = x;

    /* Compute the result as a product of powers of 2 of x.
       - this method may produce (slightly) different results than pow(x, p) */

    /* Handle least significant bit in p here in order to avoid an unnecessary
       multiplication of pow2 - which could cause an over- or underflow */
    /* Also, 0^0 is 1, while any other power of 0 is 0 */
    result = p & 1 ? x : 1.0;

    while (p >>= 1) {
        pow2 *= pow2;
        /* Process least significant bit in p */
        if (p & 1) result *= pow2;
    }

    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Fill a matrix with the product of A * B'
  @param    self  The matrix to fill, is or else will be set to size M x N
  @param    ma    The matrix A, of size M x K
  @param    mb    The matrix B, of size N x K
  @return   CPL_ERROR_NONE or the relevant CPL error code on error
  @note     The use of the transpose of B causes a more efficient memory access
  @note     Changing the order of A and B is allowed, it transposes the result
  @see      cpl_matrix_product_create()

*/
/*----------------------------------------------------------------------------*/
static cpl_error_code irplib_matrix_product_transpose(cpl_matrix * self,
                                                      const cpl_matrix * ma,
                                                      const cpl_matrix * mb)
{

    double         sum;

    double       * ds = cpl_matrix_get_data(self);
    const double * d1 = cpl_matrix_get_data_const( ma);
    const double * d2 = cpl_matrix_get_data_const( mb);
    const double * di;

    const int      nr = cpl_matrix_get_nrow(ma);
    const int      nc = cpl_matrix_get_nrow(mb);
    const int      nk = cpl_matrix_get_ncol(mb);
    int            i, j, k;


    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(ma   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(mb   != NULL, CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(cpl_matrix_get_ncol(ma) == nk,
                    CPL_ERROR_INCOMPATIBLE_INPUT);

    if (cpl_matrix_get_nrow(self) != nr || cpl_matrix_get_ncol(self) != nc) 
        cpl_matrix_set_size(self, nr, nc);

    for (i = 0; i < nr; i++, d1 += nk) {
        /* Since ma and mb are addressed in the same manner,
           they can use the same index, k */

        di = d2; /* di points to first entry in i'th row */
        for (j = 0; j < nc; j++, di += nk) {
            sum = 0.0;
            for (k = 0; k < nk; k++) {
                sum += d1[k] * di[k];
            }
            ds[nc * i + j] = sum;
        }
    }

    return CPL_ERROR_NONE;

}

/*----------------------------------------------------------------------------*/
/**
  @brief    Given p and u, modify the polynomial to p(x) := p(x+u)
  @param    coeffs  The polynomial coefficients to be modified in place
  @param    n  The number of coefficients
  @param    u  The shift
  @return   void
  @see      cpl_polynomial_shift_1d
  @note     The function will assert() on NULL input.

  FIXME: Duplicated from cpl_polynomial_shift_double().


*/
/*----------------------------------------------------------------------------*/
static void irplib_polynomial_shift_double(double * coeffs, int n, double u)
{

    int i, j;


    assert( coeffs );

    assert( n > 0 );

    for (j = 0; j < n-1; j++)
        for (i = 1; i < n - j; i++ )
            coeffs[n-1-i] += coeffs[n-i] * u;

}

/*----------------------------------------------------------------------------*/
/**
  @brief    Transform: xhat = x - mean(x)
  @param    x   The vector to be transformed
  @param    pm  On return, *pm is the mean of x
  @return   The created transformed vector
  @note     The function will assert() on NULL input.
  @see cpl_vector_transform_mean

  FIXME: Duplicated from cpl_vector_transform_mean().

 */
/*----------------------------------------------------------------------------*/
static cpl_vector * irplib_vector_transform_mean(const cpl_vector * x,
                                                 double * pm)
{

    cpl_vector * xhat = cpl_vector_duplicate(x);


    assert( xhat != NULL );
    assert( pm   != NULL );

    *pm = cpl_vector_get_mean(xhat);
    cpl_vector_subtract_scalar(xhat, *pm);

    return xhat;

}


/**
 * @brief Create and compute A = B * transpose(B)
 *
 * @param self     M x N Matrix
 * @return Pointer to created M x M product matrix, or @c NULL on error.
 * @note Only the upper triangle of A is computed, while the elements
 *       below the main diagonal have undefined values.
 * @see cpl_matrix_product_create()
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         Any input matrix is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * To destroy the new matrix the function @c cpl_matrix_delete() should
 * be used.

  FIXME: Improved from cpl_matrix_product_normal_create().

 */

static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix * self)
{

    cpl_matrix   * product;
    const double * ai = cpl_matrix_get_data_const(self);
    const double * aj;
    double       * bwrite;
    double         sum;
    const int      m = cpl_matrix_get_nrow(self);
    const int      n = cpl_matrix_get_ncol(self);
    int            i, j, k;


    cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);

    bwrite = (double *) cpl_malloc(m * m * sizeof(double));

    product = cpl_matrix_wrap(m, m, bwrite);

    /* The result at (i,j) is the dot-product of i'th and j'th row */
    for (i = 0; i < m; i++, ai += n, bwrite += m) {
        aj = ai; /* aj points to first entry in j'th row */
        for (j = i; j < m; j++, aj += n) {
            sum = 0.0;
            for (k = 0; k < n; k++) {
                sum += ai[k] * aj[k];
            }
            bwrite[j] = sum;
        }
    }

    return product;

}



/*----------------------------------------------------------------------------*/
/**
   @brief Solve a L*transpose(L)-system with a transposed Right Hand Side
   @param self  N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol()
   @param rhs   M right-hand-sides to be replaced by their solution
   @return CPL_ERROR_NONE on success, or the relevant CPL error code
   @see cpl_matrix_solve_chol()
   @note Only the lower triangle of self is accessed
   @note The use of the transpose of rhe causes a more efficient memory access


   @error
     <table class="ec" align="center">
       <tr>
         <td class="ecl">CPL_ERROR_NULL_INPUT</td>
         <td class="ecr">
           An input pointer is <tt>NULL</tt>.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
         <td class="ecr">
           <i>self</i> is not an n by n matrix.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
         <td class="ecr">
           Selfs number of rows differs from rhs' number of columns.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td>
         <td class="ecr">
           The main diagonal of L contains a zero. This error can only occur
           if the L*transpose(L)-matrix does not come from a successful call to
           cpl_matrix_decomp_chol().
         </td>
       </tr>
     </table>
   @enderror
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
irplib_matrix_solve_chol_transpose(const cpl_matrix * self,
                                   cpl_matrix * rhs)
{

    int n, i, j, k;
    int nrhs;
    const double * aread;
    const double * ai;
    double * bk;
    double sub;

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(rhs  != NULL, CPL_ERROR_NULL_INPUT);

    n = cpl_matrix_get_ncol(self);

    cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(cpl_matrix_get_ncol(rhs)  == n, CPL_ERROR_INCOMPATIBLE_INPUT);

    nrhs = cpl_matrix_get_nrow(rhs);

    aread = cpl_matrix_get_data_const(self);

    /* bk points to first entry in k'th right hand side */
    bk = cpl_matrix_get_data(rhs);

    for (k=0; k < nrhs; k++, bk += n) {

        /* Forward substitution in column k */

        /* Since self and rhs are addressed in the same manner,
           they can use the same index, j */
        ai = aread; /* ai points to first entry in i'th row */
        for (i = 0; i < n; i++, ai += n) {
            sub = 0.0;
            for (j = 0; j < i; j++) {
                sub += ai[j] * bk[j];
            }
            cpl_ensure_code(k > 0 || ai[j] != 0.0, CPL_ERROR_DIVISION_BY_ZERO);
            bk[j] = (bk[j] - sub) / ai[j];
        }

        /* Back substitution in column k */

        for (i = n-1; i >= 0; i--) {
            sub = bk[i];
            for (j = i+1; j < n; j++) {
                sub -= aread[n * j + i] * bk[j];
            }
            bk[i] = sub/aread[n * i + i];
        }
    }

    return CPL_ERROR_NONE;

}


/*---------------------------------------------------------------------------*/
/**
  @brief    Apply a gaussian fit on an image sub window
  @param    im      the input image
  @param    xpos    the x position of the center (1 for the first pixel)
  @param    ypos    the y position of the center (1 for the first pixel)
  @param    size    the window size in pixels
  @param    norm    the norm of the gaussian or NULL
  @param    xcen    the x center of the gaussian or NULL
  @param    ycen    the y center of the gaussian or NULL
  @param    sig_x   the sigma in x of the gaussian or NULL
  @param    sig_y   the sigma in y of the gaussian or NULL
  @param    fwhm_x  the FHHM in x or NULL
  @param    fwhm_y  the FHHM in y or NULL
  @return   the #_cpl_error_code_ or CPL_ERROR_NONE 

  The computed norm, xcen, ycen, sig_x, sig_y coefficients are defining the 
  gaussian:
 
  f(x, y) = (norm/(2*pi*sig_x*sig_y)) * exp(-(x-xcen)^2/(2*sig_x^2)) * 
            exp(-(y-ycen)^2/(2*sig_y^2))

  fwhm_x and fwhm_y are derived from sig_x and sig_y like:
  fwhm = 2 * sqrt(2*ln(2)) * sigma

  Images can be CPL_TYPE_INT, CPL_TYPE_FLOAT or CPL_TYPE_DOUBLE.

  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_NULL_INPUT if the input image pointer is NULL
  - CPL_ERROR_ILLEGAL_INPUT if xpos, ypos or size or illegal
  - CPL_ERROR_TYPE_MISMATCH if the passed image type is not supported

 @note: code extracted from CPL4.0

*/

/*---------------------------------------------------------------------------*/
cpl_error_code 
xsh_image_find_barycenter(
        const cpl_image *   im,
        int                 xpos,
        int                 ypos,
        int                 size,
        double          *   norm,
        double          *   xcen,
        double          *   ycen,
        double          *   sig_x,
        double          *   sig_y,
        double          *   fwhm_x,
        double          *   fwhm_y) 
{
    cpl_image   *   extracted ;
    int             is_rejected;
    int             llx, lly, urx, ury ;
    double          u0, ux, uy, uxx, uyy ;
    double          cenx, ceny;
    const double *  pi ;
    int             pos ;
    double          max_val ;
    int             i, j ;
    int nx=0;
    int ny=0;
    int enx=0;
    int eny=0;

    /* Check entries */
    cpl_ensure_code(im, CPL_ERROR_NULL_INPUT) ;

    nx=cpl_image_get_size_x(im);
    ny=cpl_image_get_size_y(im);

    cpl_ensure_code(xpos>=1 && xpos<=nx, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(ypos>=1 && ypos<=ny, CPL_ERROR_ILLEGAL_INPUT);

    cpl_ensure_code(size>1 && size<nx && size<ny,
            CPL_ERROR_ILLEGAL_INPUT) ;

    /* Extraction zone */
    llx = xpos - (int)(size/2) ;
    lly = ypos - (int)(size/2) ;
    urx = xpos + (int)(size/2) ;
    ury = ypos + (int)(size/2) ;
    if (llx < 1) llx = 1 ;
    if (lly < 1) lly = 1 ;
    if (urx > nx) urx = nx ;
    if (ury > ny) ury = ny ;
    
    /* Extract the image zone to fit */
    extracted = cpl_image_extract(im, llx, lly, urx, ury) ;
    cpl_ensure_code(extracted, CPL_ERROR_ILLEGAL_INPUT) ;
  
    /* Check if there are enough good pixels */
    if (5 * cpl_image_count_rejected(extracted) > 
            cpl_image_get_size_x(extracted) * cpl_image_get_size_y(extracted)) {
        cpl_image_delete(extracted) ;
        cpl_ensure_code(0, CPL_ERROR_ILLEGAL_INPUT) ;
    }
    
    if (cpl_image_get_type(extracted) != CPL_TYPE_DOUBLE) {
        /* Convert the image to double */
        cpl_image * tmp = extracted;
        extracted = cpl_image_cast(tmp, CPL_TYPE_DOUBLE);
        cpl_image_delete(tmp);
        cpl_ensure_code(extracted, CPL_ERROR_TYPE_MISMATCH);
    }
    pi = cpl_image_get_data_double(extracted);


    enx=cpl_image_get_size_x(extracted);
    eny=cpl_image_get_size_y(extracted);
    /* Compute xcen and ycen */
    u0 = ux = uy = 0.0 ;
    for (j=0 ; j<eny ; j++) {
        for (i=0 ; i<enx ; i++) {
            if (!cpl_image_is_rejected(extracted, i+1, j+1)) {
                pos = i + j * enx ;
                u0 += pi[pos] ;
                ux += (i+1) * pi[pos] ;
                uy += (j+1) * pi[pos] ;
            }
        }
    }

    /* cenx = ux/u0 may not be outside 1 and nx and
       ceny = uy/u0 may not be outside 1 and ny */
    if (u0 == 0 || u0 > ux || ux > u0*enx ||
        u0 > uy || uy > u0*eny) {
        cpl_image_delete(extracted) ;
        cpl_ensure_code(0, CPL_ERROR_ILLEGAL_INPUT) ;
    }

    cenx = ux/u0;
    ceny = uy/u0;
    
    /* Compute sig_x and sig_y */
    uxx = uyy = 0.0 ;
    for (j=0 ; j<eny ; j++) {
        for (i=0 ; i<enx ; i++) {
            if (!cpl_image_is_rejected(extracted, i+1, j+1)) {
                pos = i + j * enx ;
                uxx += ((i+1)-cenx) * ((i+1)-cenx) * pi[pos] ;
                uyy += ((j+1)-ceny) * ((j+1)-ceny) * pi[pos] ;
            }
        }
    }
    if (sig_x) *sig_x = sqrt(fabs(uxx/u0)) ;
    if (sig_y) *sig_y = sqrt(fabs(uyy/u0)) ;
    if (fwhm_x) *fwhm_x = 2 * sqrt(2 * log(2.0)) * sqrt(fabs(uxx/u0)) ;
    if (fwhm_y) *fwhm_y = 2 * sqrt(2 * log(2.0)) * sqrt(fabs(uyy/u0)) ;

    /* Compute norm */
    max_val = cpl_image_get(extracted, (int)cenx, (int)ceny,
                            &is_rejected);
    cpl_ensure_code(is_rejected >= 0, cpl_error_get_code());

    if (is_rejected) {
        cpl_errorstate pstate = cpl_errorstate_get();
        max_val = cpl_image_get_mean_window(extracted, (int)cenx,
                (int)ceny,  (int)(cenx+1), (int)(ceny+1)) ;
        cpl_ensure_code(cpl_errorstate_is_equal(pstate), cpl_error_get_code());
    }

    cpl_image_delete(extracted) ;

    if (norm) *norm = max_val*2*CX_PI*sqrt(fabs(uxx/u0))*sqrt(fabs(uyy/u0)) ; 
    
    /* Shift xcen and ycen to coordinates in the input big image */
    if (xcen) *xcen = cenx + llx - 1 ;
    if (ycen) *ycen = ceny + lly - 1 ;
    

    return CPL_ERROR_NONE ;
}


/* Define the C-type dependent functions */

/* These two macros are needed for support of the different pixel types */

#define CONCAT(a,b) a ## _ ## b
#define CONCAT2X(a,b) CONCAT(a,b)

#define CPL_TYPE double
#include "xsh_fit_body.h"
#undef CPL_TYPE

#define CPL_TYPE float
#include "xsh_fit_body.h"
#undef CPL_TYPE

#define CPL_TYPE int
#include "xsh_fit_body.h"
#undef CPL_TYPE

/**@}*/