/* -*- Mode: C; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim:set sw=2 sts=2 et cin: */
/*
 * This file is part of the MUSE Instrument Pipeline
 * Copyright (C) 2015 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.
 */

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

/*---------------------------------------------------------------------------*
 *                             Includes                                      *
 *---------------------------------------------------------------------------*/
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <math.h>
#include <cpl.h>
#include <muse.h>
#include "muse_exp_align_z.h"


typedef struct muse_indexpair muse_indexpair;

struct muse_indexpair {
    cpl_size first;
    cpl_size second;
};

typedef void (*muse_free_func)(void *);


/* Conversion factor degree to arcsecond */
static const double deg2as = 3600.;


/*---------------------------------------------------------------------------*
 *                             Functions code                                *
 *---------------------------------------------------------------------------*/

static void
muse_vfree(void **array, cpl_size size, muse_free_func deallocator)
{
  if (array) {
    cpl_size idx;
    for (idx = 0; idx < size; ++idx) {
      if (deallocator) {
        deallocator(array[idx]);
      }
    }
    cpl_free(array);
  }
  return;
}

static int
_muse_condition_less_than(double aValue1, double aValue2)
{
  return (aValue1 < aValue2) ? TRUE : FALSE;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Replace NaN values in a MUSE image.
  @param    aImage  the MUSE image to be cleaned
  @param    aValue  the numerical value to use for replacing NaN values
  @return   The return value is CPL_ERROR_NONE on success, and an
            appropriate error code otherwise.

  @error{set CPL_ERROR_NULL_INPUT\, return CPL_ERROR_NULL_INPUT, aImage is NULL}
  @error{set CPL_ERROR_TYPE_MISMATCH\, return CPL_ERROR_TYPE_MISMATCH,
         aImage does not have data section}
  @error{set CPL_ERROR_ILLEGAL_INPUT\, return CPL_ERROR_ILLEGAL_INPUT,
         aImage does not have data section}
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
muse_utils_replace_nan(muse_image *aImage, float aValue)
{
  cpl_ensure(aImage, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT);
  cpl_ensure(aImage->data, CPL_ERROR_ILLEGAL_INPUT, CPL_ERROR_ILLEGAL_INPUT);

  cpl_size npixel = cpl_image_get_size_x(aImage->data) *
      cpl_image_get_size_y(aImage->data);

  float *data = cpl_image_get_data_float(aImage->data);
  if (cpl_error_get_code() == CPL_ERROR_TYPE_MISMATCH) {
    cpl_error_set_message(__func__, CPL_ERROR_TYPE_MISMATCH,
                          "MUSE image data buffer has invalid type!");
    return CPL_ERROR_TYPE_MISMATCH;
  }

  cpl_size ipixel;
  for (ipixel = 0; ipixel < npixel; ++ipixel) {
    if (isnan(data[ipixel])) {
      data[ipixel] = aValue;
    }
  }

  if (aImage->stat) {
    npixel = cpl_image_get_size_x(aImage->stat) *
        cpl_image_get_size_y(aImage->stat);

    data = cpl_image_get_data_float(aImage->stat);
    if (cpl_error_get_code() == CPL_ERROR_TYPE_MISMATCH) {
      cpl_error_set_message(__func__, CPL_ERROR_TYPE_MISMATCH,
                            "MUSE image data buffer has invalid type!");
      return CPL_ERROR_TYPE_MISMATCH;
    }

    for (ipixel = 0; ipixel < npixel; ++ipixel) {
      if (isnan(data[ipixel])) {
        data[ipixel] = aValue;
      }
    }
  }

  return CPL_ERROR_NONE;
}

/* XXX: Moved to muse_utils, kept here disabled until final place is found */
#if 0
/*----------------------------------------------------------------------------*/
/**
  @brief    Load a FOV image into a MUSE image.
  @param    aFilename  name of the file to load
  @return   a muse_image or NULL on error

  Load the DATA extension containing the FOV image data from the input
  file. Bad pixels may be encoded as NaN values in the image data directly,
  or, if a DQ extension is found in the input file, it is used to replace
  the bad pixels in the FOV image data with NaN values.

  Alternatively, if the DATA extension is an extension with higher-dimensional
  data or no image content (NAXIS != 2), load the first 2D image extension. In
  that case, search for the related STAT and DQ extension by prefixing them with
  the filter name.
 */
/*----------------------------------------------------------------------------*/
static muse_image *
muse_fov_load(const char *aFilename)
{
  muse_image *image = muse_image_new();

  image->header = cpl_propertylist_load(aFilename, 0);
  if (!image->header) {
    cpl_error_set_message(__func__, cpl_error_get_code(), "Loading primary FITS "
                          "header of \"%s\" did not succeed", aFilename);
    muse_image_delete(image);
    return NULL;
  }

  /* assuming an IMAGE_FOV input, start by searching for the DATA extension */
  int extension = cpl_fits_find_extension(aFilename, EXTNAME_DATA);
  cpl_propertylist *hdata = cpl_propertylist_load(aFilename, extension);
  while (muse_pfits_get_naxis(hdata, 0) != 2) {
    /* Not an image, this means we were given a cube, possibly *
     * with image extensions; search for the first of those.   */
    cpl_msg_debug(__func__, "Skipping extension %d [%s]", extension,
                  muse_pfits_get_extname(hdata));
    cpl_propertylist_delete(hdata);
    extension++;
    hdata = cpl_propertylist_load(aFilename, extension);
  } /* while */
  cpl_msg_debug(__func__, "Taking extension %d [%s]", extension,
                muse_pfits_get_extname(hdata));
  /* keep the extension name, to check if we loaded *
   * from an IMAGE_FOV or from a cube extension     */
  char *extname = cpl_strdup(muse_pfits_get_extname(hdata));
  image->data = cpl_image_load(aFilename, CPL_TYPE_FLOAT, 0, extension);
  if (!image->data) {
    cpl_error_set_message(__func__, MUSE_ERROR_READ_DATA, "Could not load "
                          "extension %s from \"%s\"", extname, aFilename);
    muse_image_delete(image);
    cpl_free(extname);
    return NULL;
  }

  if (cpl_propertylist_has(hdata, "BUNIT")) {
    cpl_propertylist_append_string(image->header, "BUNIT",
                                   muse_pfits_get_bunit(hdata));
    cpl_propertylist_set_comment(image->header, "BUNIT",
                                 cpl_propertylist_get_comment(hdata, "BUNIT"));
  } else {
    cpl_msg_warning(__func__, "No BUNIT given in extension %d [%s] of \"%s\"!",
                    extension, extname, aFilename);
  }

  if (!cpl_propertylist_has(hdata, "CUNIT1") ||
      !cpl_propertylist_has(hdata, "CUNIT2")) {
    cpl_msg_warning(__func__, "No WCS found in extension %d [%s] of \"%s\"!",
                    extension, extname, aFilename);
  }

  /* Merge WCS keywords and ESO hierarchical keywords from the data   *
   * extension into the image header                                  */
  cpl_propertylist_erase_regexp(hdata, "(^ESO |" MUSE_WCS_KEYS ")", 1);
  cpl_propertylist_append(image->header, hdata);
  cpl_propertylist_delete(hdata);

  /* Load STAT extension if it is found and contains data, ignore it *
   * otherwise */
  if (extname && !strncmp(extname, EXTNAME_DATA, strlen(EXTNAME_DATA)+1)) {
    /* if this is an IMAGE_FOV, then a STAT extension might be there */
    extension = cpl_fits_find_extension(aFilename, EXTNAME_STAT);
  } else {
    /* otherwise we might have an extension <filtername>_STAT */
    char *statname = cpl_sprintf("%s_STAT", extname);
    extension = cpl_fits_find_extension(aFilename, statname);
    cpl_free(statname);
  }
  if (extension > 0) {
    cpl_errorstate status = cpl_errorstate_get();
    image->stat = cpl_image_load(aFilename, CPL_TYPE_INT, 0, extension);
    if (!cpl_errorstate_is_equal(status)) {
      if (cpl_error_get_code() == CPL_ERROR_DATA_NOT_FOUND) {
        cpl_errorstate_set(status);
        cpl_msg_debug(__func__, "Ignoring empty extension %s in \"%s\"",
                      EXTNAME_STAT, aFilename);
      } else {
        cpl_error_set_message(__func__, MUSE_ERROR_READ_STAT, "Could not load "
                            "extension %s from \"%s\"", EXTNAME_STAT, aFilename);
        muse_image_delete(image);
        cpl_free(extname);
        return NULL;
      }
    }
  }

  /* If present load the DQ extension and encode flagged pixels in the *
   * image data (and possibly the statistics as NaNs.                  */
  if (extname && !strncmp(extname, EXTNAME_DATA, strlen(EXTNAME_DATA)+1)) {
    /* if this is an IMAGE_FOV, then a DQ extension might be there */
    extension = cpl_fits_find_extension(aFilename, EXTNAME_DQ);
  } else {
    /* otherwise we might have an extension <filtername>_DQ */
    char *dqname = cpl_sprintf("%s_DQ", extname);
    extension = cpl_fits_find_extension(aFilename, dqname);
    cpl_free(dqname);
  }
  if (extension > 0) {
    cpl_errorstate status = cpl_errorstate_get();
    image->dq = cpl_image_load(aFilename, CPL_TYPE_INT, 0, extension);
    if (!cpl_errorstate_is_equal(status)) {
      cpl_errorstate_set(status);
      cpl_error_set_message(__func__, MUSE_ERROR_READ_DQ, "Could not load "
                            "extension %s from \"%s\"", EXTNAME_DQ, aFilename);
      muse_image_delete(image);
      cpl_free(extname);
      return NULL;
    }
    muse_image_dq_to_nan(image);
  }

  cpl_free(extname);
  return image;
}
#endif

/* XXX: Maybe there is a better name for this function */
/*----------------------------------------------------------------------------*/
/**
  @brief    Load all FOV images into a MUSE image list.
  @param    aProcessing  the processing structure
  @return   a muse_imagelist or NULL on error
 */
/*----------------------------------------------------------------------------*/
static muse_imagelist *
muse_processing_fov_load_all(muse_processing *aProcessing)
{
  cpl_ensure(aProcessing, CPL_ERROR_NULL_INPUT, NULL);
  cpl_size nframes = cpl_frameset_get_size(aProcessing->inframes);
  cpl_ensure(nframes, CPL_ERROR_DATA_NOT_FOUND, NULL);

  muse_imagelist *fovimages = muse_imagelist_new();

  cpl_size iexposure = 0;
  cpl_size iframe;
  for (iframe = 0; iframe < nframes; ++iframe) {
    const cpl_frame *frame = cpl_frameset_get_position(aProcessing->inframes,
                                                       iframe);
    const char *tag = cpl_frame_get_tag(frame);

    if (muse_processing_check_intags(aProcessing, tag, strlen(tag))) {
      const char *filename = cpl_frame_get_filename(frame);

      cpl_msg_debug(__func__, "Loading FOV image '%s' as exposure %"
                    CPL_SIZE_FORMAT, filename, iexposure + 1);

      muse_image *image = muse_fov_load(filename);
      if (!image) {
        cpl_msg_error(__func__, "Could not load FOV image '%s'", filename);
        muse_imagelist_delete(fovimages);
        return NULL;
      }

      muse_imagelist_set(fovimages, image, iexposure);
      muse_processing_append_used(aProcessing, (cpl_frame *)frame,
                                  CPL_FRAME_GROUP_RAW, 1);
      ++iexposure;
    }
  }
  return fovimages;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Validate the exposure alignment task parameters.
  @param    aParams  object with parameters to validate
  @return   CPL_TRUE if everything is ok, CPL_FALSE if not.

 */
/*----------------------------------------------------------------------------*/
static cpl_boolean
muse_align_check_detection_params(muse_exp_align_params_t *aParams)
{
  if (aParams->threshold <= 0.) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "Detection threshold must be greater than zero!");
    return CPL_FALSE;
  }
  if (aParams->fwhm < 0.5) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "FWHM must be greater than 0.5 pixels!");
    return CPL_FALSE;
  }
  if (aParams->srcmax <= aParams->srcmin) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "Maximum number of sources must be grater than the "
                          "minimum number of sources!");
    return CPL_FALSE;
  }
  if (aParams->roundmax < aParams->roundmin) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "Upper limit of the point-source roundness must be "
                          "greater than the lower limit!");
    return CPL_FALSE;
  }
  if (aParams->sharpmin <= 0.) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "Low limit of the point-source sharpness must be "
                          "greater than zero!");
    return CPL_FALSE;
  }
  if (aParams->sharpmax < aParams->sharpmin) {
    cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                          "Upper limit of the point-source sharpness must be "
                          "greater than the lower limit!");
    return CPL_FALSE;
  }
  return CPL_TRUE;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Parse a comma separated list of values into an array.
  @param    aValuelist  the string encoded list of values.
  @return   the array object containing the parsed value on success, and NULL
            otherwise.

 */
