/*
 * This file is part of the KMOS Pipeline
 * Copyright (C) 2002,2003 European Southern Observatory
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

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

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

#include <math.h>
#include <string.h>

#include <cpl.h>

#include "kmclipm_constants.h"
#include "kmclipm_math.h"

#include "kmo_dfs.h"
#include "kmo_error.h"
#include "kmo_cpl_extensions.h"
#include "kmo_priv_fit_profile.h"
#include "kmo_priv_shift.h"

#include "kmo_priv_combine.h"

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

static cpl_bivector * kmos_combine_shift(cpl_imagelist **, cpl_imagelist **,
        cpl_propertylist **, cpl_propertylist **, cpl_size, int, const char *,
        const char *, const char *, const char *, const char *, double, double,
        int, int, int, const char *, int, const enum extrapolationType, double, 
        const char *, int *, int *) ;
static cpl_error_code kmos_reject_deviant(kmclipm_vector *, double, double) ;

static double kmos_combine_vector(const kmclipm_vector *, const char *,
        double, double, int, int, int, double *,
        double, enum combine_status *);

/*----------------------------------------------------------------------------*/
/**
    @defgroup kmos_priv_combine     Helper functions for recipe kmo_combine.
 */
/*----------------------------------------------------------------------------*/

/**@{*/

/*----------------------------------------------------------------------------*/
/**
  @brief Calculates the mean of several images using different methods.
  @param data       The data.
  @param cmethod    The method to apply:
                    "ksigma":  At each position pixels which deviate
                               significantly
                               (val > mean + stdev * cpos_rej  or
                                val < mean - stdev * cneg_rej)
                               will be rejected. This method is
                               iterative.
                    "median":  At each pixel position the median is
                               calculated. This method is applied once.
                    "average": At each pixel position the average is
                               calculated. This method is applied once.
                    "min_max": The specified number of minimum and
                               maximum pixel values will be rejected.
                               This method is applied once.
                    "sum":     All selected frames are added.
  @param cpos_rej   Positive rejection threshold (cmethod == "ksigma" only)
  @param cneg_rej   Negative rejection threshold (cmethod == "ksigma" only)
  @param citer      Number of iterations (cmethod == "ksigma" only)
  @param cmax       Number of maximum pixel values to reject
                    (cmethod == "min_max" only)
  @param cmin       Number of minimum pixel values to reject
                    (cmethod == "min_max" only)
  @param avg_data   Output: The averaged data or NULL on error case.
  @param avg_noise  Input: If NULL no noise will be calculated.
                    Output: The averaged noise or NULL on error case.
  @param default_val  The default value set for the pixels where all values
                      have been rejected.

  @return CPL_ERROR_NONE in case of success.

  The returned images have to be deallocated with cpl_image_delete().

  The bad pixel maps of the input frames are not taken into account, and
  the ones of the created images are empty.

  Some notes on calculating the output noise frame:
  - An averaged noise image is only calculated when @c avg_noise is not NULL.
  - If the size of @c data equals one, the input noise frame is just
    propagated. If no input noise is available, the output noise can't be
    estimated.
  - If the size of @c data equals two, the two input noise frames are combined
    applying simple error propagation (sigma = sqrt(sigma1^2 + sigma2^2) / 2).
    If no input noise is available, the output noise can't be estimated.
  - If the size of @c data is greater than two, the noise is in any case
    recalculated (sigma = stddev(data)/sqrt(n)) either if there exist or not
    input noise frames.

  Possible cpl_error_code set in this function:
  @li CPL_ERROR_NULL_INPUT if data pointer is NULL
  @li CPL_ERROR_ILLEGAL_INPUT if the input image list is NULL or of size 1 or
  if any of the other inputs is zero or negative.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code kmos_combine_frames(
        const cpl_imagelist *   data,
        const char          *   cmethod,
        double                  cpos_rej,
        double                  cneg_rej,
        int                     citer,
        int                     cmax,
        int                     cmin,
        cpl_image           **  avg_data,
        cpl_image           **  avg_noise,
        double                  default_val)
{
    const cpl_image     *   cur_img ; 
    cpl_image           *   cur_img2 ;
    kmclipm_vector      *   data_vec ;
    float               *   pavg_data ;
    float               *   pavg_noise = NULL;
    float               *   pcur_img2 ;
    int                     nx, ny, nz, ix, iy, iz, rej ;
    double                  std_err;
    double                  tol, tmp_dbl ;
    enum combine_status     status = combine_ok;

    /* Check inputs */
    if (data==NULL || avg_data==NULL || cmethod==NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return CPL_ERROR_NULL_INPUT ;
    }
    if (cpl_imagelist_get_size(data)<=0 ||
            (strcmp(cmethod, "ksigma") && strcmp(cmethod, "median") &&
             strcmp(cmethod, "average") && strcmp(cmethod, "min_max") &&
             strcmp(cmethod, "sum")) ||
            cpl_imagelist_is_uniform(data) != 0) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    /* Initialise */
    tol = 1e-6 ;
    rej = 0 ;

    cur_img = cpl_imagelist_get_const(data, 0);
    nx = cpl_image_get_size_x(cur_img);
    ny = cpl_image_get_size_y(cur_img);
    nz = cpl_imagelist_get_size(data);

    /* Method dependend checks */
    if (!strcmp(cmethod, "ksigma")) {
        if (cpos_rej < 0 || cneg_rej < 0 || citer < 0) {
            cpl_msg_error(__func__, "Illegal inputs") ;
            cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
            return CPL_ERROR_ILLEGAL_INPUT ;
        }
        if (citer == 0) {
            cpl_msg_warning(cpl_func, "citer = 0 - Use cmethod average");
            cmethod = "average";
        }
        if ((fabs(cpos_rej) < tol) && (fabs(cneg_rej) < tol)) {
            cpl_msg_warning(cpl_func, 
                    "cpos_rej / cneg_rej are 0 - Use cmethod average");
            cmethod = "average";
        }

        if (nz == 1) {
            if (!kmclipm_omit_warning_one_slice) {
                cpl_msg_warning(cpl_func, "Only 1 plane - Use cmethod average");
                kmclipm_omit_warning_one_slice = TRUE;
            } else {
                kmclipm_omit_warning_one_slice++;
            }
            cmethod = "average";
        }
    }
    if (strcmp(cmethod, "min_max") == 0) {
        if (cmax < 0 || cmin < 0) {
            cpl_msg_error(__func__, "Illegal inputs") ;
            cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
            return CPL_ERROR_ILLEGAL_INPUT ;
        }
        if (cmin + cmax >= nz) {
            cpl_msg_warning(cpl_func, "cmin + cmax > nz - Use cmethod average");
            cmethod = "average";
        }
    }

    /* Create output noise-image and set to zero */
    if (avg_noise != NULL) {
        *avg_noise = cpl_image_new(nx, ny, CPL_TYPE_FLOAT);
        cpl_image_multiply_scalar(*avg_noise, 0.0);
        pavg_noise = cpl_image_get_data_float(*avg_noise);
    }

    /* Create the output combined image */
    *avg_data = cpl_image_new(nx, ny, CPL_TYPE_FLOAT);
    pavg_data = cpl_image_get_data_float(*avg_data);

    /* Combine */
    data_vec = kmclipm_vector_new(nz);
    /* Loop on all pixels */
    for (iy = 0; iy < ny; iy++) {
        for (ix = 0; ix < nx; ix++) {

            /* Fill the Z vector data_vec (NAN for bad pixels)*/
            for (iz = 0; iz < nz; iz++) {
                cur_img = cpl_imagelist_get_const(data, iz);
                tmp_dbl = cpl_image_get(cur_img, ix+1, iy+1, &rej);
                if (rej == 0) {
                    kmclipm_vector_set(data_vec, iz, tmp_dbl);
                } else {
                    kmclipm_vector_set(data_vec, iz, 0.0/0.0);
                }
            } /* for iz = 0 */
            std_err = 0;
            pavg_data[ix+iy*nx] = (float)kmos_combine_vector(data_vec, 
                    cmethod, cpos_rej, cneg_rej, citer, cmax, cmin,
                    &std_err, default_val, &status);

            if (status == combine_rejected) {
                /* this pixel is invalid */
                cpl_image_reject(*avg_data, ix+1, iy+1);
            }
            if (avg_noise != NULL) {
                pavg_noise[ix+iy*nx] = (float)std_err;
                if (status == combine_rejected || ((std_err == -1)&&(nz != 1))) {
                    /* This noise pixel is invalid */
                    cpl_image_reject(*avg_noise, ix+1, iy+1);
                }
            }
        } /* for ix = 0 */
    } /* for iy = 0 */
    kmclipm_vector_delete(data_vec);

    /* if nz is 1, kmclipm_combine_vector()
       returns default_val for std_err. We replace this by stddev(data)*/
    if ((nz == 1) && (avg_noise != NULL)) {
        cpl_imagelist *data_dup = cpl_imagelist_duplicate(data);
        cur_img2 = cpl_imagelist_get(data_dup, 0);
        pcur_img2 = cpl_image_get_data_float(cur_img2);
        /* reject infinite values */
        for (iy = 0; iy < ny; iy++) {
            for (ix = 0; ix < nx; ix++) {
                if (kmclipm_is_nan_or_inf(pcur_img2[ix+iy*nx])) {
                    cpl_image_reject(cur_img2, ix+1, iy+1);
                }
            }
        }

        tmp_dbl = cpl_image_get_stdev(cur_img2);
        for (iy = 0; iy < ny; iy++) {
            for (ix = 0; ix < nx; ix++) {
                if (cpl_image_is_rejected(cur_img2, ix+1, iy+1) == 0) {
                    pavg_noise[ix+iy*nx] = (float)tmp_dbl;
                } else {
                    pavg_noise[ix+iy*nx] = (float)default_val;
                    cpl_image_reject(*avg_noise, ix+1, iy+1);
                }
            }
        }
        cpl_imagelist_delete(data_dup); data_dup = NULL;
    }
    
    return CPL_ERROR_NONE ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief Check if there is a short repeating pattern
  @param in String to check
  @return if yes return this, otherwise truncate
*/
/*----------------------------------------------------------------------------*/
char * kmo_shorten_ifu_string(const char* in)
{
    char    *retstr     = NULL,
            *substr     = NULL,
            *found      = NULL;
    int     lensubstr   = 1,
            pos         = 0;

    KMO_TRY
    {
        /* Create first substring of length one */
        KMO_TRY_EXIT_IF_NULL(substr = calloc(strlen(in), sizeof(char)));
        strncpy(substr, in, lensubstr);

        // expand substring as much as possible
        found = strstr(in+1, substr);
        while ((found != NULL) && (strlen(substr)+strlen(found)!=strlen(in))) {
            lensubstr++;
            strncpy(substr, in, lensubstr);
            found = strstr(in+1, substr);
        }

        if (found != NULL) {
            // check if substring applies to whole input string
            pos = lensubstr;
            found = strstr(in+pos, substr);
            while ((found != NULL) && (pos + strlen(found) == strlen(in))) {
                pos += lensubstr;
                found = strstr(in+pos, substr);
            }

            if ((found == NULL) && (pos-1+lensubstr == (int)strlen(in))) {
                /* Pattern repeating until the end, ok!  */
                /* Just assign anything to found */
                found = substr;
            } else {
                /* Pattern not completely repeatable */
                found = NULL;
            }
        }

        if (found == NULL) {
            /* Didn't find any pattern, just truncate it to length of 10 */
            lensubstr = 10;
            if ((int)strlen(in) < lensubstr) {
                lensubstr = strlen(in);
            }
            strncpy(substr, in, lensubstr);

            KMO_TRY_EXIT_IF_NULL(retstr = cpl_sprintf("_%s_etc", substr));
        } else {
            KMO_TRY_EXIT_IF_NULL(retstr = cpl_sprintf("_%s", substr));
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        cpl_free(retstr); retstr = NULL;
    }
    return retstr;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Apply the subpixelshift here using kmo_priv_shift()
  @param    x            (Input) The desired pixel-shift (whole and sub-pixels)
  @param    y            (Input) The desired pixel-shift (whole and sub-pixels)
                         (Output) The remaining whole pixel-shift 
                                    (subpix-shift is applied)
  @param    data         (In/Output) The data cube, subpix-shifted afterwards.
  @param    noise        (In/Output) The noise cube, subpix-shifted afterwards.
  @param    header_data  (In/Output) The image data header.
  @param    header_noise (In/Output) The image noise header.
  @param    flux          1 if flux conservation should be applied, 0 otherwise.
  @param    method        Interpolation method.
  @param    extrapolate   Extrapolation type.
  @param    tol           Sub-pixelshift tolerance to ignore.
  @param    fid           File identifier to write pixel-shifts to.
  @param    xmin          (Output) Minimum extent in x.
  @param    xmax          (Output) Maximum extent in x.
  @param    ymin          (Output) Minimum extent in y.
  @param    ymax          (Output) Maximum extent in y.
  @param    name          The name of the IFU to combine
  @return   CPL_ERROR_NONE if everything is ok
  The headers will be altered, but it doesn't matter in this context, since the
  WCS of the output cubes are the ones of the first cube in the list (which is
  the reference). And kmo_align_subpix() isn't applied to this cube!
*/
/*----------------------------------------------------------------------------*/
cpl_error_code kmo_align_subpix(
        double              *   x, 
        double              *   y,
        cpl_imagelist       **  data,
        cpl_imagelist       **  noise,
        cpl_propertylist    **  header_data,
        cpl_propertylist    **  header_noise,
        int                     flux,
        const char          *   method,
        const enum extrapolationType extrapolate,
        double                  tol,
        FILE                *   fid,
        int                 *   xmin,
        int                 *   xmax,
        int                 *   ymin,
        int                 *   ymax,
        const char          *   name)
{
    cpl_error_code ret = CPL_ERROR_NONE;

    int         int_part_x      = 0,
                int_part_y      = 0,
                ifunr           = 0;
    double      fract_part_x    = 0.0,
                fract_part_y    = 0.0;
    cpl_image   *tmp_img        = NULL;
    const char  *frname         = NULL;

    KMO_TRY
    {
        if ((strcmp(name, "mapping") != 0) && (strcmp(name, "mapping8") != 0) &&
            (strcmp(name, "mapping24") != 0) && (strcmp(name, MAPPING8) != 0) &&
            (strcmp(name, MAPPING24) != 0)) {
            if (!((*x < KMOS_SLITLET_X) && (*x > -KMOS_SLITLET_X))) {
                cpl_msg_warning(__func__,
                        "X-shift for following IFU is larger than 14 pix!");
            }
            if (!((*y < KMOS_SLITLET_Y) && (*y > -KMOS_SLITLET_Y))) {
                cpl_msg_warning(__func__,
                        "Y-shift for following IFU is larger than 14 pix!");
            }
        }

        KMO_TRY_EXIT_IF_NULL(
            frname=cpl_propertylist_get_string(*header_data, "ESO PRO FRNAME"));
        ifunr = cpl_propertylist_get_int(*header_data, "ESO PRO IFUNR");
        KMO_TRY_CHECK_ERROR_STATE();

        cpl_msg_info(__func__, 
                "[%s, IFU %d] Shifts in x: %7.3f pix, in y: %7.3f pix", 
                frname, ifunr, *x, *y);

        /* Check shift in x */
        if (fabs(*x - floor(*x)) < tol) {
            /* Value is slightly greater than the real int value */
            int_part_x = floor(*x);
        } else {
            if (fabs(*x - floor(*x + tol)) < tol) {
                /* Value is slightly smaller than the real int value */
                int_part_x = floor(*x + tol);
            } else {
                /* Sub pixel shift */
                int_part_x = floor(*x);
                fract_part_x = *x - int_part_x;
            }
        }

        /* Check shift in y */
        if (fabs(*y - floor(*y)) < tol) {
            /* Value is slightly greater than the real int value */
            int_part_y = floor(*y);
        } else {
            if (fabs(*y - floor(*y + tol)) < tol) {
                /* Value is slightly smaller than the real int value */
                int_part_y = floor(*y + tol);
            } else {
                // sub pixel shift
                int_part_y = floor(*y);
                fract_part_y = *y - int_part_y;
            }
        }

        /* Check if subpixel-shift is <= 0.5 */
        /*      if not subtract 1 from it and increase */
        /* One pixel shift */
        if (fract_part_x > 0.5) {
            fract_part_x -= 1;
            int_part_x +=1;
        }
        if (fract_part_y > 0.5) {
            fract_part_y -= 1;
            int_part_y +=1;
        }

        /* Apply sub-pixel shift */
        if ((fabs(fract_part_x) > tol) || (fabs(fract_part_y) > tol)) {
            KMO_TRY_EXIT_IF_ERROR(
                kmo_priv_shift(data, noise, header_data, header_noise,
                    fract_part_x, fract_part_y, flux, method, extrapolate));
        } else {
            fract_part_x = 0;
            fract_part_y = 0;
        }

        fprintf(fid, "[%s, IFU %d] x: %20.9g\ty: %20.9g\n", frname, ifunr, 
                int_part_x+fract_part_x, int_part_y+fract_part_y);

        /* Correct here for clipped image due to subpix-shift */
        /* (applies only when shifting to left/up) */
        if (fract_part_x < 0.0) {
            int_part_x -=1;
        }
        if (fract_part_y > 0.0) {
            int_part_y +=1;
        }

        /* Calculate min/max-extent of new frame */
        KMO_TRY_EXIT_IF_NULL(tmp_img = cpl_imagelist_get(*data, 0));

        if (int_part_y + cpl_image_get_size_y(tmp_img) > *ymax) {
            *ymax = int_part_y + cpl_image_get_size_y(tmp_img);
        }
        if (int_part_x + cpl_image_get_size_x(tmp_img) > *xmax) {
            *xmax = int_part_x + cpl_image_get_size_x(tmp_img);
        }
        if (*ymin > int_part_y+1) {
            *ymin = int_part_y+1;
        }
        if (*xmin > int_part_x+1) {
            *xmin = int_part_x+1;
        }

        // return whole pixel-shifts
        *x = -1 * int_part_x;
        *y = int_part_y;
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        ret = cpl_error_get_code();
        *x = 0;
    }

    return ret;
}
 
/*----------------------------------------------------------------------------*/
/**
  @brief Shift cubes 
  @param cube_data        (Input/output) An array with the input data cubes
  @param cube_noise       (Input/output) An array with the input noise cubes 
                            (can be empty if @c noise_counter is 0)
  @param header_data      (Input/output) An array with data headers of the IFUs
  @param header_noise     (Input/output) An array with noise headers of the IFUs
                            (can be empty if @c noise_counter is 0)
  @param data_counter     The size of @c cube_data
  @param noise_counter    The size of @c cube_noise
  @param method           The comining method (none, header, user, center)
  @param smethod          The shift method (NN or BCS)
  @param fmethod          The kind of profile to fit (only for method==center)
  @param filename         The shifts input file name
  @param cmethod          The combine method
  @param cpos_rej         The positive sigma rejection threshold
  @param cneg_rej         The negative sigma rejection threshold
  @param citer            The number of iterations for rejection
  @param cmin             The max threshold
  @param cmax             The min threshold
  @param fn_shifts        Shifts file name
  @param flux             If flux conservation should be applied
  @param extrapolate      The kind of extrapolation applied
  @param tol              Tolerance
  @param name             The name of the IFU to combine
  @param nx_new
  @param nx_new
  @return                   
*/
/*----------------------------------------------------------------------------*/
static cpl_bivector * kmos_combine_shift(
        cpl_imagelist       **  cube_data,
        cpl_imagelist       **  cube_noise,
        cpl_propertylist    **  header_data,
        cpl_propertylist    **  header_noise,
        cpl_size                data_counter,
        int                     noise_counter,
        const char          *   method,
        const char          *   smethod,
        const char          *   fmethod,
        const char          *   filename,
        const char          *   cmethod,
        double                  cpos_rej,
        double                  cneg_rej,
        int                     citer,
        int                     cmin,
        int                     cmax,
        const char          *   fn_shifts,
        int                     flux,
        const enum extrapolationType extrapolate,
        double                  tol,
        const char          *   name,
        int                 *   nx_new,
        int                 *   ny_new)
{
    cpl_bivector    *   shifts ;
    double          *   pxshifts ;
    double          *   pyshifts ;
    const char      *   frname ;
    int                 ifunr, nx, ny, nz, x_min, x_max, y_min, y_max ;
    FILE            *   fid ;
    cpl_matrix      *   phys_ref ;
    cpl_matrix      *   world ;
    cpl_matrix      *   phys ;
    cpl_wcs         *   wcs_ref ;
    cpl_wcs         *   wcs ;
    cpl_array       *   status ;
    cpl_vector      *   identified ;
    cpl_image       *   tmp_img ;
    cpl_image       *   tmp_img2 ;
    cpl_bivector    *   tmp_shifts ;
    double          *   pxtmp_shifts ;
    double          *   pytmp_shifts ;
    cpl_vector      *   fit_pars ;
    cpl_vector      *   tmp_data_vec ;
    double              xref, yref ;
    int                 i ;

    /* Check Entries */

    /* Initialise */
    world = NULL ;
    status = NULL ;
    phys = NULL ;
    x_min = x_max = y_min = y_max = 0 ;
    *nx_new = *ny_new = 0 ;
    nz = cpl_imagelist_get_size(cube_data[0]);

    /* shifts: the shift of the reference frame wrt larger possible output */
    /* px/yshifts: the shiftvalues wrt the reference frame. */
    shifts = cpl_bivector_new(data_counter);
    pxshifts = cpl_bivector_get_x_data(shifts);
    pyshifts = cpl_bivector_get_y_data(shifts);
    for (i = 0; i < data_counter; i++) {
        pxshifts[i] = 0.0;
        pyshifts[i] = 0.0;
    }
    frname = cpl_propertylist_get_string(header_data[0], "ESO PRO FRNAME");
    ifunr = cpl_propertylist_get_int(header_data[0], "ESO PRO IFUNR");

    /* Open fid to save applied shifts (not for "none") */
    fid = fopen(fn_shifts, "w");
    fprintf(fid, "#shifts in pixels (to the left and up)\n");
    fprintf(fid, "#-------------------------------------\n");
    fprintf(fid, "[%s, IFU %d] x: %20.9g\ty: %20.9g\n", frname, ifunr, 0., 0.);
    
    /* Calculate output frame dimensions , do any subshifting and */
    /* Calculate whole-pixel shifts */
    if (!strcmp(method, "none")) {
        /* Fill shift vector --> already set to zero! */

        /* Check spatial dimension of frames */
        tmp_img=cpl_imagelist_get(cube_data[0],0);
        *nx_new = cpl_image_get_size_x(tmp_img);
        *ny_new = cpl_image_get_size_y(tmp_img);

        for (i = 1; i < data_counter; i++) {
            tmp_img = cpl_imagelist_get(cube_data[i], 0);
            nx = cpl_image_get_size_x(tmp_img);
            ny = cpl_image_get_size_y(tmp_img);
            if (nx != *nx_new || ny != *ny_new) {
                cpl_msg_warning(__func__, 
                        "Provided frames differ in spatial size!"
                        "(they will be aligned to lower left corner)");
            }
            if (nx > *nx_new)    *nx_new = nx;
            if (ny > *ny_new)    *ny_new = ny;
        }
    } else if (!strcmp(method, "header")) {
        /* Fill shift vector */
        phys_ref = cpl_matrix_new (1, 3);
        cpl_matrix_set(phys_ref, 0, 0, 1);  // lower left corner
        cpl_matrix_set(phys_ref, 0, 1, 1);
        cpl_matrix_set(phys_ref, 0, 2, 1);

        if ((wcs_ref = cpl_wcs_new_from_propertylist(header_data[0])) == NULL) {
            cpl_matrix_delete(phys_ref);
            cpl_bivector_delete(shifts) ;
            cpl_msg_error(__func__, "Cannot create WCS from Header") ; 
            cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
            return NULL ;
        }
        cpl_wcs_convert(wcs_ref, phys_ref, &world, &status, CPL_WCS_PHYS2WORLD);
        cpl_array_delete(status);

        tmp_img=cpl_imagelist_get(cube_data[0],0);
        y_min = 1;
        x_min = 1;
        y_max = cpl_image_get_size_y(tmp_img);
        x_max = cpl_image_get_size_x(tmp_img);

        cpl_msg_info(__func__, "Calculating & applying shifts for %d cubes:", 
                (int)data_counter);
        cpl_msg_info(__func__, 
                "[%s, IFU %d] Shifts in x: %7.3f pix, in y: %7.3f pix",
                frname, ifunr, 0., 0.);
        for (i = 1; i < data_counter; i++) {
            if ((wcs = cpl_wcs_new_from_propertylist(header_data[i])) == NULL) {
                cpl_matrix_delete(phys_ref);
                cpl_bivector_delete(shifts) ;
                cpl_matrix_delete(world);
                cpl_msg_error(__func__, "Cannot create WCS from Header") ; 
                cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
                return NULL ;
            }
            cpl_wcs_convert(wcs, world, &phys, &status, CPL_WCS_WORLD2PHYS);
            cpl_array_delete(status);

            pxshifts[i] = cpl_matrix_get(phys, 0, 0) - 1;
            pyshifts[i] = -1 * (cpl_matrix_get(phys, 0, 1) - 1);
            cpl_matrix_delete(phys);

            /* Align any subpixel shifts */
            if (noise_counter > 0) {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]),
                        &(cube_data[i]), &(cube_noise[i]), &(header_data[i]), 
                        &(header_noise[i]), flux, smethod, extrapolate, tol, 
                        fid, &x_min, &x_max, &y_min, &y_max, name);
            } else {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]), 
                        &(cube_data[i]), NULL, &(header_data[i]), NULL, flux, 
                        smethod, extrapolate, tol, fid, &x_min, &x_max, &y_min,
                        &y_max, name);
            }
            cpl_wcs_delete(wcs);
        }
        cpl_matrix_delete(world);
        cpl_wcs_delete(wcs_ref);
        cpl_matrix_delete(phys_ref);
    } else if (!strcmp(method, "user")) {
        /* Fill shift vector */
        cpl_bivector_delete(shifts); 
        shifts = cpl_bivector_read(filename);
        if (data_counter-1 != cpl_bivector_get_size(shifts)) {
            cpl_msg_error(__func__,
                    "Number of identified frames in sof-file (%lld) "
                    "with identified objects doesn't "
                    "match the number of pairs shift values in "
                    "provided file (%lld)! For n pairs of shift "
                    "values, n+1 frames are expected.",
                    data_counter, cpl_bivector_get_size(shifts));
            cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
            return NULL ;
        }

        /* Insert reference position (shift) of reference frame */
        tmp_shifts=cpl_bivector_new(data_counter);
        pxshifts=cpl_bivector_get_x_data(shifts);
        pyshifts=cpl_bivector_get_y_data(shifts);
        pxtmp_shifts = cpl_bivector_get_x_data(tmp_shifts);
        pytmp_shifts = cpl_bivector_get_y_data(tmp_shifts);
        for (i = 1; i < data_counter; i++) {
            pxtmp_shifts[i] = pxshifts[i-1];
            pytmp_shifts[i] = pyshifts[i-1];
        }
        cpl_bivector_delete(shifts);
        shifts = tmp_shifts;

        pxshifts=cpl_bivector_get_x_data(shifts);
        pyshifts=cpl_bivector_get_y_data(shifts);
        tmp_img=cpl_imagelist_get(cube_data[0],0);
        y_min = 1;
        x_min = 1;
        y_max = cpl_image_get_size_y(tmp_img);
        x_max = cpl_image_get_size_x(tmp_img);

        cpl_msg_info(__func__, 
                "Applying shifts for " "%d cubes:", (int)data_counter);
        cpl_msg_info(__func__, 
                "[%s, IFU %d] Shifts in x: %7.3f pix, in y: %7.3f pix",
                frname, ifunr, 0., 0.);
        for (i = 1; i < data_counter; i++) {
            /* Align any subpixel shifts */
            if (noise_counter > 0) {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]),
                        &(cube_data[i]), &(cube_noise[i]), &(header_data[i]),
                        &(header_noise[i]), flux, smethod, extrapolate, tol, 
                        fid, &x_min, &x_max, &y_min, &y_max, name);
            } else {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]),
                        &(cube_data[i]), NULL, &(header_data[i]), NULL, flux, 
                        smethod, extrapolate, tol, fid, &x_min, &x_max, &y_min,
                        &y_max, name);
            }
        }
    } else if (!strcmp(method, "center")) {
        /* Fill shift vector */
        identified = cpl_vector_new(nz);
        cpl_vector_fill(identified, 1.0);
        kmclipm_make_image(cube_data[0], NULL, &tmp_img, NULL, identified, 
                cmethod, cpos_rej, cneg_rej, citer, cmax, cmin); 
        fit_pars = kmo_fit_profile_2D(tmp_img, NULL, fmethod, &tmp_img2, NULL);
        cpl_image_delete(tmp_img2);
        xref = cpl_vector_get(fit_pars, 2);
        yref = cpl_vector_get(fit_pars, 3);
        cpl_vector_delete(fit_pars);

        y_min = 1;
        x_min = 1;
        y_max = cpl_image_get_size_y(tmp_img);
        x_max = cpl_image_get_size_x(tmp_img);
        cpl_image_delete(tmp_img);

        cpl_msg_info(__func__, "Calculating & applying shifts for %d cubes:", 
                (int)data_counter);
        cpl_msg_info(__func__, 
                "[%s, IFU %d] Shifts in x: %7.3f pix, in y: %7.3f pix",
                frname, ifunr, 0., 0.);
        for (i = 1; i < data_counter; i++) {
            cpl_vector_fill(identified, 1.0);
            kmclipm_make_image(cube_data[i], NULL, &tmp_img, NULL, identified, 
                    cmethod, cpos_rej, cneg_rej, citer, cmax, cmin);
            fit_pars = kmo_fit_profile_2D(tmp_img, NULL, fmethod, &tmp_img2, 
                    NULL);
            cpl_image_delete(tmp_img);
            cpl_image_delete(tmp_img2);

            double x2 = cpl_vector_get(fit_pars, 2);
            double y2 = cpl_vector_get(fit_pars, 3);
            cpl_vector_delete(fit_pars);

            pxshifts[i] = x2 - xref;
            pyshifts[i] = yref - y2;

            /* Align any subpixel shifts */
            if (noise_counter > 0) {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]), 
                        &(cube_data[i]), &(cube_noise[i]), &(header_data[i]), 
                        &(header_noise[i]), flux, smethod, extrapolate, tol, 
                        fid, &x_min, &x_max, &y_min, &y_max, name);
            } else {
                kmo_align_subpix(&(pxshifts[i]), &(pyshifts[i]), 
                        &(cube_data[i]), NULL, &(header_data[i]), NULL, flux, 
                        smethod, extrapolate, tol, fid, &x_min, &x_max, &y_min,
                        &y_max, name);
            }
        }
        cpl_vector_delete(identified);
    }
    fclose(fid); 

    /* Set reference frame position in respect to the WCS of first frame */
    if (strcmp(method, "none")) {
        tmp_data_vec = cpl_vector_wrap(data_counter, pxshifts);
        pxshifts[0] = abs(cpl_vector_get_min(tmp_data_vec)) ;
        cpl_vector_unwrap(tmp_data_vec);

        tmp_data_vec = cpl_vector_wrap(data_counter, pyshifts);
        pyshifts[0] = abs(cpl_vector_get_min(tmp_data_vec)) ;
        cpl_vector_unwrap(tmp_data_vec);

        for (i = 1; i < data_counter; i++) {
            pxshifts[i] += pxshifts[0];
            pyshifts[i] += pyshifts[0];
        }

        /* Check spatial dimension of frames */
        *nx_new = x_max - x_min + 1 ;
        *ny_new = y_max - y_min + 1 ;
    }
    return shifts;
}