/*----------------------------------------------------------------------------*/
static cpl_array *
muse_align_parse_valuelist(const char *aValuelist)
{
  cpl_ensure(aValuelist, CPL_ERROR_NULL_INPUT, NULL);
  if (strlen(aValuelist) == 0) {
    cpl_error_set_message(__func__, CPL_ERROR_DATA_NOT_FOUND,
                          "List of values is empty!");
    return NULL;
  }

  cpl_array *strings = muse_cplarray_new_from_delimited_string(aValuelist, ",");
  cpl_size nvalues = cpl_array_get_size(strings);

  cpl_array *values  = cpl_array_new(nvalues, CPL_TYPE_DOUBLE);
  cpl_size ivalue;
  for (ivalue = 0; ivalue < nvalues; ++ivalue) {
    const char *svalue = cpl_array_get_string(strings, ivalue);
    if ((strncasecmp(svalue, "inf", 3) == 0) ||
        (strncasecmp(svalue, "nan", 3) == 0)) {
      cpl_error_set_message(__func__, CPL_ERROR_ILLEGAL_INPUT,
                            "Illegal value \"%s\" encountered!", svalue);
      cpl_array_delete(values);
      cpl_array_delete(strings);
      return NULL;
    }
    else {
      char *last = NULL;
      double value = strtod(svalue, &last);
      if( (errno == ERANGE) || ((value == 0.) && (last == svalue))) {
        cpl_error_set_message(__func__, CPL_ERROR_TYPE_MISMATCH,
                              "Could not convert \"%s\" to a decimal number!", svalue);
        cpl_array_delete(values);
        cpl_array_delete(strings);
        return NULL;
      }
      cpl_array_set_double(values, ivalue, value);
    }
  }
  cpl_array_delete(strings);
  return values;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Compute celestial coordinates from pixel positions.
  @param    aTable      the table containing the pixel positions of the sources.
  @param    aWCSHeader  the property list containing WCS definition.
  @return   0 if everything is ok, -1 something went wrong

 */
/*----------------------------------------------------------------------------*/
static int
muse_align_celestial_from_pixel(cpl_table *aTable,
                                    cpl_propertylist *aWCSHeader)
{
  muse_wcs *wcs = muse_wcs_new(aWCSHeader);

  const char *unit1 = muse_pfits_get_cunit(aWCSHeader, 1);
  const char *unit2 = muse_pfits_get_cunit(aWCSHeader, 2);

  if (unit1 && unit2) {
    if ((strncmp(unit1, unit2, 4) == 0) && (strncmp(unit1, "deg", 3) == 0)) {
      wcs->iscelsph = CPL_TRUE;
    }
  } else {
    return -1;
  }

  cpl_errorstate status = cpl_errorstate_get();

  cpl_table_new_column(aTable, "RA", CPL_TYPE_DOUBLE);
  cpl_table_new_column(aTable, "DEC", CPL_TYPE_DOUBLE);

  cpl_size irow;
  for (irow = 0; irow < cpl_table_get_nrow(aTable); ++irow) {
    double x = cpl_table_get_double(aTable, "X", irow, NULL);
    double y = cpl_table_get_double(aTable, "Y", irow, NULL);
    double ra;
    double dec;
    muse_wcs_celestial_from_pixel_fast(wcs, x, y, &ra, &dec);

    cpl_table_set_double(aTable, "RA", irow, ra);
    cpl_table_set_double(aTable, "DEC", irow, dec);
  }
  cpl_free(wcs);

  if (!cpl_errorstate_is_equal(status)) {
    return -1;
  }
  return 0;
}

/* XXX: Keep the conditional until the SVD code has been fully verified. *
 *      Once this is done, completely replace the old code with the SVD  *
 *      version.                                                         */
#define USE_SVD_SOLVER
#ifdef USE_SVD_SOLVER

#if CPL_VERSION_CODE < 459008
#define cpl_matrix_solve_svd muse_cplmatrix_solve_svd

/*
 * The following implementation of one-sided Jacobi orthogonalization
 * to compute the SVD of a MxN matrix, with M >= N is based on the
 * implementation found in the GNU Scientific Library (GSL).
 */

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Compute the singular value decomposition of a real matrix using
            Jacobi plane rotations.
  @param U  The M x N matrix to be factorized.
  @param S  A vector of N elements for storing the singular values.
  @param V  An N x N square matrix for storing the right singular vectors.
  @return   @c CPL_ERROR_NONE if the factorization was successful and an error
            code otherwise.
 
  @error{set CPL_ERROR_NULL_INPUT\, return CPL_ERROR_NULL_INPUT,
         One or more arguments are NULL}
  @error{set CPL_ERROR_ILLEGAL_INPUT\, return CPL_ERROR_ILLEGAL_INPUT,
         the matrix U has less rows than columns (M < N), or the matrix V
         is not a square matrix}
  @error{set CPL_ERROR_INCOMPATIBLE_INPUT\, return CPL_ERROR_INCOMPATIBLE_INPUT,
         the number of rows of the matrix V does not match the number
         of columns of U (N), or the size of the vector S does not match
         the number of rows of U (M)}
 
  The function computes singular value decomposition (SVD) of a real M x N
  matrix @em U, where M >= N. It uses a one-sided Jacobi orthogonalization
  as described in James Demmel, Kresimir Veselic, "Jacobi's Method is more
  accurate than QR", Lapack Working Note 15 (LAWN15), October 1989,
  Algorithm 4.1 which is available at @em netlib.org.

  The input matrix @em U is overwritten during the process and contains
  in the end the left singular vectors of @em U. The matrix @em V will be
  updated with the right singular vectors of @em U, and the vector @em S
  will contain the singular values. For efficiency reasons the singular
  values are not written to the diagonal elements of a N x N diagonal matrix,
  but an N-elements vector.
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
muse_cplmatrix_decomp_sv_jacobi(cpl_matrix *U, cpl_vector *S, cpl_matrix *V)
{
  cpl_ensure_code((U != NULL) && (V != NULL) && (S != NULL),
                  CPL_ERROR_NULL_INPUT);
  cpl_ensure_code(cpl_matrix_get_nrow(U) >= cpl_matrix_get_ncol(U),
                  CPL_ERROR_ILLEGAL_INPUT);
  cpl_ensure_code(cpl_matrix_get_nrow(V) == cpl_matrix_get_ncol(V),
                  CPL_ERROR_ILLEGAL_INPUT);
  cpl_ensure_code(cpl_matrix_get_nrow(V) == cpl_matrix_get_ncol(U),
                  CPL_ERROR_INCOMPATIBLE_INPUT);
  cpl_ensure_code(cpl_vector_get_size(S) == cpl_matrix_get_ncol(U),
                  CPL_ERROR_INCOMPATIBLE_INPUT);

  const cpl_size m = cpl_matrix_get_nrow(U);
  const cpl_size n = cpl_matrix_get_ncol(U);

  /* Initialize the rotation counter and the sweep counter. *
   * Use a minimum number of 12 sweeps.                     */
  cpl_size count = 1;
  cpl_size sweep = 0;
  cpl_size sweepmax = CPL_MAX(5 * n, 12);

  double tolerance = 10. * m * DBL_EPSILON;

  /* Fill V with the identity matrix. */
  cpl_matrix_fill(V, 0.);
  cpl_matrix_fill_diagonal(V, 1., 0);

  /* Store the column error estimates in S, for use during the *
   * orthogonalization.                                        */

  cpl_size i;
  cpl_size j;
  cpl_vector *cv1 = cpl_vector_new(m);
  double *const _cv1 = cpl_vector_get_data(cv1);

  for (j = 0; j < n; ++j) {
    for (i = 0; i < m; ++i) {
      _cv1[i] = cpl_matrix_get(U, i, j);
    }

    double s = sqrt(cpl_vector_product(cv1, cv1));
    cpl_vector_set(S, j, DBL_EPSILON * s);
  }

  /* Orthogonalization of U by plane rotations. */
  cpl_vector *cv2 = cpl_vector_new(m);
  double *const _cv2 = cpl_vector_get_data(cv2);

  while ((count > 0) && (sweep <= sweepmax)) {

    /* Initialize the rotation counter. */
    count = n * (n - 1) / 2;

    for (j = 0; j < n - 1; ++j) {
      cpl_size k;
      for (k = j + 1; k < n; ++k) {
        for (i = 0; i < m; ++i) {
          _cv1[i] = cpl_matrix_get(U, i, j);
        }
        for (i = 0; i < m; ++i) {
          _cv2[i] = cpl_matrix_get(U, i, k);
        }

        /* Compute the (j, k) submatrix elements of Ut*U. The *
         * non-diagonal element c is scaled by 2.             */
        double a = sqrt(cpl_vector_product(cv1, cv1));
        double b = sqrt(cpl_vector_product(cv2, cv2));
        double c = 2. * cpl_vector_product(cv1, cv2);

        /* Test whether columns j, k are orthogonal, or dominant errors. */
        double abserr_a = cpl_vector_get(S, j);
        double abserr_b = cpl_vector_get(S, k);
        cpl_boolean orthogonal = (fabs(c) <= tolerance * a * b);
        cpl_boolean sorted     = (a >= b);
        cpl_boolean noisy_a    = (a < abserr_a);
        cpl_boolean noisy_b    = (b < abserr_b);

        if (sorted && (orthogonal || noisy_a || noisy_b)) {
          --count;
          continue;
        }

        /* Compute the Jacobi rotation which diagonalizes the (j, k) *
         * submatrix.                                                */
        double q = a * a - b * b;
        double v = sqrt(c * c + q * q);
        double cs;  /* cos(theta) */
        double sn;  /* sin(theta) */

        if (v == 0. || !sorted) {
          cs = 0.;
          sn = 1.;
        } else {
          cs = sqrt((v + q) / (2. * v));
          sn = c / (2. * v * cs);
        }

        /* Update the columns j and k of U and V */
        for (i = 0; i < m; ++i) {
          const double u_ij = cpl_matrix_get(U, i, j);
          const double u_ik = cpl_matrix_get(U, i, k);
          cpl_matrix_set(U, i, j,  u_ij * cs + u_ik * sn);
          cpl_matrix_set(U, i, k, -u_ij * sn + u_ik * cs);
        }

        for (i = 0; i < n; ++i) {
          const double v_ij = cpl_matrix_get(V, i, j);
          const double v_ik = cpl_matrix_get(V, i, k);
          cpl_matrix_set(V, i, j,  v_ij * cs + v_ik * sn);
          cpl_matrix_set(V, i, k, -v_ij * sn + v_ik * cs);
        }

        cpl_vector_set(S, j, fabs(cs) * abserr_a +
            fabs(sn) * abserr_b);
        cpl_vector_set(S, k, fabs(sn) * abserr_a +
            fabs(cs) * abserr_b);
      }
    }
    ++sweep;

  }
  cpl_vector_delete(cv2);

  /* Compute singular values. */
  double prev_norm = -1.;
  for (j = 0; j < n; ++j) {
    for (i = 0; i < m; ++i) {
      _cv1[i] = cpl_matrix_get(U, i, j);
    }

    double norm = sqrt(cpl_vector_product(cv1, cv1));

    /* Determine if the singular value is zero, according to the  *
     * criteria used in the main loop above (i.e. comparison with *
     * norm of previous column).                                  */
    if ((norm == 0.) || (prev_norm == 0.) ||
        ((j > 0) && (norm <= tolerance * prev_norm))) {
      cpl_vector_set(S, j, 0.);          /* singular */
      cpl_matrix_fill_column(U, 0., j);  /* annihilate column */
      prev_norm = 0.;
    } else {
      cpl_vector_set(S, j, norm);     /* non-singular */

      /* Normalize the column vector of U */
      for (i = 0; i < m; ++i) {
        cpl_matrix_set(U, i, j, _cv1[i] / norm);
      }
      prev_norm = norm;
    }
  }
  cpl_vector_delete(cv1);

  if (count > 0) {
    return CPL_ERROR_CONTINUE;
  }
  return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief  Solve a linear system in a least square sense using an
          SVD factorization.
  @param  coeff Linear system coefficients
  @param  rhs   Right hand side of the linear system
  @return the solution vector or NULL on error
 */
/*----------------------------------------------------------------------------*/
static cpl_matrix *
muse_cplmatrix_solve_svd(const cpl_matrix *coeff, const cpl_matrix *rhs)
{
  cpl_ensure((coeff != NULL) && (rhs != NULL), CPL_ERROR_NULL_INPUT, NULL);
  cpl_ensure(cpl_matrix_get_nrow(coeff) == cpl_matrix_get_nrow(rhs),
             CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
  cpl_ensure(cpl_matrix_get_ncol(rhs) == 1, CPL_ERROR_ILLEGAL_INPUT, NULL);

  /* Compute the SVD factorization of the coefficient matrix */
  cpl_matrix *U = cpl_matrix_duplicate(coeff);
  cpl_size nc = cpl_matrix_get_ncol(U);

  cpl_matrix *V = cpl_matrix_new(nc, nc);
  cpl_vector *S = cpl_vector_new(nc);

  cpl_error_code status = muse_cplmatrix_decomp_sv_jacobi(U, S, V);
  if (status != CPL_ERROR_NONE) {
    cpl_vector_delete(S);
    cpl_matrix_delete(V);
    cpl_matrix_delete(U);
    cpl_error_set_where(cpl_func);
    return NULL;
  }

  /* Solve the linear system */
  cpl_matrix *Ut = cpl_matrix_transpose_create(U);
  cpl_matrix *w  = cpl_matrix_product_create(Ut, rhs);
  cpl_matrix_delete(Ut);

  const double *_s = cpl_vector_get_data_const(S);
  double *_w = cpl_matrix_get_data(w);
  cpl_size ic;

  for (ic = 0; ic < nc; ++ic) {
    double s = _s[ic];
    if (s != 0.) {
      s = 1. / s;
    }
    _w[ic] *= s;
  }

  cpl_matrix *solution = cpl_matrix_product_create(V, w);
  cpl_matrix_delete(w);
  cpl_vector_delete(S);
  cpl_matrix_delete(V);
  cpl_matrix_delete(U);
  return solution;
}

#endif

#endif

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief Compute the least-squares solution of a linear matrix equation.
  @param  aMatrix  the left-hand side MxN coefficient matrix.
  @param  aVector  the right-hand side MxK ordinate matrix.
  @return a newly allocated NxK least-squares solution matrix.

 */
/*----------------------------------------------------------------------------*/
static cpl_matrix *
muse_cplmatrix_solve_least_square(const cpl_matrix *aMatrix1,
                                  const cpl_matrix *aMatrix2)
{
  cpl_ensure(aMatrix1 && aMatrix2, CPL_ERROR_NULL_INPUT, NULL);

  cpl_size nc = cpl_matrix_get_ncol(aMatrix1);
  cpl_size nr = cpl_matrix_get_nrow(aMatrix1);
  cpl_ensure(cpl_matrix_get_nrow(aMatrix2) == nr,
             CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

  cpl_matrix *result = NULL;
  cpl_errorstate status = cpl_errorstate_get();

#ifndef USE_SVD_SOLVER
  if (nr == nc) {
    result = cpl_matrix_solve(aMatrix1, aMatrix2);
  } else {
    /* Handle over- and under-determined systems */
    if (nr > nc) {
      result = cpl_matrix_solve_normal(aMatrix1, aMatrix2);
    } else {
      cpl_matrix *mt = cpl_matrix_transpose_create(aMatrix1);
      cpl_matrix *mgram = cpl_matrix_product_create(aMatrix1, mt);
      cpl_matrix *mw = cpl_matrix_solve(mgram, aMatrix2);

      result = cpl_matrix_product_create(mt, mw);
      cpl_matrix_delete(mw);
      cpl_matrix_delete(mgram);
      cpl_matrix_delete(mt);
    }
  }
#else
  if (nr >= nc) {
    result = cpl_matrix_solve_svd(aMatrix1, aMatrix2);
  } else {
    cpl_matrix *mt = cpl_matrix_transpose_create(aMatrix1);
    cpl_matrix *mgram = cpl_matrix_product_create(aMatrix1, mt);
    cpl_matrix *mw = cpl_matrix_solve(mgram, aMatrix2);

    result = cpl_matrix_product_create(mt, mw);
    cpl_matrix_delete(mw);
    cpl_matrix_delete(mgram);
    cpl_matrix_delete(mt);
  }
#endif

  if (status != cpl_errorstate_get()) {
    cpl_matrix_delete(result);
    result = NULL;
  }
  return result;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief Compute the cosine of matrix elements.
  @param aMatrix  the matrix.
  @return CPL_ERROR_NONE on success or an appropriate error code on failure

  @error{set CPL_ERROR_NULL_INPUT\, return CPL_ERROR_NULL_INPUT, aMatrix is NULL}

 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
muse_cplmatrix_cosine(cpl_matrix *aMatrix)
{
  cpl_ensure(aMatrix, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT);

  cpl_size size = cpl_matrix_get_nrow(aMatrix) * cpl_matrix_get_ncol(aMatrix);
  double *mdata = cpl_matrix_get_data(aMatrix);

  cpl_size i;
  for (i = 0; i < size; ++i) {
    mdata[i] = cos(mdata[i]);
  }
  return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Compute distance between object positions from position differences.
  @param    aDeltaRA   Matrix of right ascension offsets.
  @param    aDeltaDEC  Matrix of declination offsets.
  @param    aMeanDEC   Matrix of mean declination.
  @param    aOffsetRA  Optional, additional right ascension offset.
  @param    aOffsetDEC Optional, additional declination offset.
  @return   A matrix containing the computed distances, or NULL.

  The function computes for each matrix element the angular distance from
  the given position and differences in right ascension and declination.
  Extra shift parameters may be given. They are completely ignored if they are
  set to NULL, otherwise they are applied as extra offsets in the computation.
 */
/*----------------------------------------------------------------------------*/
static cpl_matrix *
muse_align_compute_distances(const cpl_matrix *aDeltaRA, const cpl_matrix *aDeltaDEC,
                             const cpl_matrix *aMeanDEC, const double *aOffsetRA,
                             const double *aOffsetDEC)
{

  cpl_errorstate status = cpl_errorstate_get();

  /* XXX: Cannot use cpl_matrix_power() here, since it chokes on negative *
   *      elements, however it should not if the exponent is integer!     */

  cpl_matrix *dDEC;
  if (aOffsetDEC) {
    cpl_matrix *tmp = cpl_matrix_duplicate(aDeltaDEC);
    cpl_matrix_subtract_scalar(tmp, *aOffsetDEC);

    dDEC = muse_cplmatrix_multiply_create(tmp, tmp);
    cpl_matrix_delete(tmp);
  } else {
    dDEC = muse_cplmatrix_multiply_create(aDeltaDEC, aDeltaDEC);
  }

  cpl_matrix *dec = cpl_matrix_duplicate(aMeanDEC);
  muse_cplmatrix_cosine(dec);

  cpl_matrix *dRA;
  if (aOffsetRA) {
    dRA = cpl_matrix_duplicate(aDeltaRA);
    cpl_matrix_subtract_scalar(dRA, *aOffsetRA);
    cpl_matrix_multiply(dRA, dec);
  } else {
    dRA = muse_cplmatrix_multiply_create(aDeltaRA, dec);
  }
  cpl_matrix_delete(dec);

  cpl_matrix *distance = muse_cplmatrix_multiply_create(dRA, dRA);
  cpl_matrix_delete(dRA);

  cpl_matrix_add(distance, dDEC);
  cpl_matrix_power(distance, 0.5);
  cpl_matrix_delete(dDEC);

  if (status != cpl_errorstate_get()) {
    cpl_matrix_delete(distance);
    return NULL;
  }

  return distance;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Estimate field offsets from a set of object position offsets
  @param    aOffsetRA  Estimated right ascension offset of the fields.
  @param    aOffsetDEC Estimated declination offset of the fields.
  @param    aWeight    Weight of the estimated offset (number of occurrences
                       of the estimates offset).
  @param    aDeltaRA   Right ascension offset matrix of pairs of detections
  @param    aDeltaDEC  Declination offset matrix of pairs of detections
  @param    aMeanDEC   Mean declination of pairs of detections
  @param    aRadius    Maximum allowed distance between candidate sources
  @param    aNbins     Number of offset histogram bins used to estimate the
                       field offset.
  @return   the number of matches or zero on failure to find matches

  Uses a histogram, extending from -aRadius to aRadius on both axes, to
  estimate the field offset as the most likely offset (histogram peak) of
  object positions.
 */
/*----------------------------------------------------------------------------*/
static cpl_size
muse_align_estimate_offsets(double *aOffsetRA, double *aOffsetDEC, double *aWeight,
                            const cpl_matrix *aDeltaRA, const cpl_matrix *aDeltaDEC,
                            const cpl_matrix *aMeanDEC, double aRadius, int aNbins)
{
  cpl_ensure((aOffsetRA && aOffsetDEC) && aWeight, CPL_ERROR_NULL_INPUT, 0);
  cpl_ensure((aDeltaRA && aDeltaDEC) && aMeanDEC, CPL_ERROR_NULL_INPUT, 0);
  cpl_ensure(cpl_matrix_get_nrow(aDeltaRA) == cpl_matrix_get_nrow(aDeltaDEC),
             CPL_ERROR_INCOMPATIBLE_INPUT, 0);
  cpl_ensure(cpl_matrix_get_nrow(aDeltaRA) == cpl_matrix_get_nrow(aMeanDEC),
             CPL_ERROR_INCOMPATIBLE_INPUT, 0);
  cpl_ensure(aRadius > 0., CPL_ERROR_ILLEGAL_INPUT, 0);
  cpl_ensure(aNbins > 0, CPL_ERROR_ILLEGAL_INPUT, 0);

  cpl_matrix *distance = muse_align_compute_distances(aDeltaRA, aDeltaDEC,
                                                      aMeanDEC, NULL, NULL);
  if (cpl_matrix_get_min(distance) >= aRadius) {
    cpl_matrix_delete(distance);
    *aOffsetRA  = 0.;
    *aOffsetDEC = 0.;
    *aWeight    = 1.;
    return 0;
  }

  cpl_array *selection = muse_cplmatrix_where(distance, aRadius,
                                              _muse_condition_less_than);
  cpl_matrix_delete(distance);
  cpl_matrix *dRA  = muse_cplmatrix_extract_selected(aDeltaRA, selection);
  cpl_matrix *dDEC = muse_cplmatrix_extract_selected(aDeltaDEC, selection);
  cpl_array_delete(selection);

  /* Search box from -aRadius to aRadius for both axes. */
  double bstep = 2. * aRadius / (double)aNbins;

  /* Note: Despite of the previous selection of positions which have        *
   * distances smaller than the search radius the bin index of the right    *
   * ascension may become invalid and must be restricted to valid range.    *
   * The reason is that in the following the difference in right ascension  *
   * is directly compared to the search radius, while in the selection of   *
   * positions this difference is corrected by the cosine of the mean       *
   * declination (cf. muse_align_compute_distances()), and thus differences *
   * in right ascension which are larger than the search radius may still   *
   * be accepted by the distance criterion!                                 *
   * To play it save also the index along the y-axis is validated.          */

  cpl_matrix *bins = cpl_matrix_new(1, aNbins);
  cpl_size ibin;
  for (ibin = 0; ibin < aNbins; ++ibin) {
    cpl_matrix_set(bins, 0, ibin, -aRadius + ibin * bstep);
  }
  cpl_matrix *histogram = cpl_matrix_new(aNbins, aNbins);
  double *hdata = cpl_matrix_get_data(histogram);
  cpl_size npos = cpl_matrix_get_ncol(dRA);
  cpl_size nmatch = 0;
  cpl_size ipos;
  for (ipos = 0; ipos < npos; ++ipos) {
    cpl_size ix = (cpl_size)((cpl_matrix_get(dRA, 0, ipos) + aRadius) / bstep);
    cpl_size iy = (cpl_size)((cpl_matrix_get(dDEC, 0, ipos) + aRadius) / bstep);
    if (((ix >= 0) && (ix < aNbins)) && ((iy >= 0) && (iy < aNbins))) {
      hdata[iy * aNbins + ix] += 1.;
      ++nmatch;
    }
  }
  cpl_matrix_delete(dRA);
  cpl_matrix_delete(dDEC);

  if (nmatch == 0) {
    cpl_matrix_delete(histogram);
    cpl_matrix_delete(bins);
    return 0;
  }

  cpl_size row;
  cpl_size column;
  cpl_matrix_get_maxpos(histogram, &row, &column);

  *aOffsetRA  = cpl_matrix_get(bins, 0, column) + 0.5 * bstep;
  *aOffsetDEC = cpl_matrix_get(bins, 0, row) + 0.5 * bstep;
  *aWeight    = cpl_matrix_get_max(histogram);
  cpl_matrix_delete(histogram);
  cpl_matrix_delete(bins);

  return nmatch;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Updates an estimated field offset from a set of object position
            offsets
  @param    aOffsetRA  Initial estimate of the right ascension offset of the
                       fields.
  @param    aOffsetDEC Initial estimate of the declination offset of the fields.
  @param    aWeight    Weight of the estimated offset (number of occurrences
                       of the estimates offset).
  @param    aDeltaRA   Right ascension offset matrix of pairs of detections
  @param    aDeltaDEC  Declination offset matrix of pairs of detections
  @param    aMeanDEC   Mean declination of pairs of detections
  @param    aRadius    Maximum allowed distance between candidate sources
  @return   the number of matches or zero on failure to find matches

  The initially given estimate of the field offset is updated (refined) to be
  the median of the offset of the object positions after correcting for the
  initial estimate.
 */
/*----------------------------------------------------------------------------*/
static cpl_size
muse_align_update_offsets(double *aOffsetRA, double *aOffsetDEC, double *aWeight,
                          const cpl_matrix *aDeltaRA, const cpl_matrix *aDeltaDEC,
                          const cpl_matrix *aMeanDEC, double aRadius)
{
  cpl_ensure((aOffsetRA && aOffsetDEC) && aWeight, CPL_ERROR_NULL_INPUT,
             CPL_FALSE);
  cpl_ensure((aDeltaRA && aDeltaDEC) && aMeanDEC, CPL_ERROR_NULL_INPUT,
             CPL_FALSE);
  cpl_ensure(cpl_matrix_get_nrow(aDeltaRA) == cpl_matrix_get_nrow(aDeltaDEC),
             CPL_ERROR_INCOMPATIBLE_INPUT, CPL_FALSE);
  cpl_ensure(cpl_matrix_get_nrow(aDeltaRA) == cpl_matrix_get_nrow(aMeanDEC),
             CPL_ERROR_INCOMPATIBLE_INPUT, CPL_FALSE);
  cpl_ensure(aRadius > 0., CPL_ERROR_ILLEGAL_INPUT, CPL_FALSE);

  cpl_matrix *distance = muse_align_compute_distances(aDeltaRA, aDeltaDEC,
                                                      aMeanDEC, aOffsetRA, aOffsetDEC);
  if (cpl_matrix_get_min(distance) >= aRadius) {
    cpl_matrix_delete(distance);
    *aOffsetRA  = 0.;
    *aOffsetDEC = 0.;
    *aWeight    = 1.;
    return 0;
  }

  cpl_array *selection = muse_cplmatrix_where(distance, aRadius,
                                              _muse_condition_less_than);
  cpl_matrix_delete(distance);
  cpl_matrix *dRA  = muse_cplmatrix_extract_selected(aDeltaRA, selection);
  cpl_matrix *dDEC = muse_cplmatrix_extract_selected(aDeltaDEC, selection);
  cpl_array_delete(selection);

  cpl_size nselected = cpl_matrix_get_ncol(dRA);
  double variance = 2. * aRadius * aRadius;
  if (nselected > 1) {
    double sdevRA  = cpl_matrix_get_stdev(dRA);
    double sdevDEC = cpl_matrix_get_stdev(dDEC);
    double _variance = sdevRA * sdevRA + sdevDEC * sdevDEC;
    if (_variance > nselected / DBL_MAX) {
      variance = _variance;
    }
  }

  *aOffsetRA  = cpl_matrix_get_median(dRA);
  *aOffsetDEC = cpl_matrix_get_median(dDEC);
  *aWeight = nselected / variance;

  cpl_matrix_delete(dRA);
  cpl_matrix_delete(dDEC);

  return nselected;
}

/*----------------------------------------------------------------------------*/
/**
  @private
  @brief    Compute field offsets for a set of exposures
  @param    aImagelist     the list of FOV images
  @param    aCataloglist   the list of sources for each FOV image.
  @param    aSearchradius  list of search radii
  @param    aNbins         number of histogram bins for first iteration
  @param    aWeight        Turn weighting on and off
  @param    aHeader        FITS header to add QC parameters
  @return   an offset table on success, or NULL otherwise.

  @note Passing a NULL propertylist is not a critical failure but results in
        lots of error messages about that.
 */
/*----------------------------------------------------------------------------*/
static cpl_table *
muse_align_compute_field_offsets(muse_imagelist *aImagelist,
                                 cpl_table **aCataloglist,
                                 const cpl_array *aSearchradius,
                                 int aNbins, cpl_boolean aWeight,
                                 cpl_propertylist *aHeader)
{
  cpl_ensure(aImagelist && aCataloglist, CPL_ERROR_NULL_INPUT, NULL);
  cpl_ensure(aSearchradius, CPL_ERROR_NULL_INPUT, NULL);
  cpl_ensure(aNbins > 1, CPL_ERROR_ILLEGAL_INPUT, NULL);

  cpl_size nfields = muse_imagelist_get_size(aImagelist);
  cpl_ensure(nfields >= 2, CPL_ERROR_DATA_NOT_FOUND, NULL);

  /* Create a list of all possible pairings of the input fields *
   * number of combinations: nfields choose 2                   */
  cpl_size npairs = nfields * (nfields - 1) / 2;

  muse_indexpair *combinations = cpl_calloc(npairs, sizeof *combinations);
  cpl_size ipair = 0;
  cpl_size ifield;
  for (ifield = 0; ifield < nfields; ++ifield) {
    cpl_size jfield;
    for (jfield = ifield + 1; jfield < nfields; ++jfield) {
      combinations[ipair].first = ifield;
      combinations[ipair].second = jfield;
      ++ipair;
    }
  }

  /* record number of detections in each image as QC parameter */
  for (ifield = 0; ifield < nfields; ++ifield) {
    char *kw = cpl_sprintf(QC_ALIGN_NDETi, (int)ifield + 1);
    cpl_propertylist_append_int(aHeader, kw,
                                cpl_table_get_nrow(aCataloglist[ifield]));
    cpl_free(kw);
  }

  /* prepare the data structure to record the number of matching  *
   * objects found in each of the other fields to, eventually,    *
   * compute the median number of matching detections for each    *
   * field.                                                       */
  cpl_array **aoverlaps = cpl_calloc(nfields, sizeof(cpl_array *));
  for (ifield = 0; ifield < nfields; ++ifield) {
    aoverlaps[ifield] = cpl_array_new(nfields, CPL_TYPE_INT);
  }

  /* search for correlations */
  cpl_matrix *ra_offsets = NULL;
  cpl_matrix *dec_offsets = NULL;
  cpl_size *has_neighbors = cpl_malloc(nfields * sizeof *has_neighbors);
  cpl_size isearch;
  for (isearch = 0; isearch < cpl_array_get_size(aSearchradius); ++isearch) {
    double radius = cpl_array_get_double(aSearchradius, isearch, NULL);

    /* Clear the number of matching detections. Note that this resets all   *
     * elements to valid. To exclude the elements which should not be taken *
     * into account in the final median computation they are invalidated.   *
     * This must at least be done for the "diagonal" elements of this       *
     * matrix like structure which would refer to the number of matching    *
     * detections a field has with itself, since these are never computed.  */
    for (ifield = 0; ifield < nfields; ++ifield) {
      cpl_array_fill_window_int(aoverlaps[ifield], 0, nfields, 0);
      /* XXX: To include combinations with zero matches in the median *
       *      statistic, use the alternative invalidation scheme      */
#if 0
      cpl_array_set_invalid(aoverlaps[ifield], ifield);
#else
      cpl_array_fill_window_invalid(aoverlaps[ifield], 0, nfields);
#endif
    }

    /* The linear system consists of one row for each pair of fields, and *
     * the closure relation                                               */
    cpl_matrix *weights = cpl_matrix_new(npairs + 1, nfields);
    cpl_matrix *offsets_ra = cpl_matrix_new(npairs + 1, 1);
    cpl_matrix *offsets_dec = cpl_matrix_new(npairs + 1, 1);

    cpl_matrix_fill_row(weights, 1., 0);
    cpl_size kpairs = 0;

    memset(has_neighbors, 0, nfields * sizeof *has_neighbors);

    for (ipair = 0; ipair < npairs; ++ipair) {
      /* Compute the distance between any 2 stars in the 2 fields */
      const cpl_table *catalog1 = aCataloglist[combinations[ipair].first];
      const cpl_table *catalog2 = aCataloglist[combinations[ipair].second];

      cpl_size nstars1 = cpl_table_get_nrow(catalog1);
      cpl_size nstars2 = cpl_table_get_nrow(catalog2);

      const double *ra1 = cpl_table_get_data_double_const(catalog1, "RA");
      const double *ra2 = cpl_table_get_data_double_const(catalog2, "RA");
      const double *dec1 = cpl_table_get_data_double_const(catalog1, "DEC");
      const double *dec2 = cpl_table_get_data_double_const(catalog2, "DEC");

      cpl_matrix *delta_ra  = cpl_matrix_new(nstars1, nstars2);
      cpl_matrix *delta_dec = cpl_matrix_new(nstars1, nstars2);
      cpl_matrix *dec_mean  = cpl_matrix_new(nstars1, nstars2);

      cpl_size irow;
      for (irow = 0; irow < nstars1; ++irow) {
        cpl_size icol;
        for (icol = 0; icol < nstars2; ++icol) {
          cpl_matrix_set(delta_ra, irow, icol, (ra1[irow] - ra2[icol]) * deg2as);
          cpl_matrix_set(delta_dec, irow, icol, (dec1[irow] - dec2[icol]) * deg2as);
          cpl_matrix_set(dec_mean, irow, icol,
                         0.5 *(dec1[irow] + dec2[icol]) * CPL_MATH_RAD_DEG);
        }
      }

      double offset_ra = 0.;
      double offset_dec = 0.;
      double weight = 1.;
      cpl_size noverlap = 0;
      if (isearch == 0) {
        noverlap = muse_align_estimate_offsets(&offset_ra, &offset_dec, &weight,
                                               delta_ra, delta_dec, dec_mean,
                                               radius, aNbins);
      } else {
        offset_ra = cpl_matrix_get(ra_offsets, combinations[ipair].first, 0) -
            cpl_matrix_get(ra_offsets, combinations[ipair].second, 0);
        offset_dec = cpl_matrix_get(dec_offsets, combinations[ipair].first, 0) -
            cpl_matrix_get(dec_offsets, combinations[ipair].second, 0);

        noverlap = muse_align_update_offsets(&offset_ra, &offset_dec, &weight,
                                             delta_ra, delta_dec, dec_mean,
                                             radius);
      }
      cpl_matrix_delete(dec_mean);
      cpl_matrix_delete(delta_ra);
      cpl_matrix_delete(delta_dec);

      if (noverlap > 0) {
        if (!aWeight) {
          weight = 1.;
        }

        ++kpairs;
        cpl_matrix_set(offsets_ra, kpairs, 0, weight * offset_ra);
        cpl_matrix_set(offsets_dec, kpairs, 0, weight * offset_dec);
        cpl_matrix_set(weights, kpairs, combinations[ipair].first, weight);
        cpl_matrix_set(weights, kpairs, combinations[ipair].second, -weight);
        has_neighbors[combinations[ipair].first] += 1;
        has_neighbors[combinations[ipair].second] += 1;

        /* record the number of overlaps for later median computation */
        cpl_array_set_int(aoverlaps[combinations[ipair].first],
                          combinations[ipair].second, noverlap);
        cpl_array_set_int(aoverlaps[combinations[ipair].second],
                          combinations[ipair].first, noverlap);
      } /* if noverlap positive */
    } /* for ipair */

    /* Replace the solution from the previous iteration */
    cpl_matrix_delete(ra_offsets);
    cpl_matrix_delete(dec_offsets);

    /* If there is no field with overlapping neighbors there is no  *
     * correction to the field coordinates, i.e. all offsets are 0. */
    if (!kpairs) {
      cpl_matrix_delete(offsets_dec);
      cpl_matrix_delete(offsets_ra);
      cpl_matrix_delete(weights);

      cpl_msg_warning(__func__, "No overlapping fields of view were found "
                      "for search radius %.4f!", radius);

      ra_offsets = cpl_matrix_new(nfields, 1);
      dec_offsets = cpl_matrix_new(nfields, 1);

    } else {
      /* Use only the valid part of the matrices when solving the linear system */
      cpl_matrix *_weights     = cpl_matrix_wrap(kpairs + 1, nfields,
                                                 cpl_matrix_get_data(weights));
      cpl_matrix *_offsets_ra  = cpl_matrix_wrap(kpairs + 1, 1,
                                                 cpl_matrix_get_data(offsets_ra));
      cpl_matrix *_offsets_dec = cpl_matrix_wrap(kpairs + 1, 1,
                                                 cpl_matrix_get_data(offsets_dec));

      ra_offsets = muse_cplmatrix_solve_least_square(_weights, _offsets_ra);
      dec_offsets = muse_cplmatrix_solve_least_square(_weights, _offsets_dec);

      cpl_matrix_unwrap(_offsets_dec);
      cpl_matrix_unwrap(_offsets_ra);
      cpl_matrix_unwrap(_weights);

      cpl_matrix_delete(offsets_dec);
      cpl_matrix_delete(offsets_ra);
      cpl_matrix_delete(weights);
    }
  } /* for isearch */
  cpl_free(combinations);

  /* Build the results table from the matrices, and add observation start  *
   * from the image header as observation identifier. For isolated fields, *
   * i.e.fields which do not overlap with any other, the pointing          *
   * corrections are forced to 0. For fields which have an offset could be *
   * computed it is converted to degrees.                                  */
  cpl_table *offsets = muse_cpltable_new(muse_offset_list_def,
                                         muse_imagelist_get_size(aImagelist));
  cpl_size minmatch = LLONG_MAX,
           nomatch = 0;
  for (ifield = 0; ifield < nfields; ++ifield) {
    muse_image *fov = muse_imagelist_get(aImagelist, ifield);
    const char *timestamp = muse_pfits_get_dateobs(fov->header);
    double mjd = muse_pfits_get_mjdobs(fov->header);
    cpl_table_set_string(offsets, MUSE_OFFSETS_DATEOBS, ifield, timestamp);
    cpl_table_set_double(offsets, MUSE_OFFSETS_MJDOBS, ifield, mjd);

    if (has_neighbors[ifield]) {
      double ra_offset = cpl_matrix_get(ra_offsets, ifield, 0);
      double dec_offset = cpl_matrix_get(dec_offsets, ifield, 0);
      cpl_table_set_double(offsets, MUSE_OFFSETS_DRA, ifield, ra_offset / deg2as);
      cpl_table_set_double(offsets, MUSE_OFFSETS_DDEC, ifield, dec_offset / deg2as);
      /* record QC parameter */
      int nmedian = cpl_array_get_median(aoverlaps[ifield]);
      char *kw = cpl_sprintf(QC_ALIGN_NMATCHi, (int)ifield + 1);
      cpl_propertylist_append_int(aHeader, kw, nmedian);
      cpl_free(kw);
      if (nmedian < minmatch) {
        minmatch = nmedian;
      }
    } else {
      cpl_table_set_double(offsets, MUSE_OFFSETS_DRA, ifield, 0.);
      cpl_table_set_double(offsets, MUSE_OFFSETS_DDEC, ifield, 0.);
      char *kw = cpl_sprintf(QC_ALIGN_NMATCHi, (int)ifield + 1);
      cpl_propertylist_append_int(aHeader, kw, 0);
      cpl_free(kw);
      nomatch++;
    }
  } /* for ifield */
  cpl_propertylist_append_int(aHeader, QC_ALIGN_NMATCH_MIN, minmatch);
  cpl_propertylist_append_int(aHeader, QC_ALIGN_NOMATCH, nomatch);
  muse_vfree((void **)aoverlaps, nfields, (muse_free_func)cpl_array_delete);
  cpl_free(has_neighbors);
  cpl_matrix_delete(dec_offsets);
  cpl_matrix_delete(ra_offsets);

  /* Force the offset of the first field of view to be zero and *
   * invalidate the flux scaling column, so that by no scaling  *
   * occurs during the exposure combination.                    */
  double ra_shift  = cpl_table_get(offsets, MUSE_OFFSETS_DRA, 0, NULL);
  double dec_shift = cpl_table_get(offsets, MUSE_OFFSETS_DDEC, 0, NULL);
  cpl_table_subtract_scalar(offsets, MUSE_OFFSETS_DRA, ra_shift);
  cpl_table_subtract_scalar(offsets, MUSE_OFFSETS_DDEC, dec_shift);
  cpl_table_set_column_invalid(offsets, MUSE_OFFSETS_FSCALE, 0, nfields);

  /* Extra QC parameters: offset statistics, in arcsec */
  double qcmin  = cpl_table_get_column_min(offsets, MUSE_OFFSETS_DRA) * 3600.;
  double qcmax  = cpl_table_get_column_max(offsets, MUSE_OFFSETS_DRA) * 3600.;
  double qcmean = cpl_table_get_column_mean(offsets, MUSE_OFFSETS_DRA) * 3600.;
  double qcsdev = cpl_table_get_column_stdev(offsets, MUSE_OFFSETS_DRA) * 3600.;

  cpl_propertylist_append_double(aHeader, QC_ALIGN_DRA_MIN, qcmin);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DRA_MAX, qcmax);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DRA_MEAN, qcmean);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DRA_STDEV, qcsdev);

  qcmin  = cpl_table_get_column_min(offsets, MUSE_OFFSETS_DDEC) * 3600.;
  qcmax  = cpl_table_get_column_max(offsets, MUSE_OFFSETS_DDEC) * 3600.;
  qcmean = cpl_table_get_column_mean(offsets, MUSE_OFFSETS_DDEC) * 3600.;
  qcsdev = cpl_table_get_column_stdev(offsets, MUSE_OFFSETS_DDEC) * 3600.;

  cpl_propertylist_append_double(aHeader, QC_ALIGN_DDEC_MIN, qcmin);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DDEC_MAX, qcmax);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DDEC_MEAN, qcmean);
  cpl_propertylist_append_double(aHeader, QC_ALIGN_DDEC_STDEV, qcsdev);

  return offsets;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Compute coordinate offset for each exposure with respect to the
            reference.
  @param    aProcessing  the processing structure
  @param    aParams      the parameters list
  @return   0 if everything is ok, -1 something went wrong

 */
/*----------------------------------------------------------------------------*/
int
muse_exp_align_compute(muse_processing *aProcessing,
                       muse_exp_align_params_t *aParams)
{

  cpl_size nexposures = cpl_frameset_count_tags(aProcessing->inframes,
                                                MUSE_TAG_IMAGE_FOV);
  if (nexposures < 2) {
    cpl_msg_error(__func__, "This recipe requires at least 2 input FOV "
                  "images!");
    return -1;
  }

  /* Validate input parameters */
  cpl_msg_debug(__func__, "Validating source detection parameters");
  if (!muse_align_check_detection_params(aParams)) {
    cpl_error_set_where(__func__);
    return -1;
  }

  /* Load and validate all fov input images */
  cpl_msg_info(__func__, "Loading FOV images");

  muse_imagelist *fovimages = muse_processing_fov_load_all(aProcessing);
  if (!fovimages) {
    cpl_msg_error(__func__, "Could not load FOV input images!");
    return -1;
  }
  nexposures = (cpl_size)muse_imagelist_get_size(fovimages);

  cpl_size iexposure;
  for (iexposure = 0; iexposure < nexposures; ++iexposure) {
    const muse_image *fov = muse_imagelist_get(fovimages, iexposure);
    cpl_errorstate es = cpl_errorstate_get();
    double mjdobs = muse_pfits_get_mjdobs(fov->header);

    if (!cpl_errorstate_is_equal(es) &&
        cpl_error_get_code() == CPL_ERROR_DATA_NOT_FOUND) {
      cpl_msg_error(__func__, "Missing \"MJD-OBS\" in FOV image %"
                    CPL_SIZE_FORMAT, iexposure + 1);
      muse_imagelist_delete(fovimages);
      return -1;
    } else {
      cpl_size jexposure;
      for (jexposure = iexposure + 1; jexposure < nexposures; ++jexposure) {
        const muse_image *_fov = muse_imagelist_get(fovimages, jexposure);
        double _mjdobs = muse_pfits_get_mjdobs(_fov->header);
        if (_mjdobs == mjdobs) {
          cpl_msg_warning(__func__, "Found FOV images with non-unique "
                          "\"MJD-OBS\" value (image %" CPL_SIZE_FORMAT
                          " and %" CPL_SIZE_FORMAT ")!", iexposure + 1,
                          jexposure + 1);
        }
      }
    }
  } /* for iexposure */

  cpl_msg_info(__func__, "Computing pointing corrections for %" CPL_SIZE_FORMAT
               " FOV images", nexposures);

  /* Cleanup NaN values which are possibly present in the input images to *
   * flag bad pixel. Replace them with an appropriate numerical value     */
  for (iexposure = 0; iexposure < nexposures; ++iexposure) {
    muse_image *image = muse_imagelist_get(fovimages, iexposure);
    muse_utils_replace_nan(image, 0.);
  }

  /* For each FOV input image run the source detection and collect *
   * the created source lists                                      */
  int nobjects[2] = {aParams->srcmin, aParams->srcmax};

  double roundness[2] = {aParams->roundmin, aParams->roundmax};
  double sharpness[2] = {aParams->sharpmin, aParams->sharpmax};

  cpl_table **srclists = cpl_calloc(nexposures, sizeof *srclists);

  for (iexposure = 0; iexposure < nexposures; ++iexposure) {
    cpl_msg_debug(__func__, "Detecting sources on image %" CPL_SIZE_FORMAT
                  " of %" CPL_SIZE_FORMAT, iexposure + 1, nexposures);

    muse_image *fov = muse_imagelist_get(fovimages, iexposure);
    double threshold = aParams->threshold;
    cpl_boolean iterate = CPL_TRUE;
    int iteration = 0;
    cpl_size ndetections = 0;
    cpl_table *detections;
    cpl_image *image = cpl_image_cast(fov->data, CPL_TYPE_DOUBLE);

    /* If the image does not have an even number of pixels along the *
     * any axis discard the last column and ro respectively.         */

    /* XXX: The size limit along the x- and y-axis corresponds to    *
     *      minimum convolution box size of muse_find_stars(). This  *
     *      check should better be done in muse_find_stars() itself! */
    cpl_size nx = cpl_image_get_size_x(image);
    cpl_size ny = cpl_image_get_size_y(image);

    if ((nx < 6) || (ny < 6)) {
      cpl_image_delete(image);
      muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
      muse_imagelist_delete(fovimages);
      cpl_msg_error(__func__, "Image %" CPL_SIZE_FORMAT " of %"
                    CPL_SIZE_FORMAT "is too small!", iexposure + 1, nexposures);
      return -1;
    }

    if ((nx % 2 != 0) || (ny % 2 != 0)) {
      cpl_msg_debug(__func__, "Trimming image %" CPL_SIZE_FORMAT " to an "
                    "even number of pixels along the axes.", iexposure + 1);
      if (nx % 2 != 0) {
        --nx;
      }
      if (ny % 2 != 0) {
        --ny;
      }
      cpl_image *_image = cpl_image_extract(image, 1, 1, nx, ny);
      if (!_image) {
        cpl_image_delete(image);
        muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
        muse_imagelist_delete(fovimages);
        cpl_msg_error(__func__, "Trimming image %" CPL_SIZE_FORMAT " failed",
                      iexposure + 1);
        return -1;
      }
      cpl_image_delete(image);
      image = _image;
    } /* if odd image dimension(s) */

    while (iterate) {
      cpl_errorstate status = cpl_errorstate_get();

      detections = muse_find_stars(image, threshold, aParams->fwhm,
                                   roundness, sharpness);
      if (!detections) {
        if (cpl_error_get_code() == CPL_ERROR_DATA_NOT_FOUND) {
          cpl_errorstate_set(status);
          ndetections = 0;
        } else {
          cpl_image_delete(image);
          muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
          muse_imagelist_delete(fovimages);
          cpl_msg_error(__func__, "Source detection failed on image %"
                        CPL_SIZE_FORMAT " of %" CPL_SIZE_FORMAT, iexposure + 1,
                        nexposures);
          return -1;
        }
      } else {
        ndetections = cpl_table_get_nrow(detections);
      }

      if ((ndetections > 0) &&
          ((ndetections >= nobjects[0]) && (ndetections <= nobjects[1]))) {
        iterate = CPL_FALSE;
      } else {
        cpl_table_delete(detections);
        detections = NULL;
        if (++iteration < aParams->iterations) {
          if (ndetections > nobjects[1]) {
            threshold += aParams->step;
          } else {
            threshold -= aParams->step;
            if (threshold <= 0.) {
              iterate = CPL_FALSE;
            }
          }
        } else {
          iterate = CPL_FALSE;
        }
      }
    } /* while iterate */
    cpl_image_delete(image);

    if (!detections) {
      muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
      muse_imagelist_delete(fovimages);
      if (ndetections == 0) {
        cpl_msg_error(__func__, "No point-sources were detected in image %"
                      CPL_SIZE_FORMAT, iexposure + 1);
      } else if (ndetections < nobjects[0]) {
        cpl_msg_error(__func__, "Too few (%" CPL_SIZE_FORMAT ") point-sources "
                      "were detected in image %" CPL_SIZE_FORMAT, ndetections,
                      iexposure + 1);
      } else if (ndetections > nobjects[1]) {
        cpl_msg_error(__func__, "Too many (%" CPL_SIZE_FORMAT ") point-sources "
                      "were detected in image %" CPL_SIZE_FORMAT, ndetections,
                      iexposure + 1);
      } else {
        cpl_msg_error(__func__, "Invalid number (%" CPL_SIZE_FORMAT ") of "
                      "point-sources were detected in image %" CPL_SIZE_FORMAT,
                      ndetections, iexposure + 1);
      }
      return -1;
    } /* if no detections */

    cpl_msg_info(__func__, "%" CPL_SIZE_FORMAT " point-sources detected in "
                 "image %" CPL_SIZE_FORMAT, ndetections, iexposure + 1);

    /* Compute right ascension and declination from the pixel coordinates of *
     * each detected point-source using the WCS information of each exposure */
    if (muse_align_celestial_from_pixel(detections, fov->header)) {
      muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
      muse_imagelist_delete(fovimages);
      cpl_msg_error(__func__, "Computing WCS coordinates from pixel positions "
                    "failed for image %" CPL_SIZE_FORMAT " of %"
                    CPL_SIZE_FORMAT, iexposure + 1, nexposures);
      return -1;
    }
    srclists[iexposure] = detections;
  } /* for iexposure */


  /* Compute exposure offsets */
  cpl_array *radii = muse_align_parse_valuelist(aParams->rsearch);
  if (!radii) {
    muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
    muse_imagelist_delete(fovimages);
    cpl_msg_error(__func__, "Cannot compute field offsets! No valid search "
                  "radius was given!");
    return -1;
  }

  cpl_msg_info(__func__, "Calculating field offsets...");
  cpl_propertylist *header = cpl_propertylist_new();
  cpl_table *offsets = muse_align_compute_field_offsets(fovimages, srclists, radii,
                                                        aParams->nbins, aParams->weight,
                                                        header);
  cpl_array_delete(radii);
  muse_vfree((void **)srclists, nexposures, (muse_free_func)cpl_table_delete);
  muse_imagelist_delete(fovimages);
  if (!offsets) {
    cpl_msg_error(__func__, "Could not compute FOV offsets for %"
                  CPL_SIZE_FORMAT " images", nexposures);
    cpl_propertylist_delete(header);
    return -1;
  }

  /* Write offset table product to file */
  cpl_error_code rc = muse_processing_save_table(aProcessing, -1, offsets,
                                                 header, MUSE_TAG_OFFSET_LIST,
                                                 MUSE_TABLE_TYPE_CPL);
  cpl_propertylist_delete(header);
  cpl_table_delete(offsets);

  if (rc != CPL_ERROR_NONE) {
    return -1;
  }

  return 0;
} /* muse_exp_align_compute() */