/*----------------------------------------------------------------------------*/
/**
  @brief Combines cubes by shifting them accordingly
  @param cube_data        (Input/output) An array with the input data cubes
  @param cube_noise       (Input/output) An array with the input noise cubes 
                            (can be empty if @c noise_counter is 0)
  @param header_data      (Input/output) An array with data headers of the IFUs
  @param header_noise     (Input/output) An array with noise headers of the IFUs
                            (can be empty if @c noise_counter is 0)
  @param data_counter     The size of @c cube_data
  @param noise_counter    The size of @c cube_noise
  @param name             The name of the IFU to combine
  @param ifus_txt         The indices of the IFUs to combine (e.g. "5;4;7")
  @param method           The comining method (none, header, user, center)
  @param smethod          The shift method (NN or BCS)
  @param fmethod          The kind of profile to fit 
                            (only for @c method==center)
  @param filename         The filename containing ths shifts 
                            (only for @c method==user)
  @param cmethod          The combine method
  @param cpos_rej         The positive sigma rejection threshold
  @param cneg_rej         The negative sigma rejection threshold
  @param citer            The number of iterations for rejection
  @param cmin             The max threshold
  @param cmax             The min threshold
  @param extrapolate      The kind of extrapolation applied
  @param flux             If flux conservation should be applied
  @param cube_combined_data  (Output) The combined data cube
  @param cube_combined_noise (Output) The combined noise cube
  @param exp_mask            (Output) The exposure time mask
  @return                 xxx, ang1= 0.0, ang2= 0.0;
*/
/*----------------------------------------------------------------------------*/
cpl_error_code kmo_priv_combine(
        cpl_imagelist       **  cube_data,
        cpl_imagelist       **  cube_noise,
        cpl_propertylist    **  header_data,
        cpl_propertylist    **  header_noise,
        cpl_size                data_counter,
        int                     noise_counter,
        const char          *   name,
        const char          *   ifus_txt,
        const char          *   method,
        const char          *   smethod,
        const char          *   fmethod,
        const char          *   filename,
        const char          *   cmethod,
        double                  cpos_rej,
        double                  cneg_rej,
        int                     citer,
        int                     cmin,
        int                     cmax,
        const enum extrapolationType extrapolate,
        int                     flux,
        cpl_imagelist       **  cube_combined_data,
        cpl_imagelist       **  cube_combined_noise,
        cpl_image           **  exp_mask)
{
    char            *   clean_suffix; 
    char            *   ext_name ; 
    char            *   fn_shifts ; 
    int                 nx, ny, nz, nx_new, ny_new, tmp_int, nr_identified ;
    double              cd1_1, cd1_2, ang1, ang2, stdev, tol, std_err ;
    cpl_bivector    *   shifts ; 
    double          *   pxshifts ;
    double          *   pyshifts ;
    double          *   ptmp_data_vec ;
    double          *   ptmp_noise_vec = NULL;
    cpl_vector      *   identified ;
    double          *   pidentified ;
    int                 i, ix, iy, iz ;
    cpl_image       *   img_data_tmp ;
    cpl_image       *   img_noise_tmp ;
    cpl_image       **  data_cur_img_list ;
    cpl_image       **  noise_cur_img_list = NULL;
    float           *   pimg_data_tmp ;
    float           *   pimg_noise_tmp ;
    float           *   pexp_mask ;
    float           *   p ;
    cpl_vector      *   tmp_data_vec ;
    cpl_vector      *   tmp_noise_vec ;
    enum combine_status combStatus ;

    /* Check entries */
    if (cube_data == NULL || header_data == NULL) {
        cpl_msg_error(__func__, "Null Inputs") ; 
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return CPL_ERROR_NULL_INPUT ;
    }
    if (strcmp(smethod, "NN") && strcmp(smethod, "BCS")) {
        cpl_msg_error(__func__, "Wrong smethod") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }
    if (data_counter <= 0) {
        cpl_msg_error(__func__, "No data to combine") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }
    if (noise_counter > 0 && (cube_noise==NULL || header_noise == NULL)) { 
        cpl_msg_error(__func__, "Missing Noise data") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }
    nz = cpl_imagelist_get_size(cube_data[0]);
    for (i = 1; i < data_counter; i++) {
        if (nz != cpl_imagelist_get_size(cube_data[i])) {
            cpl_msg_error(__func__, "Data Size mismatch") ;
            cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
            return CPL_ERROR_ILLEGAL_INPUT ;
        }
    }
    if (noise_counter > 0) {
        for (i = 0; i < noise_counter; i++) {
            if (nz != cpl_imagelist_get_size(cube_data[i])) {
                cpl_msg_error(__func__, "Noise Size mismatch") ;
                cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
                return CPL_ERROR_ILLEGAL_INPUT ;
            }
        }
    }
    /* Check if rotation angle matches (allow tolerance of 0.5deg) */
    cd1_1 = kmo_dfs_get_property_double(header_data[0], CD1_1);
    cd1_2 = kmo_dfs_get_property_double(header_data[0], CD1_2);
    ang1 = atan(cd1_2/cd1_1)*180/CPL_MATH_PI;
    for (i = 1; i < data_counter; i++) {
        cd1_1 = kmo_dfs_get_property_double(header_data[i], CD1_1);
        cd1_2 = kmo_dfs_get_property_double(header_data[i], CD1_2);
        ang2 = atan(cd1_2/cd1_1)*180/CPL_MATH_PI;
        if (strcmp(method, "none") != 0) {
            /* Center, header, user */
            if (fabs(ang1-ang2) > 0.5) {
                cpl_msg_error(__func__, "Cube Orientation mismatch") ;
                cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
                return CPL_ERROR_ILLEGAL_INPUT ;
            }
        } else {
            /* none */
            if (fabs(ang1-ang2) > 0.5) 
                cpl_msg_warning(__func__, "Cube Orientation mismatch") ;
        }
    }

    /* Check if the keywords were found */
    if (cpl_error_get_code() != CPL_ERROR_NONE) {
        cpl_msg_error(__func__, "Missing Keywords") ;
        cpl_error_set(__func__, CPL_ERROR_DATA_NOT_FOUND) ;
        return CPL_ERROR_DATA_NOT_FOUND ;
    }

    /* Initialіse */
    tol = 0.00001 ;
    combStatus = combine_ok;

    /* Remove unwanted characters from suffix */
    if (name != NULL) {
        clean_suffix = cpl_sprintf("%s", name);
        kmo_clean_string(clean_suffix);
        fn_shifts = cpl_sprintf("shifts_applied_%s.txt", clean_suffix);
        cpl_free(clean_suffix); 
    } else {
        fn_shifts = cpl_sprintf("shifts_applied.txt");
    }
    
    /* No repeating warnings "The number of identified slices is one! */
    kmclipm_omit_warning_one_slice = TRUE;

    /* Allocate Outputs */
    data_cur_img_list=cpl_malloc(data_counter*sizeof(cpl_image*));
    if (noise_counter > 0) {
        noise_cur_img_list = cpl_malloc(noise_counter*sizeof(cpl_image*));
    }

    /* Apply the Shifts */
    if ((shifts = kmos_combine_shift(cube_data, cube_noise, header_data, 
            header_noise, data_counter, noise_counter, method, smethod, 
            fmethod, filename, cmethod, cpos_rej, cneg_rej, citer, cmin, cmax, 
            fn_shifts, flux, extrapolate, tol, name, &nx_new, &ny_new))==NULL) {
        cpl_free(fn_shifts); 
        cpl_free(data_cur_img_list) ;
        if (noise_counter > 0) cpl_free(noise_cur_img_list) ;
        cpl_msg_error(__func__, "Shifting failed") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }
    cpl_free(fn_shifts); 

    pxshifts=cpl_bivector_get_x_data(shifts);
    pyshifts=cpl_bivector_get_y_data(shifts);

    /* --- process data --- */
    tmp_data_vec = cpl_vector_new(data_counter);
    ptmp_data_vec = cpl_vector_get_data(tmp_data_vec);
    identified = cpl_vector_new(data_counter);
    pidentified = cpl_vector_get_data(identified);
    if (noise_counter > 0) {
        tmp_noise_vec = cpl_vector_new(data_counter);
        ptmp_noise_vec = cpl_vector_get_data(tmp_noise_vec);
    }

    *cube_combined_data = cpl_imagelist_new();
    if (cube_combined_noise != NULL) {
        *cube_combined_noise=cpl_imagelist_new();
    }

    if (exp_mask != NULL) {
        *exp_mask = cpl_image_new(nx_new, ny_new, CPL_TYPE_FLOAT);
        pexp_mask = cpl_image_get_data_float(*exp_mask);
        kmo_image_fill(*exp_mask,0.);
    }

    // loop through all slices
    for (iz = 0; iz< nz; iz++) {
        /* printf("\rCombining frames: %3d%% done",(int)(100*iz/nz)); */

        /* Get all img-pointers to images of slice iz */
        for (i = 0; i < data_counter; i++) {
            data_cur_img_list[i] = cpl_imagelist_get(cube_data[i], iz);
            if (noise_counter > 0) {
                noise_cur_img_list[i] = cpl_imagelist_get(cube_noise[i], iz);
            }
        }
        img_data_tmp = cpl_image_new(nx_new, ny_new, CPL_TYPE_FLOAT);
        img_noise_tmp = cpl_image_new(nx_new, ny_new, CPL_TYPE_FLOAT);
        pimg_data_tmp = cpl_image_get_data_float(img_data_tmp);
        pimg_noise_tmp = cpl_image_get_data_float(img_noise_tmp);

        // fill data vector
        for (iy = 0; iy < ny_new; iy++) {
            for (ix = 0; ix < nx_new; ix++) {
                nr_identified = 0;
                // the valid pixel is calculated
                for (i = 0; i < data_counter; i++) {
                    nx = cpl_image_get_size_x(data_cur_img_list[i]);
                    ny = cpl_image_get_size_y(data_cur_img_list[i]);
                    if (ix - pxshifts[i] >= 0 && ix - pxshifts[i] < nx &&
                        iy - pyshifts[i] >= 0 && iy - pyshifts[i] < ny) {
                        pidentified[i] = 1.0;
                        nr_identified++;

                        // don't use cpl_image_get() here, it 
                        // returns 0 instead of NaN for rej pixels
                        p = cpl_image_get_data_float(data_cur_img_list[i]);
                        ptmp_data_vec[i] = p[(int)((ix - pxshifts[i]) + 
                                nx*(iy - pyshifts[i]))];
                        if (noise_counter > 0) {
                            p = cpl_image_get_data_float(
                                    noise_cur_img_list[i]);
                            ptmp_noise_vec[i] = p[(int)((ix - pxshifts[i]) 
                                    + nx*(iy - pyshifts[i]))];
                        }
                    } else {
                        pidentified[i] = 0.0;
                        ptmp_data_vec[i] = 0.0;
                        if (noise_counter > 0)  ptmp_noise_vec[i] = 0.0;
                    }
                } // for i=data_counter
                cpl_vector *dupd=cpl_vector_duplicate(tmp_data_vec);
                cpl_vector *dupm = cpl_vector_duplicate(identified);
                kmclipm_vector *kvd = NULL, *kvn = NULL;
                cpl_vector *dupn = NULL;
                kvd = kmclipm_vector_create2(dupd, dupm);
                if (noise_counter > 0) {
                    dupn = cpl_vector_duplicate(tmp_noise_vec);
                    kvn = kmclipm_vector_create2(dupn, 
                            cpl_vector_duplicate(dupm));
                }
                // call kmclipm_combine_vector here
                if (nr_identified > 0) {
                    pimg_data_tmp[ix+iy*nx_new] =
                        kmclipm_combine_vector(kvd, kvn, cmethod, cpos_rej,
                                cneg_rej, citer, cmax, cmin, &tmp_int, 
                                &stdev, &std_err, 0., &combStatus);
                    if (exp_mask != NULL) {
                        pexp_mask[ix+iy*nx_new] = nr_identified;
                    }
                } else {
                    // no data value here, reject pixel
                    cpl_image_reject(img_data_tmp, ix+1, iy+1);
                }
                kmclipm_vector_delete(kvd);
                if (noise_counter > 0) kmclipm_vector_delete(kvn);
                if (nr_identified > 1) {
                    pimg_noise_tmp[ix+iy*nx_new] = std_err;
                } else {
                    // no noise value here, reject pixel
                    cpl_image_reject(img_noise_tmp, ix+1,iy+1);
                }
            }  // for ix < nx_new
        }  // for iy < ny_new

        cpl_imagelist_set(*cube_combined_data, img_data_tmp, iz);
        if (cube_combined_noise != NULL) {
            cpl_imagelist_set(*cube_combined_noise, img_noise_tmp, iz);
        } else {
            cpl_image_delete(img_noise_tmp) ;
        }
    }  // for iz < nz
    /* printf("\rCombining frames: %3d%% done", 100); */
    /* printf("\n"); */
    cpl_free(data_cur_img_list) ;
    if (noise_counter > 0) cpl_free(noise_cur_img_list) ;
    cpl_vector_delete(tmp_data_vec) ;
    if (noise_counter > 0) cpl_vector_delete(tmp_noise_vec) ;
    cpl_vector_delete(identified) ;
 
    /* Setup sub_header */
    if (header_noise!=NULL && noise_counter==0) {
        header_noise[0] = cpl_propertylist_duplicate(header_data[0]);
        /* !!!!! Dealllocate this outside the function !!!!!! */
    } 
    if (strcmp(ifus_txt, "") != 0) {
        ext_name = cpl_sprintf("IFU.[%s].DATA", ifus_txt);
        kmclipm_update_property_string(header_data[0], EXTNAME, ext_name, 
                "FITS extension name");
        cpl_free(ext_name);
        if (header_noise != NULL) {
            ext_name = cpl_sprintf("IFU.[%s].NOISE", ifus_txt);
            kmclipm_update_property_string(header_noise[0], EXTNAME, ext_name, 
                    "FITS extension name");
            cpl_free(ext_name);
        }
    }
    if (strcmp(name, "") != 0) {
        ext_name = cpl_sprintf("%s.DATA", name);
        kmclipm_update_property_string(header_data[0], EXTNAME, ext_name, 
                "FITS extension name");
        cpl_free(ext_name);
        if (header_noise != NULL) {
            ext_name = cpl_sprintf("%s.NOISE", name);
            kmclipm_update_property_string(header_noise[0], EXTNAME, ext_name, 
                    "FITS extension name");
            cpl_free(ext_name);
        }
    }
    kmclipm_update_property_double(header_data[0], CRPIX1, 
            cpl_propertylist_get_double(header_data[0], CRPIX1) + pxshifts[0], 
            "[pix] Reference pixel in x");
    kmclipm_update_property_double(header_data[0], CRPIX2, 
            cpl_propertylist_get_double(header_data[0], CRPIX2) + pyshifts[0], 
            "[pix] Reference pixel in y");
    if (header_noise != NULL) {
        kmclipm_update_property_double(header_noise[0], CRPIX1, 
                cpl_propertylist_get_double(header_noise[0], CRPIX1) + 
                pxshifts[0], "[pix] Reference pixel in x");
        kmclipm_update_property_double(header_noise[0], CRPIX2, 
                cpl_propertylist_get_double(header_noise[0], CRPIX2) + 
                pyshifts[0], "[pix] Reference pixel in y");
    }
    cpl_bivector_delete(shifts) ;
    return CPL_ERROR_NONE;
}

cpl_error_code kmo_edge_nan(
        cpl_imagelist   *   data, 
        int                 ifu_nr)
{
    cpl_error_code  err     = CPL_ERROR_NONE;
    cpl_image       *img    = NULL;
    float           *pimg   = NULL;
    int             nx      = 0,
                    ny      = 0,
                    nz      = 0,
                    ix      = 0,
                    iy      = 0,
                    iz      = 0;

    KMO_TRY
    {
        KMO_TRY_ASSURE(data != NULL, CPL_ERROR_NULL_INPUT,
                "No input data is provided!");

        KMO_TRY_ASSURE((ifu_nr >= 0) && (ifu_nr <= KMOS_NR_IFUS),
                CPL_ERROR_ILLEGAL_INPUT, 
                "ifu_nr must be between 1 and %d", KMOS_NR_IFUS);

        KMO_TRY_EXIT_IF_NULL(img = cpl_imagelist_get(data, 0));

        nx = cpl_image_get_size_x(img);
        ny = cpl_image_get_size_y(img);
        nz = cpl_imagelist_get_size(data);
        KMO_TRY_CHECK_ERROR_STATE();

        for (iz = 0; iz < nz; iz++) {
            KMO_TRY_EXIT_IF_NULL(img = cpl_imagelist_get(data, iz));
            KMO_TRY_EXIT_IF_NULL(pimg = cpl_image_get_data(img));
            for (ix = 0; ix < nx; ix++) {
                for (iy = 0; iy < ny; iy++) {
                    if (ifu_nr <= 16) {
                        // IFU 1-16
                        //cube[*,0,*] = !values.f_nan
                        //cube[*,13,*] = !values.f_nan
                        if ((iy == 0) || (iy == ny-1)) {
                            pimg[ix+iy*nx] = 0./0.;
                        }
                    } else {
                        // IFU 17-24
                        //cube[0,*,*] = !values.f_nan
                        //cube[13,*,*] = !values.f_nan
                        if ((ix == 0) || (ix == nx-1)) {
                            pimg[ix+iy*nx] = 0./0.;
                        }
                    }
                }
            }
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        err = cpl_error_get_code();
    }

    return err;
}

/*----------------------------------------------------------------------------*/
/**
  @brief Rejects outliers in a vector.
  @param kv       The vector.
  @param cpos_rej Positive rejection threshold factor.
  @param cneg_rej Negative rejection threshold factor.
  @return A possibly shorter vector than @c data . 
        It contains the indices of the valid pixels to consider later
  Only works on vectors of size >= 5
  Deviant values will be removed using a two-step rejection.
  Possible cpl_error_code set in this function:
  @li CPL_ERROR_NULL_INPUT    if @c data is NULL.
  @li CPL_ERROR_ILLEGAL_INPUT if @c cpos_rej or @c cneg_rej < 0
*/
/*----------------------------------------------------------------------------*/
static cpl_error_code kmos_reject_deviant(
        kmclipm_vector  *   kv,
        double              cpos_rej,
        double              cneg_rej)
{
    cpl_vector      *tmp_vec_sort       = NULL;
    kmclipm_vector  *kv_dup             = NULL;
    double          median_val          = 0.0,
                    stddev_val          = 0.0,
                    clip_val            = 0.0,
                    clip_val_pos        = 0.0,
                    clip_val_neg        = 0.0,
                    *pkvdata            = NULL,
                    *pkvmask            = NULL;
    int             size                = 0,
                    i                   = 0,
                    index               = 0,
                    nz                  = 0;

    /* Check Entries */
    if (kv == NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return CPL_ERROR_NULL_INPUT ;
    }
    if (cpos_rej < 0 || cneg_rej < 0) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    nz = cpl_vector_get_size(kv->data);

    /* Switch off this rejection for small vectors */
    if (nz < 5) return CPL_ERROR_NONE ;

    /* 1st rejection iteration (80% clipping) */
    kv_dup = kmclipm_vector_duplicate(kv);
    kmclipm_vector_abs(kv_dup);

    /* sort subtracted values of non-rejected elements and get 80% clipping
       value of sorted values
       (the clipping is only applied if the vector is large enough (size>3))
    */
    tmp_vec_sort = kmclipm_vector_create_non_rejected(kv_dup);
    kmclipm_vector_delete(kv_dup);
    size = cpl_vector_get_size(tmp_vec_sort);
    /* CPL_SORT_ASCENDING */
    cpl_vector_sort(tmp_vec_sort, 1);

    index = (int)ceil(0.79 * size) -1;
    if ((nz > 3) && (index < size)) {
        pkvdata = cpl_vector_get_data(kv->data);
        pkvmask = cpl_vector_get_data(kv->mask);

        /* clip values larger than 80% threshold on input data */
        clip_val = cpl_vector_get(tmp_vec_sort, index);
        clip_val_pos = clip_val * 1.5 *cpos_rej;
        clip_val_neg = -clip_val * 1.5 *cneg_rej;
        for (i = 0; i < nz; i++) {
            if ((pkvmask[i] >= 0.5) && ((pkvdata[i] > clip_val_pos) ||
                (pkvdata[i] < clip_val_neg)))   pkvmask[i] = 0.;
        }
    }
    cpl_vector_delete(tmp_vec_sort);

    /*  2nd rejection iteration */

    /* get absolute values from data minus median */
    median_val = kmclipm_vector_get_median(kv, KMCLIPM_ARITHMETIC);
    stddev_val = kmclipm_vector_get_stdev_median(kv);
    if (stddev_val < 0) {
        cpl_error_reset();
        /* all values are rejected */
        stddev_val = 0.0;
    }

    pkvdata = cpl_vector_get_data(kv->data);
    pkvmask = cpl_vector_get_data(kv->mask);

    /* clip values according to cpos_rej and cneg_rej */
    clip_val_pos = median_val + cpos_rej * stddev_val;
    clip_val_neg = median_val - cneg_rej * stddev_val;
    for (i = 0; i < nz; i++) {
        if ((pkvmask[i] >= 0.5) && ((pkvdata[i] > clip_val_pos) ||
            (pkvdata[i] < clip_val_neg)))   pkvmask[i] = 0.;
    }
    return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
  @brief Calculates the mean of a vector using pixel rejection.
  @param data_in           The data vector.
  @param cmethod           The method to apply:
                           - "ksigma":  At each position pixels which deviate
                                        significantly
                                        (val > mean + stdev * cpos_rej  or
                                         val < mean - stdev * cneg_rej)
                                        will be rejected. This method is
                                        iterative.
                           - "median":  At each pixel position the median is
                                        calculated. This method is applied once.
                           - "average": At each pixel position the average is
                                        calculated. This method is applied once.
                           - "min_max": The specified number of minimum and
                                        maximum pixel values will be rejected.
                                        This method is applied once.
                           - "sum":     All selected frames are added.
  @param cpos_rej      Positive rejection threshold (cmethod == "ksigma" only)
  @param cneg_rej      Negative rejection threshold (cmethod == "ksigma" only)
  @param citer         Number of iterations (cmethod == "ksigma" only)
  @param cmax          Number of maximum pixel values to reject
                       (cmethod == "min_max" only)
  @param cmin          Number of minimum pixel values to reject
                       (cmethod == "min_max" only)
  @param std_err       (Output) The standard error of @c data_in calculated
                       with rejection.
  @param default_val   The default value to return if all values have been
                       rejected.

  @return The averaged data or @c default_val in error case.

  Some notes on calculating the output noise frame:
  - If the size of @c data equals one, the input noise frame is just
    propagated. If no input noise is available, the output noise can't be
    estimated.
  - If the size of @c data equals two, the two input noise frames are combined
    applying simple error propagation (sigma = sqrt(sigma1^2 + sigma2^2) / 2).
    If no input noise is available, the output noise can't be estimated.
  - If the size of @c data is greater than two, the noise is in any case
    recalculated (sigma = stddev(data)/sqrt(n)) either if there exist or not
    input noise frames.

  Possible cpl_error_code set in this function:
  @li CPL_ERROR_NULL_INPUT if data_in is NULL
  @li CPL_ERROR_ILLEGAL_INPUT if size of data_in is smaller than 2
*/
/*----------------------------------------------------------------------------*/
static double kmos_combine_vector(
        const kmclipm_vector    *   data_in,
        const char              *   cmethod,
        double                      cpos_rej,
        double                      cneg_rej,
        int                         citer,
        int                         cmax,
        int                         cmin,
        double                  *   std_err,
        double                      default_val,
        enum combine_status     *   status)
{
    kmclipm_vector  *   data ;
    cpl_vector      *   vec_non_rej ;
    double              out_val, low_thresh, high_thresh, mean, stdev ;
    int                 i, j, nz, size, all_rejected, calc_noise ;

    /* Check inputs */
    if (data_in==NULL || cmethod==NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return -1.0 ;
    }

    /* Initialise */
    all_rejected = FALSE ;
    calc_noise = FALSE;

    nz = cpl_vector_get_size(data_in->data);
    *status  = combine_ok;
    out_val  = default_val;
    *std_err = default_val;

    data = kmclipm_vector_duplicate(data_in);
    size = kmclipm_vector_count_non_rejected(data);
    
    if (size != 0) {
        if (!strcmp(cmethod, "median")) {
            out_val  = kmclipm_vector_get_median(data, KMCLIPM_ARITHMETIC);
            calc_noise = TRUE;
        } else if (!strcmp(cmethod, "average")) {
            out_val  = kmclipm_vector_get_mean(data);
            calc_noise = TRUE;
        } else if (!strcmp(cmethod, "sum")) {
            out_val  = kmclipm_vector_get_sum(data);
            calc_noise = TRUE;
        } else if (!strcmp(cmethod, "min_max")) {
            /* get minima */
            int pos = 0;
            for (i = 0; i < cmin; i++) {
                kmclipm_vector_get_min(data, &pos);
                kmclipm_vector_reject(data, pos);
            }
            /* get maxima */
            for (i = 0; i < cmax; i++) {
                kmclipm_vector_get_max(data, &pos);
                kmclipm_vector_reject(data, pos);
            }
            /* compute the average out of the rejected values */
            out_val = kmclipm_vector_get_mean(data);
            calc_noise = TRUE;
        } else if (!strcmp(cmethod, "ksigma")) {

            /* do 80% clipping and median-rejection*/
            kmos_reject_deviant(data, cpos_rej, cneg_rej);

            /* Iterate the rejection process */
            for (i = 0; i < citer-1; i++) {
                /* Get number of identified values */
                size = kmclipm_vector_count_non_rejected(data);

                /* Not enough values left to calculate stdev exit loop */
                if (size < 2) {
                    if (size == 0)  all_rejected = TRUE;
                    break ;
                }

                /* calculate mean and stdev based on non-rejected values */
                vec_non_rej = kmclipm_vector_create_non_rejected(data) ;
                mean  = cpl_vector_get_mean(vec_non_rej);
                stdev = cpl_vector_get_stdev(vec_non_rej);
                cpl_vector_delete(vec_non_rej) ;

                low_thresh = mean - cneg_rej * stdev;
                high_thresh = mean + cpos_rej * stdev;

                /* apply threshold on identified values */
                for (j = 0; j < nz; j++) {
                    if (!kmclipm_vector_is_rejected(data, j)) {
                        if(kmclipm_vector_get(data, j, NULL)>high_thresh ||
                            kmclipm_vector_get(data, j, NULL) < low_thresh){
                            /* value has been rejected */
                            kmclipm_vector_reject(data, j);
                        }
                    }
                }
            }
            if (all_rejected == TRUE) {
                size = 0;
                out_val  = default_val;
                *status  = combine_rejected;
                *std_err = default_val;
                calc_noise = FALSE;
            } else {
                /* compute the average out of the remaining values */
                out_val = kmclipm_vector_get_mean(data);
                calc_noise = TRUE;
            }
        }
    } else {
        out_val  = default_val;
        *status  = combine_rejected;
        *std_err = default_val;
    }

    /* Compute std_err */
    if (calc_noise == TRUE) {
        size = kmclipm_vector_count_non_rejected(data);
        if (size > 2) {
            if (!strcmp(cmethod, "median")) {
                *std_err = kmclipm_vector_get_stdev_median(data) / sqrt(size) ;
            } else {
                *std_err = kmclipm_vector_get_stdev(data) / sqrt(size) ;
            }
        } else {
            /* error estimation on input data */
            if (size == 1) {
                *std_err = default_val;
            } else if (size == 2) {
                i = 0;
                while (kmclipm_vector_is_rejected(data, i)) i++;
                j = i+1;
                while (kmclipm_vector_is_rejected(data, j)) j++;

                *std_err = fabs(kmclipm_vector_get(data, i, NULL) -
                              kmclipm_vector_get(data, j, NULL)) / sqrt(2);
            }
        }
    }
    kmclipm_vector_delete(data);
    return out_val;
}

/** @} */
