/*
 * 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

/*-----------------------------------------------------------------------------
    Functions Hierarchy (o : public / x : static)

    kmos_priv_sky_tweak() (o)
        - kmos_sky_tweak_get_spectra() (x)
        - kmos_sky_tweak_correct_vibrational_trans() (x) 
            - amoeba
            - fitsky
        - kmos_sky_tweak_thermal_bgd() (x)
            - amoeba
            - fitbkd
 *----------------------------------------------------------------------------*/

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

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

#include <cpl.h>

#include "irplib_wlxcorr.h"
#include "irplib_spectrum.h"

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

#include "kmo_error.h"
#include "kmos_priv_sky_tweak.h"
#include "kmo_priv_lcorr.h"
#include "kmo_utils.h"

/*-----------------------------------------------------------------------------
 *                             Glaobal vars
 *----------------------------------------------------------------------------*/

#define HC_K 14387.7512979   // h*c/k in [micrometer*K]
#define PLANCK(lambda,t) (pow((lambda),-5.0) / (exp(HC_K/((lambda) * fabs(t))) - 1.))

int         cont_size,
            lines_size;
double  *   cont_sky_sig=NULL,
        *   line_sky_sig=NULL,
        *   cont_object_sig=NULL,
        *   line_object_sig=NULL,
        *   cont_lambda=NULL,
        *   line_lambda=NULL;

int spectrum_size;
double *spectrum_lambda=NULL;
double *spectrum_value=NULL;
double *thermal_background=NULL;

/*-----------------------------------------------------------------------------
                                Functions prototypes
 -----------------------------------------------------------------------------*/
static cpl_imagelist * kmos_priv_sky_stretch(
        cpl_imagelist           *   obj,
        cpl_imagelist           *   sky,
        float                       min_frac,
        int                         poly_degree,
        int                         resampling_method,
        int                         plot) ;
static cpl_polynomial * kmos_stretch_get_poly(
        const cpl_vector        *   obj,
        const cpl_vector        *   sky,
        double                      min_gap,
        int                         degree,
        cpl_bivector            **  matching_lines) ;
static cpl_imagelist * kmos_stretch_apply_linear(
        cpl_imagelist           *   sky, 
        cpl_polynomial          *   stretch) ;
static cpl_imagelist * kmos_stretch_apply_spline(
        cpl_imagelist           *   sky, 
        cpl_polynomial          *   stretch) ;
static int kmos_stretch_check(
        cpl_imagelist           *   obj,
        cpl_imagelist           *   sky,
        cpl_imagelist           *   new_sky,
        cpl_image               *   mask) ;
static int kmos_stretch_plot_positions_differences(
        cpl_bivector            *   matching_lines,
        cpl_bivector            *   new_matching_lines) ;
static cpl_bivector * kmos_strech_get_matching_lines(
        const cpl_vector        *   obj,
        const cpl_vector        *   sky,
        double                      min_gap,
        int                         remove_outliers,
        int                         degree) ;

static void amoeba(double**, double y[], int, double, 
        double (*funk)(double []), int *) ;
static double amotry(double **, double y[], double psum[], int, 
        double (*funk)(double []), int, double); 

static double fitsky(double *) ;
static double fitbkd(double *) ;

static int kmos_sky_tweak_identify_lines_cont(cpl_vector *, int *, int *, 
        int *, int *) ;
static void kmos_plot_arrays(double *, double *, double *, int) ;
static cpl_bivector * kmos_sky_tweak_thermal_bgd(cpl_bivector *, double) ;
static double kmos_sky_tweak_get_mean_wo_outliers(const cpl_vector *) ;
static cpl_error_code kmos_sky_tweak_get_spectra(const cpl_imagelist *, 
        const cpl_imagelist *, const cpl_vector *, const cpl_image *, 
        const int, cpl_bivector **, cpl_bivector **, int *) ;
static cpl_error_code kmos_sky_tweak_get_spectra_simple(const cpl_imagelist *, 
        const cpl_imagelist *, const cpl_image *, cpl_vector **, cpl_vector **);
static cpl_bivector * kmos_sky_tweak_correct_vibrational_trans(
        cpl_bivector *, cpl_bivector *, int, int) ;

/*----------------------------------------------------------------------------*/
/**
    @defgroup kmos_priv_sky_tweak    Sky tweaking algorithm
 */
/*----------------------------------------------------------------------------*/

/**@{*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Extract backgroung from cube and plot
  @param   	cube	Cube
  @return   
    Possible _cpl_error_code_ set in this function:
      @li CPL_ERROR_NULL_INPUT if input is NULL.
      @li CPL_ERROR_ILLEGAL_INPUT if input is incorrect.
 */
/*----------------------------------------------------------------------------*/
int kmos_plot_cube_background(cpl_imagelist * obj)
{
    cpl_image       *   mask ;
    cpl_vector      *   obj_spec ;
    cpl_vector      *   sky_spec ;

    /* Check inputs */
    if (obj == NULL) return -1 ;

    /* mask is a binary image : 0 -> obj / 1 -> background  */
    mask = kmo_lcorr_create_object_mask(obj, .3, NULL, NULL);

    /* Get the background spectrum from obj and sky cubes using mask */
    if (kmos_sky_tweak_get_spectra_simple(obj, obj, mask, &obj_spec,
                &sky_spec) != CPL_ERROR_NONE) {
        cpl_msg_error(cpl_func, "Cannot extract the spectra from cubes") ;
        cpl_image_delete(mask) ;
        return -1 ;
    }

    cpl_plot_vector("set grid;set xlabel 'pix';", "t 'CUBE BGD' w lines", "", 
            obj_spec);

    cpl_vector_delete(obj_spec) ;
    cpl_vector_delete(sky_spec) ;
    return 0 ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Main sky tweaking function
  @param    object  Object cube
  @param    sky     Sky cube
  @param    header  NAXIS3 / CRVAL3 / CDELT3 / CRPIX3
  @param    min_frac    Percentage of pixels kept for the background
  @param    tbsub   Subtract thermal background from new obj cube
  @param    ifu_nr  The IFU NR to process
  @param    skip_last
  @param    stretch the sky
  @param    stretch_degree : polynomial degree
  @param    stretch_resampling : 1:linear, 2:spline
  @param    plot        
  @param    new_sky
  @return   The new sky-corrected obj cube
    Possible _cpl_error_code_ set in this function:
      @li CPL_ERROR_NULL_INPUT if input is NULL.
      @li CPL_ERROR_ILLEGAL_INPUT if input is incorrect.
 */
/*----------------------------------------------------------------------------*/
cpl_imagelist * kmos_priv_sky_tweak(
        cpl_imagelist           *   obj, 
        cpl_imagelist           *   sky,
        const cpl_propertylist  *   header, 
        float                       min_frac, 
        int                         tbsub,
        int                         skip_last,
        int                         stretch,
        int                         stretch_degree,
        int                         stretch_resampling,
        int                         plot,
        cpl_imagelist           **  new_sky) 
{
    cpl_imagelist   *   result ;
    cpl_image       *   mask ;
    cpl_bivector    *   obj_spec = NULL;
    cpl_bivector    *   sky_spec = NULL;
    cpl_bivector    *   thermal_backg ;
    cpl_bivector    *   vscales ;
    cpl_vector      *   lambda ;
    int                 nx, ny, nz, ix, kx, snx, sny, snz, kmax, first_nonan ;

    /* Check Entries */
    if (obj == NULL || sky==NULL || header==NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return NULL ;
    }
    nx = cpl_image_get_size_x(cpl_imagelist_get_const(obj, 0));
    ny = cpl_image_get_size_y(cpl_imagelist_get_const(obj, 0));
    nz = cpl_imagelist_get_size(obj);
    snx = cpl_image_get_size_x(cpl_imagelist_get_const(sky, 0));
    sny = cpl_image_get_size_y(cpl_imagelist_get_const(sky, 0));
    snz = cpl_imagelist_get_size(sky);
    if (nx!=snx || ny!=sny || nz!=snz) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return NULL ;
    }

    /* Start by applying the sky stretching if wished */
    if (stretch) {
        *new_sky = kmos_priv_sky_stretch(obj, sky, .3, stretch_degree, 
                stretch_resampling, plot) ;
    } else {
        *new_sky = cpl_imagelist_duplicate(sky) ;
    }

    /* Create wavelengths vector corresponding to the input cubes */
    lambda = kmo_lcorr_create_lambda_vector(header);
    if (nz != cpl_vector_get_size(lambda)) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return NULL ;
    }

    /* mask is a binary image : 0 -> obj / 1 -> background  */
    mask = kmo_lcorr_create_object_mask(obj, min_frac, NULL, NULL);
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) 
        cpl_image_save(mask, "mask.fits", CPL_TYPE_DOUBLE, NULL, CPL_IO_CREATE);

    /* Get the background spectrum (using mask) from obj and sky cubes */
    kmos_sky_tweak_get_spectra(obj, *new_sky, lambda, mask, 1, 
            &obj_spec, &sky_spec, &first_nonan);
   
    /* Get the corrections factors for each sub-band */
    vscales = kmos_sky_tweak_correct_vibrational_trans(obj_spec,
                sky_spec, skip_last, plot);
    if (cpl_msg_get_level() == CPL_MSG_DEBUG && plot) 
        cpl_plot_bivector("set title \"SCALES\";", "w lines", "",vscales);

    /* Get the thermal bgd */
    thermal_backg = kmos_sky_tweak_thermal_bgd(obj_spec, 1.0);

    /* Re-try if the computation failed (PIPE-5528) */
    if (thermal_backg == NULL) {
        cpl_error_reset() ;
        cpl_msg_indent_more() ;
        cpl_msg_warning(__func__, 
                "Thermal Bgd failed, re-try by clipping highest values") ;
        thermal_backg = kmos_sky_tweak_thermal_bgd(obj_spec, 0.98);
        if (thermal_backg == NULL) {
            cpl_msg_warning(__func__, 
                    "Recovery failed - skip the thermal background correction");
        } else {
            cpl_msg_info(__func__, "Recovery succeeded");
        }
        cpl_msg_indent_less() ;
    }

    if (cpl_msg_get_level() == CPL_MSG_DEBUG && plot) 
        cpl_plot_bivector("set title \"Th. bgd\";","w lines", "",thermal_backg);

    /* Correct using the computed scales */
    result = cpl_imagelist_duplicate(obj);
    kmax = cpl_vector_get_size(cpl_bivector_get_x_const(obj_spec));
    kx = 0;
    for (ix=0; ix<nz; ix++) {
        if ((kx < kmax ) && (cpl_vector_get(lambda,ix)==cpl_vector_get(
                        cpl_bivector_get_x_const(obj_spec),kx))) {
            /* Subtract thermal background from new sky cube */
            if (thermal_backg != NULL) {
                cpl_image_subtract_scalar(cpl_imagelist_get(*new_sky ,ix),
                        cpl_vector_get(cpl_bivector_get_y_const(thermal_backg),
                            kx));
            }
            
            /* Multiply new sky cube with scaling factors from  */
            /* OH line fitting */
            cpl_image_multiply_scalar(cpl_imagelist_get(*new_sky, ix),
                    cpl_vector_get(cpl_bivector_get_y_const(vscales), kx));
            /* Subtract new sky cube from obj cube copy to  */
            /* get new obj cube */
            cpl_image_subtract(cpl_imagelist_get(result, ix), 
                    cpl_imagelist_get(*new_sky, ix));

            /* Subtract thermal background from new obj cube as well */
            if (tbsub && thermal_backg != NULL) {
                cpl_image_subtract_scalar(cpl_imagelist_get(result, ix),
                        cpl_vector_get(cpl_bivector_get_y_const(
                                thermal_backg), kx));
            }
            kx++;
        }
    }
    cpl_bivector_delete(obj_spec);
    cpl_bivector_delete(sky_spec);
    if (thermal_backg != NULL) cpl_bivector_delete(thermal_backg);
    cpl_bivector_delete(vscales);
    cpl_vector_delete(lambda);

    cpl_image_delete(mask);

    return result;
}

/** @} */

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static void kmos_plot_arrays(
        double  *   wl,
        double  *   obj,
        double  *   sky,
        int         arr_size)
{
    cpl_vector      **  vectors ;

    /* Checks */
    if (obj == NULL || sky == NULL) return ;

    /* Create Vectors */
    vectors = cpl_malloc(3*sizeof(cpl_vector*)) ;
    vectors[0] = cpl_vector_wrap(arr_size, wl) ;
    vectors[1] = cpl_vector_wrap(arr_size, obj) ;
    vectors[2] = cpl_vector_wrap(arr_size, sky) ;

    CPL_DIAG_PRAGMA_PUSH_IGN(-Wcast-qual);
    cpl_plot_vectors("set grid;set xlabel 'Wavelength (microns)';",
            "t 'Obj and Sky sub-band' w lines", "", 
            (const cpl_vector **)vectors,
			3);
    CPL_DIAG_PRAGMA_POP;

    cpl_vector_unwrap(vectors[0]) ;
    cpl_vector_unwrap(vectors[1]) ;
    cpl_vector_unwrap(vectors[2]) ;
   cpl_free(vectors) ;
   return ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static cpl_bivector * kmos_sky_tweak_thermal_bgd(
        cpl_bivector    *   spectrum,
        double              clip_rate)
{
    cpl_bivector    *   result ;
    cpl_vector      *   spectrum_v ;
    cpl_vector      *   thermal_v ;
    cpl_vector      *   tmp_v ;
    cpl_vector      *   sorted_y ;
    const int           ndim = 3;
    const int           nsimplex = ndim + 1;
    double          **  p ;
    double              p_init[ndim+1];
    double          *   lspectrum = NULL,
                    *   vspectrum = NULL;
    double          *   x_amoeba ;
    double          *   y_amoeba ;
    int                 i, j, ix, loop, niter, new_size, skip, input_size ;
    double              min, max, tmp, diff, limit, clip_limit ;

    /* Check entries */
    if (spectrum == NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return NULL ;
    }
    if (clip_rate < 0.5 || clip_rate > 1.0) {
        cpl_msg_error(__func__, "Invalid clip rate: %g", clip_rate) ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return NULL ;
    }

    /* Initialise */
    min = +1.e30;
    max = -1.e30;
    niter = 0 ;
    new_size = 0;
    skip = 11;
    input_size = cpl_bivector_get_size(spectrum) ;

    lspectrum = cpl_bivector_get_x_data(spectrum);
    vspectrum = cpl_bivector_get_y_data(spectrum);
    spectrum_lambda=cpl_malloc(input_size*sizeof(double));
    spectrum_value=cpl_malloc(input_size* sizeof(double));
    thermal_background= cpl_malloc(input_size*sizeof(double));

    /* Get rid off spikes - skip highest values in the first iteration */ 
    if (clip_rate < 1.0) {
        sorted_y = cpl_vector_duplicate(cpl_bivector_get_y(spectrum));
        cpl_vector_sort(sorted_y,  CPL_SORT_ASCENDING);
        clip_limit = cpl_vector_get(sorted_y, 
                cpl_vector_get_size(sorted_y) * clip_rate);
        cpl_vector_delete(sorted_y) ;
    } else {
        clip_limit = +1.e30;
    }

    /* Move to the first non-zero value */
    for (ix=0; ix < input_size ; ix++) if (vspectrum[ix] != 0.0) break;
    /* Store the following non-zero/nan/inf values  */
    /* Also omit the clipped ones - Store the min of the retained values */
    /* Skip the 11 first values */
    for (i=ix+skip; i<input_size; i++) {
        if ((vspectrum[i] != 0.0) && (kmclipm_is_nan_or_inf(vspectrum[i])==0) 
                && (vspectrum[i] <= clip_limit) ) {
            spectrum_lambda[new_size] = lspectrum[i];
            spectrum_value[new_size] = vspectrum[i];
            if (vspectrum[i] < min) { min = vspectrum[i]; }
            new_size++;
        }
    }
    /* Skip the last 0.0 values if any */
    for (ix=new_size; ix >= 0; ix--) if (vspectrum[ix] != 0.0) break;
    /* Skip the 11 last values */
    spectrum_size = ix-skip;

    y_amoeba=cpl_malloc((nsimplex+1)*sizeof(double));
    x_amoeba=cpl_malloc((ndim+1)*sizeof(double));
    p = matrix(nsimplex+1,ndim+1);

    p_init[1] = min;
    p_init[2] = spectrum_value[spectrum_size-1];
    p_init[3] = 280.;
    p[1][1] = p_init[1];
    p[1][2] = p_init[2];
    p[1][3] = p_init[3];

    for (loop=0; loop<20; loop++) {
        /*
        printf("p initial : %d %g %g %g\n", 
                loop, p[1][1], p[1][2], p[1][3]) ;
        */
        for (i=2; i<nsimplex+1; i++) {
            for (j=1; j<ndim+1; j++) p[i][j] = p[1][j];
        }
        for (i=2; i<nsimplex+1; i++) p[i][i-1] = p[i][i-1] * 1.2;
        for (i=1; i<nsimplex+1; i++) {
            for (j=1; j<ndim+1; j++) x_amoeba[j] = p[i][j];
            y_amoeba[i] = fitbkd(x_amoeba);
        }
        amoeba(p, y_amoeba, 3, 1.e-5, fitbkd, &niter  );

        spectrum_v = cpl_vector_wrap(spectrum_size, spectrum_value);
        thermal_v = cpl_vector_wrap(spectrum_size, thermal_background);
        tmp_v =  cpl_vector_duplicate(spectrum_v);
        cpl_vector_subtract(tmp_v, thermal_v);
        limit = cpl_vector_get_median(tmp_v) + 2. * cpl_vector_get_stdev(tmp_v);
        cpl_vector_delete(tmp_v);
        cpl_vector_unwrap(spectrum_v);
        cpl_vector_unwrap(thermal_v);

        min = +1.e30;
        new_size = 0;
        for (i=0; i<spectrum_size; i++) {
            diff = spectrum_value[i] - thermal_background[i];
            if (diff < limit) {
                spectrum_lambda[new_size] = spectrum_lambda[i];
                spectrum_value[new_size] = spectrum_value[i];
                if (spectrum_value[i] < min) min = spectrum_value[i];
                new_size++;
            }
        }
        spectrum_size = new_size;
    }

    for (i=0; i<spectrum_size; i++) {
        tmp = PLANCK(spectrum_lambda[i], p[1][3]);
        if (tmp > max) max = tmp;
    }
    if (fabs(max) < 1e-20) {
        cpl_msg_error(__func__, "Cannot determine thermal Background") ;
        free_matrix(p, 5);
        cpl_free(x_amoeba); 
        cpl_free(y_amoeba);
        cpl_free(spectrum_lambda);
        cpl_free(spectrum_value);
        cpl_free(thermal_background);
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return NULL ;
    }

    for (i=0; i<input_size; i++) {
        spectrum_lambda[i] = lspectrum[i];
        spectrum_value[i] = vspectrum[i];
        tmp = PLANCK(spectrum_lambda[i], p[1][3]);
        thermal_background[i] = p[1][1] + tmp / max * p[1][2];
    }
    thermal_v = cpl_vector_wrap(input_size, thermal_background);
    tmp_v =  cpl_vector_duplicate(thermal_v);
    cpl_vector_unwrap(thermal_v);

    result = cpl_bivector_wrap_vectors(
            cpl_vector_duplicate(cpl_bivector_get_x_const(spectrum)), tmp_v);

    free_matrix(p, 5);
    cpl_free(x_amoeba); 
    cpl_free(y_amoeba);
    cpl_free(spectrum_lambda);
    cpl_free(spectrum_value);
    cpl_free(thermal_background);
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static double kmos_sky_tweak_get_mean_wo_outliers(const cpl_vector * vdata) 
{
    cpl_vector      *   tmpv1 ;
    cpl_vector      *   tmpv2 ;
    cpl_vector      *   tmpv3 ;
    cpl_vector      *   tmpv4 ;
    const double    *   data ;
    double          *   tmpv1_data;
    double              avg, median, stdev, clip;
    int                 nr_data, i, nr_i;

    nr_data = cpl_vector_get_size(vdata);
    data = cpl_vector_get_data_const(vdata);
    tmpv1 = cpl_vector_new(nr_data);
    tmpv1_data = cpl_vector_get_data(tmpv1);

    median = cpl_vector_get_median_const(vdata);
    for (i=0; i<nr_data; i++) tmpv1_data[i] = fabs(data[i] - median);
    cpl_vector_sort(tmpv1, CPL_SORT_ASCENDING);
    clip = cpl_vector_get(tmpv1, (int) .8*nr_data);
    tmpv2 = kmo_idl_where(tmpv1, clip*5., le);
    tmpv3 = kmo_idl_values_at_indices(vdata, tmpv2);
    median = cpl_vector_get_median_const(tmpv3);
    stdev = cpl_vector_get_stdev(tmpv3);
    nr_i = 0;
    for (i=0; i<nr_data; i++) {
        if ((data[i] < median + 3. * stdev) && (data[i] > median - 3. * stdev)){
            tmpv1_data[nr_i] = data[i];
            nr_i++;
        }
    }
    tmpv4 = cpl_vector_wrap(nr_i, tmpv1_data);
    avg = cpl_vector_get_mean(tmpv4);

    cpl_vector_delete(tmpv1);
    cpl_vector_delete(tmpv2);
    cpl_vector_delete(tmpv3);
    cpl_vector_unwrap(tmpv4);
    return avg;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code kmos_sky_tweak_get_spectra_simple(
        const cpl_imagelist     *   object,
        const cpl_imagelist     *   sky,
        const cpl_image         *   mask,
        cpl_vector              **  obj_spec,
        cpl_vector              **  sky_spec) 
{
    cpl_vector      *   intobj ;
    cpl_vector      *   intsky ;
    const cpl_image *   oimg ;
    const cpl_image *   simg ;
    double          *   intobj_data ;
    double          *   intsky_data ;
    double          *   vo ;
    double          *   vs ;
    cpl_vector      *   ovec ;
    cpl_vector      *   svec ;
    double              mpix, opix, spix;
    int                 nx, ny, nz, snx, sny, snz, ix, iy, iz, found, 
                        m_is_rejected, o_is_rejected, s_is_rejected;

    /* Initialise */
    nx = cpl_image_get_size_x(cpl_imagelist_get_const(object, 0));
    ny = cpl_image_get_size_y(cpl_imagelist_get_const(object, 0));
    nz = cpl_imagelist_get_size(object);
    snx = cpl_image_get_size_x(cpl_imagelist_get_const(sky, 0));
    sny = cpl_image_get_size_y(cpl_imagelist_get_const(sky, 0));
    snz = cpl_imagelist_get_size(sky);

    /* Check entries */
    if (nx!=snx || ny!=sny || nz!=snz) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    /* Allocate data */
    intobj = cpl_vector_new(nz);
    intsky = cpl_vector_new(snz);
    intobj_data = cpl_vector_get_data(intobj);
    intsky_data = cpl_vector_get_data(intsky);
    vo = cpl_malloc(nx * ny * sizeof(double));
    vs = cpl_malloc(nx * ny * sizeof(double));

    /* For each image, comput the sky value from both the obj and the sky */
    for (iz = 0; iz <nz; iz++) {
        oimg = cpl_imagelist_get_const(object, iz);
        simg = cpl_imagelist_get_const(sky, iz);
        found = 0;
        for (ix = 1; ix<=nx; ix++) {
            for (iy = 1; iy<=ny; iy++) {
                mpix = cpl_image_get(mask, ix, iy, &m_is_rejected);
                opix = cpl_image_get(oimg, ix, iy, &o_is_rejected);
                spix = cpl_image_get(simg, ix, iy, &s_is_rejected);
                if ( mpix > .5 && m_is_rejected == 0 && o_is_rejected == 0 &&
                        s_is_rejected == 0 ) {
                    vo[found] = opix;
                    vs[found] = spix;
                    found++;
                }
            }
        }
        /* Only if enough values, the mean or mediaan is taken, otherwise 0.0 */
        if (found >= nx*ny/4.) {
            ovec = cpl_vector_wrap(found, vo);
            svec = cpl_vector_wrap(found, vs);

            if (found < nx*ny/2. ) {
                intobj_data[iz] = cpl_vector_get_median(ovec);
                intsky_data[iz] = cpl_vector_get_median(svec);
            } else {
                intobj_data[iz] = kmos_sky_tweak_get_mean_wo_outliers(ovec);
                intsky_data[iz] = kmos_sky_tweak_get_mean_wo_outliers(svec);
            }
            cpl_vector_unwrap(ovec);
            cpl_vector_unwrap(svec);
        } else {
            intobj_data[iz] = 0.;
            intsky_data[iz] = 0.;
        }
    }
    cpl_free(vo);
    cpl_free(vs);

    /* Return the spectra */
    *obj_spec = intobj ;
    *sky_spec = intsky ;
    return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code kmos_sky_tweak_get_spectra(
        const cpl_imagelist     *   object,
        const cpl_imagelist     *   sky,
        const cpl_vector        *   lambda,
        const cpl_image         *   mask,
        const int                   no_nans,
        cpl_bivector            **  obj_spectrum_ptr,
        cpl_bivector            **  sky_spectrum_ptr,
        int                     *   first_nonan) 
{
    cpl_bivector    *   obj_spectrum ; 
    cpl_bivector    *   sky_spectrum ; 
    cpl_vector      *   intobj ;
    cpl_vector      *   intsky ;
    cpl_vector      *   new_lambda ;
    cpl_vector      *   new_intobj ;
    cpl_vector      *   new_intsky ;
    const cpl_image *   oimg ;
    const cpl_image *   simg ;
    double          *   intobj_data ;
    double          *   intsky_data ;
    double          *   vo ;
    double          *   vs ;
    double          *   new_lambda_d ;
    double          *   new_intobj_d ;
    double          *   new_intsky_d ;
    const double    *   lambda_d ;
    cpl_vector      *   ovec ;
    cpl_vector      *   svec ;
    double              mpix, opix, spix;
    int                 nx, ny, nz, snx, sny, snz, ix, iy, iz, ni, found, 
                        nr_valid_int, m_is_rejected, o_is_rejected, 
                        s_is_rejected;

    /* Check entries */

    if (first_nonan==NULL) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    } else {
        *first_nonan = -1 ;
    }

    nx  = cpl_image_get_size_x(cpl_imagelist_get_const(object, 0));
    ny  = cpl_image_get_size_y(cpl_imagelist_get_const(object, 0));
    nz  = cpl_imagelist_get_size(object);
    snx = cpl_image_get_size_x(cpl_imagelist_get_const(sky, 0));
    sny = cpl_image_get_size_y(cpl_imagelist_get_const(sky, 0));
    snz = cpl_imagelist_get_size(sky);

    if (nx!=snx || ny!=sny || nz!=snz) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    if (nz != cpl_vector_get_size(lambda)) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    intobj = cpl_vector_new(cpl_vector_get_size(lambda));
    intsky = cpl_vector_new(cpl_vector_get_size(lambda));
    intobj_data = cpl_vector_get_data(intobj);
    intsky_data = cpl_vector_get_data(intsky);

    vo = cpl_malloc(nx * ny * sizeof(double));
    vs = cpl_malloc(nx * ny * sizeof(double));

    nr_valid_int = 0;
    for (iz = 0; iz <nz; iz++) {
        oimg = cpl_imagelist_get_const(object, iz);
        simg = cpl_imagelist_get_const(sky, iz);
        found = 0;
        for (ix = 1; ix<=nx; ix++) {
            for (iy = 1; iy<=ny; iy++) {
                mpix = cpl_image_get(mask, ix, iy, &m_is_rejected);
                opix = cpl_image_get(oimg, ix, iy, &o_is_rejected);
                spix = cpl_image_get(simg, ix, iy, &s_is_rejected);
                if ( mpix > .5 && m_is_rejected == 0 && o_is_rejected == 0 &&
                        s_is_rejected == 0 ) {
                    vo[found] = opix;
                    vs[found] = spix;
                    found++;
                }

            }
        }
        if (found >= nx*ny/4.) {
            if (*first_nonan < 0) *first_nonan = iz ;
            nr_valid_int++;
            ovec = cpl_vector_wrap(found, vo);
            svec = cpl_vector_wrap(found, vs);

            if (found < nx*ny/2. ) {
                intobj_data[iz] = cpl_vector_get_median(ovec);
                intsky_data[iz] = cpl_vector_get_median(svec);
            } else {
                intobj_data[iz] = kmos_sky_tweak_get_mean_wo_outliers(ovec);
                intsky_data[iz] = kmos_sky_tweak_get_mean_wo_outliers(svec);
            }
            cpl_vector_unwrap(ovec);
            cpl_vector_unwrap(svec);
        } else {
            if (no_nans) {
                intobj_data[iz] = 0./0.;
                intsky_data[iz] = 0./0.;
            } else {
                nr_valid_int++;
                intobj_data[iz] = 0.;
                intsky_data[iz] = 0.;
            }
        }
    }

    if (no_nans) {
        new_lambda = cpl_vector_new(nr_valid_int);
        new_intobj = cpl_vector_new(nr_valid_int);
        new_intsky = cpl_vector_new(nr_valid_int);
        new_lambda_d=cpl_vector_get_data(new_lambda);
        new_intobj_d=cpl_vector_get_data(new_intobj);
        new_intsky_d=cpl_vector_get_data(new_intsky);
        lambda_d=cpl_vector_get_data_const(lambda);

        ni = 0;
        for (iz=0 ; iz<nz; iz++) {
            if ((! isnan(intobj_data[iz])) && ni < nr_valid_int) {
                new_lambda_d[ni] = lambda_d[iz];
                new_intobj_d[ni] = intobj_data[iz];
                new_intsky_d[ni] = intsky_data[iz];
                ni++;
            }
        }
        obj_spectrum = cpl_bivector_wrap_vectors(new_lambda, new_intobj) ;
        sky_spectrum = cpl_bivector_wrap_vectors(
                cpl_vector_duplicate(new_lambda), new_intsky);

        cpl_vector_delete(intobj);
        cpl_vector_delete(intsky);
    } else {
        obj_spectrum = cpl_bivector_wrap_vectors(cpl_vector_duplicate(lambda), 
                intobj);
        sky_spectrum = cpl_bivector_wrap_vectors(cpl_vector_duplicate(lambda), 
                intsky);
    }

    cpl_free(vo);
    cpl_free(vs);
    *obj_spectrum_ptr = obj_spectrum;
    *sky_spectrum_ptr = sky_spectrum;
    return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static cpl_bivector * kmos_sky_tweak_correct_vibrational_trans(
        cpl_bivector    *   obj_spectrum,
        cpl_bivector    *   sky_spectrum,
        int                 skip_last,
        int                 plot)
{
    cpl_bivector    *   result ;
    double          *   scalings ;
    double          *   lambda_boundaries ;
    const char      **  labels ;
    const double    *   lambda ;
    const double    *   intobj ;
    const double    *   intsky ;
    double          *   array_wls ;
    double          *   array_obj ;
    double          *   array_sky ;
    double          *   line_interpolate_object ;
    double          *   line_interpolate_sky ;
    double          *   flineres; 
    cpl_vector      *   tmp_vec ;
    cpl_vector      *   sky_lines_map_vec ;
    double              median, stdev ;
    int                 i, ix, il, lx, cx, nr_lambda, nr_samples, nr_boundaries,
                        cont_start, cont_end ;

    /* Check Entries */
    if (obj_spectrum == NULL || sky_spectrum == NULL) {
        cpl_msg_error(__func__, "NULL inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return NULL;
    }
    if (cpl_bivector_get_size(obj_spectrum) != 
            cpl_bivector_get_size(sky_spectrum)) {
        cpl_msg_error(__func__, "Illegal inputs") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return NULL;
    }

    /* Initialise */
    nr_lambda = cpl_bivector_get_size(obj_spectrum);
    lambda = cpl_vector_get_data_const(cpl_bivector_get_x_const(obj_spectrum));
    intobj = cpl_vector_get_data_const(cpl_bivector_get_y_const(obj_spectrum));
    intsky = cpl_vector_get_data_const(cpl_bivector_get_y_const(sky_spectrum));
    median = stdev = 0.0 ;

    /* Wavelengths for sky and obj need to be identical */
    for (ix=0; ix<nr_lambda; ix++) {
        if (fabs(lambda[ix] - 
                    cpl_vector_get(cpl_bivector_get_x_const(sky_spectrum), ix))
                >= 1.e-7) {
            cpl_msg_error(__func__, "Illegal inputs") ;
            cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
            return NULL;
        }
    }

    /* Create lambda boundaries for the sub-bands */
    if (skip_last) {
        nr_boundaries = 20 ;
        lambda_boundaries = cpl_malloc(nr_boundaries * sizeof(double)) ;
        labels = cpl_malloc((nr_boundaries-1) * sizeof(char*)) ;
    } else {
        nr_boundaries = 21 ;
        lambda_boundaries = cpl_malloc(nr_boundaries * sizeof(double)) ;
        labels = cpl_malloc((nr_boundaries-1) * sizeof(char*)) ;
    }
    lambda_boundaries[0]  = 0.780 ;     labels[0]  = "" ;
    lambda_boundaries[1]  = 0.824 ;     labels[1]  = "" ;
    lambda_boundaries[2]  = 0.873 ;     labels[2]  = "" ;
    lambda_boundaries[3]  = 0.926 ;     labels[3]  = "" ;
    lambda_boundaries[4]  = 0.964 ;     labels[4]  = "" ;
    lambda_boundaries[5]  = 1.014 ;     labels[5]  = "4-1 transitions" ;
    lambda_boundaries[6]  = 1.067 ;     labels[6]  = "5-2 transitions" ;
    lambda_boundaries[7]  = 1.125 ;     labels[7]  = "6-3 transitions" ;
    lambda_boundaries[8]  = 1.196 ;     labels[8]  = "7-4 transitions" ;
    lambda_boundaries[9]  = 1.252 ;     labels[9]  = " 02 transitions" ;
    lambda_boundaries[10] = 1.289 ;     labels[10] = "8-5 transitions" ;
    lambda_boundaries[11] = 1.400 ;     labels[11] = "2-0 transitions" ;
    lambda_boundaries[12] = 1.472 ;     labels[12] = "3-1 transitions" ;
    lambda_boundaries[13] = 1.5543 ;    labels[13] = "4-2 transitions" ;
    lambda_boundaries[14] = 1.6356 ;    labels[14] = "5-3 transitions" ;
    lambda_boundaries[15] = 1.7253 ;    labels[15] = "6-4 transitions" ;
    lambda_boundaries[16] = 1.840 ;     labels[16] = "7-5 transitions" ;
    lambda_boundaries[17] = 1.9570 ;    labels[17] = "8-6 transitions" ;
    lambda_boundaries[18] = 2.095 ;     labels[18] = "9-7 transitions" ;
    if (skip_last) {
        lambda_boundaries[19] = 2.460 ; 
    } else {
        lambda_boundaries[19] = 2.30 ;  labels[19] = "final bit" ;
        lambda_boundaries[20] = 2.460 ;
    }

    /* Allocate scalings */
    scalings = cpl_malloc(nr_boundaries * sizeof(double)) ;
    for (ix=0; ix<nr_boundaries; ix++) scalings[ix] = 0.;

    array_wls = cpl_malloc(nr_lambda * sizeof(double)) ;
    array_obj = cpl_malloc(nr_lambda * sizeof(double)) ;
    array_sky = cpl_malloc(nr_lambda * sizeof(double)) ;

    /* Loop on the SUB BANDS */
    for (ix = 0; ix<nr_boundaries-1; ix++) {
        cpl_msg_debug(__func__, "Sub-band %2d: %s", ix+1, labels[ix]) ;
        cpl_msg_indent_more() ;

        /* Stor non-Nan values from the current sub-band in array_* */
        nr_samples = 0;
        for (il=0; il<nr_lambda; il++) {
            if ((lambda[il] >= lambda_boundaries[ix] ) &&
                (lambda[il] < lambda_boundaries[ix+1]) ) {

                if (kmclipm_is_nan_or_inf(intsky[il]) || 
                        kmclipm_is_nan_or_inf(intobj[il])) {
                    cpl_msg_indent_less() ;
                    continue ;
                }

                array_wls[nr_samples] = lambda[il];
                array_obj[nr_samples] = intobj[il];
                array_sky[nr_samples] = intsky[il];
                nr_samples++;
            }
        }
        cpl_msg_debug(__func__, "Found %d samples", nr_samples) ;

        /* Skip the sub-band if too few samples */
        if (nr_samples <= 20) {
            cpl_msg_indent_less() ;
            continue ;
        }
        if (plot==ix+1 && cpl_msg_get_level() == CPL_MSG_DEBUG) 
            kmos_plot_arrays(array_wls, array_obj, array_sky, nr_samples);

        /* Sky vector sub-band statistics */
        tmp_vec = cpl_vector_wrap(nr_samples, array_sky);
        median = cpl_vector_get_median_const(tmp_vec);
        stdev = cpl_vector_get_stdev(tmp_vec);
        cpl_vector_unwrap(tmp_vec);
        cpl_msg_debug(__func__, "Sky Stats - Med: %g Stdev: %g", median, stdev);

        /* Create a map of the sky vector lines */
        sky_lines_map_vec = cpl_vector_new(nr_samples) ;
        cpl_vector_fill(sky_lines_map_vec, 0.0) ;
        for (i=0; i<nr_samples; i++) {
            if (array_sky[i] > median+stdev)  
                cpl_vector_set(sky_lines_map_vec, i, 10.0) ;
        }
        if (plot==ix+1 && cpl_msg_get_level() == CPL_MSG_DEBUG) 
           cpl_plot_vector("set grid;set xlabel 'Samples';set ylabel 'Map';", 
               "t 'Sky lines map' w lines", "", sky_lines_map_vec);

        /* Smoothe the map to create the sky lines signal */
        tmp_vec=cpl_vector_filter_lowpass_create(sky_lines_map_vec, 
                CPL_LOWPASS_LINEAR,2);
        cpl_vector_delete(sky_lines_map_vec);
        sky_lines_map_vec = tmp_vec ;

        if (plot==ix+1 && cpl_msg_get_level() == CPL_MSG_DEBUG) 
           cpl_plot_vector("set grid;set xlabel 'Samples';set ylabel 'Map';", 
               "t 'Smoothed sky lines map' w lines", "", 
               sky_lines_map_vec);

        /* Identify the continuum and lines sizes  */
        kmos_sky_tweak_identify_lines_cont(sky_lines_map_vec,
                &cont_start, &cont_end, &cont_size, &lines_size) ;
        if (lines_size == 0) {
            cpl_msg_warning(__func__, "No line region found") ;
            cpl_vector_delete(sky_lines_map_vec);
            cpl_msg_indent_less() ;
            continue;
        }

        /* Allocate arrays to hold concatenated line and  */
        /* continuum regions */
        cont_sky_sig =      cpl_malloc(cont_size * sizeof(double));
        cont_object_sig =   cpl_malloc(cont_size * sizeof(double));
        cont_lambda =       cpl_malloc(cont_size * sizeof(double));
        line_sky_sig =      cpl_malloc(lines_size * sizeof(double));
        line_object_sig =   cpl_malloc(lines_size * sizeof(double));
        line_lambda =       cpl_malloc(lines_size * sizeof(double));

        /* Store separately Lines and Continuum */
        cx = lx = 0;
        for (i = cont_start; i <= cont_end ; i++) {
            if (cpl_vector_get(sky_lines_map_vec, i) > 0.) {
                line_sky_sig[lx] = array_sky[i];
                line_object_sig[lx] = array_obj[i];
                line_lambda[lx] = array_wls[i];
                lx++;
            } else {
                cont_sky_sig[cx] = array_sky[i];
                cont_object_sig[cx] = array_obj[i];
                cont_lambda[cx] = array_wls[i];
                cx++;
            }
        }

        /* Minimize object - sky difference (line regions -  */
        /* interpolated background) using a single factor */
        const int ndim = 1;
        const int nsimplex = ndim + 1;
        double **p = NULL;
        double *x_amoeba  = NULL,
               *y_amoeba  = NULL;
        int niter;
        int ip,jp;
        double scale;
        y_amoeba = cpl_malloc((nsimplex+1) * sizeof(double));
        x_amoeba = cpl_malloc((ndim+1) * sizeof(double));
        p = matrix(nsimplex+1,ndim+1);

        p[1][1] = 1.;
        p[2][1] = 1.25;
        for (ip=1; ip<nsimplex+1; ip++) {
            for (jp=1; jp<ndim+1; jp++) {
                x_amoeba[jp] = p[ip][jp];
            }
            y_amoeba[ip] = fitsky(x_amoeba);
        }
        amoeba(p, y_amoeba, ndim, 1.e-5, fitsky, &niter  );
        scale = p[1][1];
        flineres = cpl_malloc(lines_size * sizeof(double));

        line_interpolate_object = polynomial_irreg_irreg_nonans(
                cont_size, cont_lambda, cont_object_sig,
                lines_size, line_lambda, 2);
        line_interpolate_sky = polynomial_irreg_irreg_nonans(
                cont_size, cont_lambda, cont_sky_sig,
                lines_size, line_lambda, 2);

        for (i=0; i<lines_size; i++) {
            flineres[i] = (line_object_sig[i] - 
                    line_interpolate_object[i]) - 
                (line_sky_sig[i] - line_interpolate_sky[i]) * scale;
        }

        tmp_vec = cpl_vector_wrap(lines_size, flineres);
        median = cpl_vector_get_median_const(tmp_vec);
        stdev = cpl_vector_get_stdev(tmp_vec);
        cpl_vector_unwrap(tmp_vec) ;

        int clip_cnt=0;
        for (i=0; i<lines_size; i++) {
            if ( fabs(flineres[i] - median) <= (3 * stdev) ) {
                clip_cnt++;
            }
        }
        cpl_msg_debug(__func__, "Outliers: %d", lines_size-clip_cnt) ;

        if ((clip_cnt != lines_size) && (clip_cnt >= 3)) {
            lx = 0;
            for (i=0; i<lines_size; i++) {
                if ( fabs(flineres[i] - median) <= (3 * stdev) ) {
                    line_sky_sig[lx] = line_sky_sig[i];
                    line_object_sig[lx] = 
                        line_object_sig[i];
                    line_lambda[lx] = line_lambda[i];
                    lx++;
                }
            }
            lines_size = lx;
            cpl_msg_debug(__func__, "2. Lines size: %d", lines_size) ;

            p[1][1] = 1.;
            p[2][1] = 1.5;
            for (ip=1; ip<nsimplex+1; ip++) {
                for (jp=1; jp<ndim+1; jp++) x_amoeba[jp] = p[ip][jp];
                y_amoeba[ip] = fitsky(x_amoeba);
            }
            amoeba(p, y_amoeba, ndim, 1.e-5, fitsky, &niter);
            scale = p[1][1];
        }
        cpl_msg_debug(__func__, "Scale: %g", scale) ;
        scalings[ix] = scale;

        cpl_vector_delete(sky_lines_map_vec);
        free_matrix(p, nsimplex+1);
        cpl_free(x_amoeba);
        cpl_free(y_amoeba);
        cpl_free(cont_sky_sig);
        cpl_free(line_sky_sig);
        cpl_free(cont_object_sig); 
        cpl_free(line_object_sig); 
        cpl_free(cont_lambda);
        cpl_free(line_lambda);
        cpl_free(flineres); 
        cpl_free(line_interpolate_object); 
        cpl_free(line_interpolate_sky); 
        
        cpl_msg_indent_less() ;
    } // end for (ix = 0; ix<nr_boundaries-1; ix++)
    cpl_free(labels) ;

    // check for outliers in the scaling factors
    int nr_valid = 0,
        v_ix = 0;
    double valid_scales[nr_boundaries];
    cpl_vector *valid_scales_v = NULL;
    for (ix=0; ix<nr_boundaries; ix++) {
        if (scalings[ix] != 0.0 ) {
            valid_scales[nr_valid] = scalings[ix];
            nr_valid++;
        }
    }
    valid_scales_v = cpl_vector_wrap(nr_valid, valid_scales);
    median = cpl_vector_get_median_const(valid_scales_v);
    stdev = cpl_vector_get_stdev(valid_scales_v);
    v_ix = 0;
    for (ix=0; ix<nr_boundaries; ix++) {
        if (scalings[ix] != 0.0 ) {
            if (fabs(valid_scales[v_ix] - median) > 2 * stdev) {
                scalings[ix] = 0.;
            }
            v_ix++;
        }
    }
    if (valid_scales_v != NULL) {
        cpl_vector_unwrap(valid_scales_v); 
        valid_scales_v = NULL; 
    }

    int scale0_size;
    double *scale0_lambda = NULL,
           *scale0_value = NULL,
           *scale = NULL;
    cpl_vector *scale_v = NULL;

    scale0_lambda = cpl_malloc(nr_lambda * sizeof(double));
    scale0_value = cpl_malloc(nr_lambda * sizeof(double));

    scale0_size = 0;
    for (il=0; il<nr_lambda; il++) {
        for (ix = 0; ix<nr_boundaries-1; ix++) {
            if (scalings[ix] != 0.) {
                if ((lambda[il] >= lambda_boundaries[ix] ) &&
                        (lambda[il] < lambda_boundaries[ix+1]) ) {
                    scale0_lambda[scale0_size] = lambda[il];
                    scale0_value[scale0_size] = scalings[ix];
                    scale0_size++;
                }
            }
        }
    }
    cpl_free(scalings) ;
    cpl_free(lambda_boundaries) ;

    scale = polynomial_irreg_irreg_nonans(scale0_size, scale0_lambda, 
            scale0_value, nr_lambda, lambda, 1);

    scale_v = cpl_vector_wrap(nr_lambda, scale);

    if (scale0_lambda != NULL) cpl_free(scale0_lambda); 
    if (scale0_value != NULL) cpl_free(scale0_value); 
    result = cpl_bivector_wrap_vectors(
            cpl_vector_duplicate(cpl_bivector_get_x_const(obj_spectrum)),
            scale_v);

    cpl_free(array_wls) ;
    cpl_free(array_obj) ;
    cpl_free(array_sky) ;

    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static int kmos_sky_tweak_identify_lines_cont(
        cpl_vector  *   sky_lines_map_vec,
        int         *   cont_start, 
        int         *   cont_end, 
        int         *   local_cont_size,
        int         *   local_lines_size)
{
    int     current_line_size, nr_samples, i ;

	/* local_ prefix added to avoid conflict with global variables */

    /* Initialise */
    current_line_size = 0 ;
    nr_samples = cpl_vector_get_size(sky_lines_map_vec) ;

    /* Make sure cont/line regions start and end with a contin. reg */
    *cont_start = -1;
    *cont_end = -1;
    *local_cont_size = 0;
    *local_lines_size = 0;
    for (i=0; i<nr_samples; i++) {
        if (cpl_vector_get(sky_lines_map_vec, i) > 0.) {
            /*** Line ***/
            /* Ignore Lines at the start */
            if (*cont_start == -1) continue;  
            current_line_size++;
        } else {
            /*** Continuum ***/
            if (*cont_start == -1) *cont_start = i;
            *cont_end = i;
            (*local_cont_size)++;
            *local_lines_size += current_line_size;
            current_line_size = 0;
        }
    }
    cpl_msg_debug(__func__, "Cont [%d, %d] size: %d / Lines size: %d",
            *cont_start, *cont_end, *local_cont_size, *local_lines_size) ;
    return 0 ;
}

static double fitsky(double *p) {
    double result = 0.;
    double *cont_sky = NULL,
           *cont_object = NULL;
    double avg = 0.;
    int ix;

    cont_sky = polynomial_irreg_irreg_nonans(cont_size, cont_lambda,
            cont_sky_sig, lines_size, line_lambda, 1);
    cont_object = polynomial_irreg_irreg_nonans(cont_size, cont_lambda,
            cont_object_sig, lines_size, line_lambda, 1);
    for (ix=0; ix<lines_size; ix++) {
        double diff = (line_object_sig[ix] - cont_object[ix]) -
                      (line_sky_sig[ix] - cont_sky[ix]) * p[1];
        avg += diff * diff;
    }
    avg /= lines_size;
    result = sqrt(avg);

    if (cont_sky != NULL)       free_vector(cont_sky); 
    if (cont_object != NULL)    free_vector(cont_object); 

    return result;
}

static double fitbkd(double *p) {
    double result = 0.;
    double *tmp = NULL;
    double max    = 0.;
    int i = 0;

    tmp = cpl_malloc(spectrum_size * sizeof(double));

    max = -1.;
    for (i=0; i<spectrum_size; i++) {
        tmp[i] = PLANCK(spectrum_lambda[i], p[3]);
        if (tmp[i] > max) {
            max = tmp[i];
        }
    }
    p[2]=fabs(p[2]);  // make sure scaling factor is positive
    if (max > 0.) {
        for (i=0; i<spectrum_size; i++) {
            thermal_background[i] = p[1] + tmp[i] / max * fabs(p[2]);
        }
    } else {
        for (i=0; i<spectrum_size; i++) {
            thermal_background[i] = tmp[i];
        }
    }

    result = 0.;
    for (i=0; i<spectrum_size; i++) {
        result += (spectrum_value[i] - thermal_background[i]) *
                  (spectrum_value[i] - thermal_background[i]);
    }

    if (tmp != NULL) cpl_free(tmp);
    return result;
}



/* !!!!!!!!! UNDER LICENSE !!!!!!! */
#define GET_PSUM \
                  for (j=1;j<=ndim;j++) {\
                       for (sum=0.0,i=1;i<=mpts;i++) sum += p[i][j];\
                       psum[j]=sum;}
#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
static void amoeba(
        double  **  p, 
        double      y[], 
        int         ndim, 
        double      ftol, 
        double (*funk)(double []), 
        int     *   nfunk)
{
    int     i,ihi,ilo,inhi,j,mpts=ndim+1;
    double  rtol, sum, swap, ysave, ytry, *psum = NULL, d;

    psum=vector(ndim+1);
    *nfunk=0;
    GET_PSUM
    for (;;) {
        ilo=1;
        ihi = y[1]>y[2] ? (inhi=2,1) : (inhi=1,2);
        for (i=1;i<=mpts;i++) {
            if (y[i] <= y[ilo]) ilo=i;
            if (y[i] > y[ihi]) {
                inhi=ihi;
                ihi=i;
            } else if (y[i] > y[inhi] && i != ihi) inhi=i;
        }
        d = fabs(y[ihi])+fabs(y[ilo]);
        if (d == 0.0) {
            rtol = ftol / 2. ; //succeeds next if statement -> breaks loop
        } else {
            rtol=2.0*fabs(y[ihi]-y[ilo])/d;
        }
        if (rtol < ftol) {
            SWAP(y[1],y[ilo])
                for (i=1;i<=ndim;i++) SWAP(p[1][i],p[ilo][i])
                    break;
            }
        if (*nfunk >= 5000) printf("5000 exceeded\n");
        *nfunk += 2;
        ytry=amotry(p,y,psum,ndim,funk,ihi,-1.0);
        if (ytry <= y[ilo])
            ytry=amotry(p,y,psum,ndim,funk,ihi,2.0);
        else if (ytry >= y[inhi]) {
            ysave=y[ihi];
            ytry=amotry(p,y,psum,ndim,funk,ihi,0.5);
            if (ytry >= ysave) {
                for (i=1;i<=mpts;i++) {
                    if (i != ilo) {
                        for (j=1;j<=ndim;j++)
                            p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
                        y[i]=(*funk)(psum);
                    }
                }
                *nfunk += ndim;
                GET_PSUM
            }
        } else --(*nfunk);
    }
    free_vector(psum);
}
#undef SWAP
#undef GET_PSUM

static double amotry(
        double  **  p, 
        double      y[], 
        double      psum[], 
        int         ndim,
        double (*funk)(double []), 
        int         ihi, 
        double      fac)
{
    int j;
    double fac1,fac2,ytry,*ptry=NULL;

    ptry=vector(ndim+1);
    fac1=(1.0-fac)/ndim;
    fac2=fac1-fac;
    for (j=1;j<=ndim;j++) ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
    ytry=(*funk)(ptry);
    if (ytry < y[ihi]) {
        y[ihi]=ytry;
        for (j=1;j<=ndim;j++) {
            psum[j] += ptry[j]-p[ihi][j];
            p[ihi][j]=ptry[j];
        }
    }
    free_vector(ptry);
    return ytry;
}
/* !!!!!!!!! END UNDER LICENSE !!!!!!! */

/* *************************************************************************  */
/* ****************************** KMOS STRETCH *****************************  */
/* *************************************************************************  */

/*----------------------------------------------------------------------------*/
/**
  @brief    Ѕtretch the sky cube to align the sky lines the object sky lines
  @param    obj     Object cube
  @param    sky     Sky cube
  @param    min_frac
  @param    poly_degree
  @param    resampling_method   1: linear, 2: spline
  @param    plot    Flag to plot
  @return   The stretched sky cube - Needs to be deallocated
  The function extracts the sky (using mask) from both the obj and the
  sky cubes. It stretches the sky spectrum to align it to the obj sky
  spectrum.
    Possible _cpl_error_code_ set in this function:
      @li CPL_ERROR_NULL_INPUT if input is NULL.
      @li CPL_ERROR_ILLEGAL_INPUT if input is incorrect.
 */
/*----------------------------------------------------------------------------*/
static cpl_imagelist * kmos_priv_sky_stretch(
        cpl_imagelist           *   obj, 
        cpl_imagelist           *   sky,
        float                       min_frac,
        int                         poly_degree,
        int                         resampling_method,
        int                         plot)
{
    cpl_vector      *   obj_spec ;
    cpl_vector      *   sky_spec ;
    cpl_imagelist   *   new_sky ;
    cpl_image       *   mask ;
    cpl_polynomial  *   transf_poly ;
    cpl_polynomial  *   new_transf_poly ;
    cpl_bivector    *   matching_lines ;
    cpl_bivector    *   new_matching_lines ;
    
    /* Check inputs */
    if (obj == NULL || sky == NULL) return NULL ;
    if (resampling_method != 1 && resampling_method != 2) return NULL ;

    /* mask is a binary image : 0 -> obj / 1 -> background  */
    mask = kmo_lcorr_create_object_mask(obj, min_frac, NULL, NULL);

    /* Get the background spectrum from obj and sky cubes using mask */
    if (kmos_sky_tweak_get_spectra_simple(obj, sky, mask, &obj_spec, 
                &sky_spec) != CPL_ERROR_NONE) {
        cpl_msg_error(cpl_func, "Cannot extract the spectra from cubes") ;
        cpl_image_delete(mask) ;
        return NULL ;
    }

    /* Create the transformation poynomial */
    if ((transf_poly = kmos_stretch_get_poly(obj_spec, sky_spec, 5, poly_degree,
                    &matching_lines))==NULL) {
        cpl_msg_error(cpl_func, "Cannot compute the transformation polynomial");
        cpl_vector_delete(obj_spec) ;
        cpl_vector_delete(sky_spec) ;
        cpl_image_delete(mask) ;
        return NULL ;
    }
    cpl_vector_delete(obj_spec) ;
    cpl_vector_delete(sky_spec) ;

    /* Apply the sky cube transformation */
    if (resampling_method == 1) 
        new_sky = kmos_stretch_apply_linear(sky, transf_poly) ;
    else 
        new_sky = kmos_stretch_apply_spline(sky, transf_poly) ;
    if (new_sky == NULL) {
        cpl_msg_error(cpl_func, "Cannot apply the transformation");
        cpl_polynomial_delete(transf_poly) ;
        cpl_image_delete(mask) ;
        return NULL ;
    }
    
    /* FOR DEBUG PUPOSE - Check the new sky */
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) {
        if (kmos_sky_tweak_get_spectra_simple(obj, new_sky, mask, &obj_spec, 
                    &sky_spec) != CPL_ERROR_NONE) {
            cpl_msg_error(cpl_func, "Cannot extract the spectra from cubes") ;
            cpl_image_delete(mask) ;
            cpl_bivector_delete(matching_lines) ;
            cpl_polynomial_delete(transf_poly) ;
            cpl_imagelist_delete(new_sky) ;
            return NULL ;
        }
        if ((new_transf_poly = kmos_stretch_get_poly(obj_spec, sky_spec, 5, 
                        poly_degree, &new_matching_lines))==NULL) {
            cpl_msg_error(cpl_func, "Cannot compute the transf. polynomial");
            cpl_vector_delete(obj_spec) ;
            cpl_vector_delete(sky_spec) ;
            cpl_image_delete(mask) ;
            cpl_bivector_delete(matching_lines) ;
            cpl_polynomial_delete(transf_poly) ;
            cpl_imagelist_delete(new_sky) ;
            return NULL ;
        }
        cpl_vector_delete(obj_spec) ;
        cpl_vector_delete(sky_spec) ;

        cpl_polynomial_dump(transf_poly, stdout) ;
        /* cpl_polynomial_dump(new_transf_poly, stdout) ; */

        if (plot) {
            kmos_stretch_plot_positions_differences(matching_lines,
                    new_matching_lines) ;
            kmos_stretch_check(obj, sky, new_sky, mask) ;
        }
        cpl_bivector_delete(new_matching_lines) ;
        cpl_polynomial_delete(new_transf_poly) ;
    }

    cpl_bivector_delete(matching_lines) ;
    cpl_polynomial_delete(transf_poly) ;
    cpl_image_delete(mask) ;
   
    return new_sky ;
}

static cpl_polynomial * kmos_stretch_get_poly(
        const cpl_vector        *   obj,
        const cpl_vector        *   sky,
        double                      min_gap,
        int                         degree,
        cpl_bivector            **  matching_lines)
{
    cpl_size            deg_loc ;
    cpl_size            nb_lines ;
    cpl_matrix      *   matchedx ;
    cpl_polynomial  *   fitted ;

    /* Check entries */
    if (obj == NULL || sky == NULL || matching_lines == NULL) return NULL ;

    /* Initialise */
    deg_loc = (cpl_size)degree ;

    /* Detect Matching lines */
    *matching_lines=kmos_strech_get_matching_lines(obj, sky, min_gap, 1,degree);
    nb_lines = cpl_bivector_get_size(*matching_lines) ;

    /* Compute the fit */
    matchedx = cpl_matrix_wrap(1, nb_lines, 
            cpl_vector_get_data(cpl_bivector_get_x(*matching_lines)));
    fitted = cpl_polynomial_new(1);
    if (cpl_polynomial_fit(fitted, matchedx, NULL, 
                cpl_bivector_get_y(*matching_lines), NULL, CPL_FALSE, 
                NULL, &deg_loc) != CPL_ERROR_NONE) {
        cpl_msg_error(cpl_func, "Cannot fit the polynomial") ;
        cpl_polynomial_delete(fitted);
        cpl_matrix_unwrap(matchedx);
        cpl_bivector_delete(*matching_lines) ;
        return NULL ;
    }
    cpl_matrix_unwrap(matchedx);
    return fitted ;
}

/* TODO */
/*----------------------------------------------------------------------------*/
/**
  @brief   Apply the stretching to a cube
  @param    in          Image list to stretch
  @param    stretch     Stretching polynomial
  @return   The stretched cube
 */
/*----------------------------------------------------------------------------*/
static cpl_imagelist * kmos_stretch_apply_spline(
        cpl_imagelist   *   in, 
        cpl_polynomial  *   stretch)
{
    cpl_imagelist   *   out ;
    cpl_vector      **  zlines ;
    cpl_vector      **  resampled_zlines ;
    float           *   image_data ;
    cpl_vector      *   pos ;
    double          *   ppos ;
    double          *   resampled_array ;
    cpl_vector      *   new_pos ;
    int                 spec_size, nx, ny, i, j, k ;

    /* Initialize */
    spec_size = cpl_imagelist_get_size(in) ;
    nx = cpl_image_get_size_x(cpl_imagelist_get_const(in, 0)) ;
    ny = cpl_image_get_size_x(cpl_imagelist_get_const(in, 0)) ;

    /* Create the z vectors */
    zlines = cpl_malloc(nx*ny*sizeof(cpl_vector*)) ;
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            zlines[i+j*nx] = cpl_vector_new(spec_size) ;

    /* Fill the z vectors */
    for (k=0 ; k<spec_size ; k++) {
        image_data = cpl_image_get_data_float(cpl_imagelist_get(in, k)) ;
        for (i=0 ; i<nx ; i++)
            for (j=0 ; j<ny ; j++) 
                if (!isnan(image_data[i+j*nx])) 
                    cpl_vector_set(zlines[i+j*nx], k, image_data[i+j*nx]) ;
                else 
                    cpl_vector_set(zlines[i+j*nx], k, 0.0) ;
    }

    /* Create the positions vector */
    pos = cpl_vector_new(spec_size) ;
    ppos = cpl_vector_get_data(pos);
    for (i=0 ; i<spec_size ; i++) ppos[i] = i+1 ;

    /* Create the new positions vector */
    new_pos = cpl_vector_new(spec_size) ;
    cpl_vector_fill_polynomial(new_pos, stretch, 1, 1) ;

    /* Extend the lambdas for the border effects */
    if (cpl_vector_get(new_pos, 0) < 1.0) cpl_vector_set(new_pos, 0, 1.0) ;
    if (cpl_vector_get(new_pos, spec_size-1) > spec_size-1)
        cpl_vector_set(new_pos, spec_size-1, spec_size-1) ;

    resampled_zlines = cpl_malloc(nx*ny*sizeof(cpl_vector*)) ;
    /* Run the interpolations */
    for (i=0 ; i<nx ; i++) {
        for (j=0 ; j<ny ; j++) {
			resampled_array = cubicspline_irreg_irreg(spec_size,
                    ppos, cpl_vector_get_data(zlines[i+j*nx]),
                    spec_size, cpl_vector_get_data(new_pos), NATURAL) ;

			resampled_zlines[i+j*nx] = cpl_vector_wrap(spec_size, 
                resampled_array) ;
        }
    }
    cpl_vector_delete(new_pos) ;
    cpl_vector_delete(pos) ;

    /* Deallocate specs */
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            cpl_vector_delete(zlines[i+j*nx]) ;
    cpl_free(zlines) ;

    /* Fill the output cube  */
    out = cpl_imagelist_duplicate(in) ;
    for (k=0 ; k<spec_size ; k++) {
        image_data = cpl_image_get_data_float(cpl_imagelist_get(out, k)) ;
        for (i=0 ; i<nx ; i++)
            for (j=0 ; j<ny ; j++)
                image_data[i+j*nx]= cpl_vector_get(resampled_zlines[i+j*nx],k) ;
    }

    /* Deallocate resampled_specs */
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            cpl_vector_delete(resampled_zlines[i+j*nx]) ;
    cpl_free(resampled_zlines) ;
    return out ;
}


/*----------------------------------------------------------------------------*/
/**
  @brief   Apply the stretching to a cube
  @param    in          Image list to stretch
  @param    stretch     Stretching polynomial
  @return   The stretched cube
 */
/*----------------------------------------------------------------------------*/
static cpl_imagelist * kmos_stretch_apply_linear(
        cpl_imagelist   *   in, 
        cpl_polynomial  *   stretch)
{
    cpl_imagelist   *   out ;
    cpl_vector      **  zlines ;
    cpl_vector      **  resampled_zlines ;
    float           *   image_data ;
    cpl_vector      *   pos ;
    double          *   ppos ;
    cpl_vector      *   new_pos ;
    cpl_bivector    *   ref_spec ;
    cpl_bivector    *   resampled_spec ;
    int                 spec_size, nx, ny, i, j, k ;

    /* Initialize */
    spec_size = cpl_imagelist_get_size(in) ;
    nx = cpl_image_get_size_x(cpl_imagelist_get_const(in, 0)) ;
    ny = cpl_image_get_size_x(cpl_imagelist_get_const(in, 0)) ;

    /* Create the z vectors */
    zlines = cpl_malloc(nx*ny*sizeof(cpl_vector*)) ;
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            zlines[i+j*nx] = cpl_vector_new(spec_size) ;

    /* Fill the z vectors */
    for (k=0 ; k<spec_size ; k++) {
        image_data = cpl_image_get_data_float(cpl_imagelist_get(in, k)) ;
        for (i=0 ; i<nx ; i++)
            for (j=0 ; j<ny ; j++) 
                if (!isnan(image_data[i+j*nx])) 
                    cpl_vector_set(zlines[i+j*nx], k, image_data[i+j*nx]) ;
                else 
                    cpl_vector_set(zlines[i+j*nx], k, 0.0) ;
    }

    /* Create the positions vector */
    pos = cpl_vector_new(spec_size) ;
    ppos = cpl_vector_get_data(pos);
    for (i=0 ; i<spec_size ; i++) ppos[i] = i+1 ;

    /* Create the new positions vector */
    new_pos = cpl_vector_new(spec_size) ;
    cpl_vector_fill_polynomial(new_pos, stretch, 1, 1) ;

    /* Extend the lambdas for the border effects */
    if (cpl_vector_get(new_pos, 0) < 1.0) cpl_vector_set(new_pos, 0, 1.0) ;
    if (cpl_vector_get(new_pos, spec_size-1) > spec_size-1)
        cpl_vector_set(new_pos, spec_size-1, spec_size-1) ;

    resampled_zlines = cpl_malloc(nx*ny*sizeof(cpl_vector*)) ;
    /* Run the interpolations */
    for (i=0 ; i<nx ; i++) {
        for (j=0 ; j<ny ; j++) {
            /* Allocate the vector holding the resampled sky */
            resampled_zlines[i+j*nx] = cpl_vector_new(spec_size) ;

            /* Wrap the bivectors */
            ref_spec = cpl_bivector_wrap_vectors(pos, zlines[i+j*nx]);
            resampled_spec = cpl_bivector_wrap_vectors(new_pos, 
                    resampled_zlines[i+j*nx]) ;
            /* Apply the resampling */
            cpl_bivector_interpolate_linear(resampled_spec, ref_spec) ;

            /* Unwrap */
            cpl_bivector_unwrap_vectors(ref_spec) ;
            cpl_bivector_unwrap_vectors(resampled_spec) ;
        }
    }
    cpl_vector_delete(new_pos) ;
    cpl_vector_delete(pos) ;

    /* Deallocate specs */
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            cpl_vector_delete(zlines[i+j*nx]) ;
    cpl_free(zlines) ;

    /* Fill the output cube  */
    out = cpl_imagelist_duplicate(in) ;
    for (k=0 ; k<spec_size ; k++) {
        image_data = cpl_image_get_data_float(cpl_imagelist_get(out, k)) ;
        for (i=0 ; i<nx ; i++)
            for (j=0 ; j<ny ; j++)
                image_data[i+j*nx]= cpl_vector_get(resampled_zlines[i+j*nx],k) ;
    }

    /* Deallocate resampled_specs */
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            cpl_vector_delete(resampled_zlines[i+j*nx]) ;
    cpl_free(resampled_zlines) ;
    return out ;
}

static int kmos_stretch_check(
        cpl_imagelist   *   obj,
        cpl_imagelist   *   sky,
        cpl_imagelist   *   new_sky,
        cpl_image       *   mask)
{
    cpl_vector      *   spec1 ;
    cpl_vector      *   spec2 ;

    kmos_sky_tweak_get_spectra_simple(obj, sky, mask, &spec1, &spec2);
    cpl_plot_vector("set grid;set xlabel 'pix';", "t 'obj' w lines", "",
            spec1);
    cpl_plot_vector("set grid;set xlabel 'pix';", 
            "t 'sky before stretching' w lines", "", spec2);
    cpl_vector_subtract(spec1, spec2) ;
   
    cpl_plot_vector("set grid;set xlabel 'pix';", 
            "t 'obj-sky before stretching' w lines", "",
            spec1);
    cpl_vector_delete(spec1) ;
    cpl_vector_delete(spec2) ;

    kmos_sky_tweak_get_spectra_simple(obj, new_sky, mask, &spec1, &spec2);
    cpl_plot_vector("set grid;set xlabel 'pix';", 
            "t 'sky after stretching' w lines", "", spec2);
    cpl_vector_subtract(spec1, spec2) ;
   
    cpl_plot_vector("set grid;set xlabel 'pix';", 
            "t 'obj-sky after stretching' w lines", "",
            spec1);
    cpl_vector_delete(spec1) ;
    cpl_vector_delete(spec2) ;

    return 0 ;
}

static int kmos_stretch_plot_positions_differences(
        cpl_bivector    *   matching_lines, 
        cpl_bivector    *   new_matching_lines) 
{
    cpl_vector      *       diff_values ;
    cpl_bivector    *       diff ;

    /* BEFORE */
    diff_values = cpl_vector_duplicate(cpl_bivector_get_x(matching_lines)) ;
    cpl_vector_subtract(diff_values, cpl_bivector_get_y(matching_lines)) ;
    diff = cpl_bivector_wrap_vectors(cpl_bivector_get_x(matching_lines), 
            diff_values) ;
    cpl_plot_bivector("set grid;", "t 'Pos. diff. BEFORE STRECHING'", "", diff);
    cpl_bivector_unwrap_vectors(diff) ;
    cpl_vector_delete(diff_values) ;

    /* AFTER */
    diff_values = cpl_vector_duplicate(cpl_bivector_get_x(new_matching_lines)) ;
    cpl_vector_subtract(diff_values, cpl_bivector_get_y(new_matching_lines)) ;
    diff = cpl_bivector_wrap_vectors(cpl_bivector_get_x(new_matching_lines), 
            diff_values) ;
    cpl_plot_bivector("set grid;", "t 'Pos. diff. AFTER STRECHING'", "", diff);
    cpl_bivector_unwrap_vectors(diff) ;
    cpl_vector_delete(diff_values) ;

    return 0 ;
}

/* TODO */
static cpl_bivector * kmos_strech_get_matching_lines(
        const cpl_vector        *   obj,
        const cpl_vector        *   sky,
        double                      min_gap,
        int                         remove_outliers,
        int                         degree)
{
    cpl_vector      *   tmp_vec ;
    cpl_vector      *   obj_lines ;
    cpl_vector      *   obj_lines_clean ;
    double          *   pobj_lines ;
    cpl_vector      *   sky_lines ;
    cpl_vector      *   sky_lines_clean ;
    double          *   psky_lines ;
    cpl_vector      *   diff ;
    double          *   obj_lines_arr ;
    double          *   sky_lines_arr ;
    double              obj_pos, sky_pos ;
    cpl_size            sky_idx, nb_lines ;
    double              fwhm, kappa, threshold ;
    int                 i, nb_found ;

    /* Check entries */
    if (obj == NULL || sky == NULL) return NULL ;

    /* Initialise */
    fwhm = 5.0 ;
    kappa = 3 ;
  
    /* TODO */
    /* Replace by keeping the 20 brightest lines dispatched on the detector */

    /* Detect lines in obj */
    /* Pre-process before detection */
    threshold = fabs(cpl_vector_get_mean(obj) + cpl_vector_get_stdev(obj)) ;
    tmp_vec = cpl_vector_duplicate(obj) ;
    for (i=0 ; i<cpl_vector_get_size(tmp_vec) ; i++) {
        if (cpl_vector_get(tmp_vec, i) < threshold) 
            cpl_vector_set(tmp_vec, i, 0.0);
    }
    if ((obj_lines = irplib_spectrum_detect_peaks(tmp_vec, fwhm,
                    kappa, 0, NULL, NULL)) == NULL) {
        cpl_msg_error(cpl_func, "Cannot detect peaks from obj") ;
        cpl_vector_delete(tmp_vec) ;
        return NULL ;
    }
    cpl_vector_delete(tmp_vec) ;

    /* Detect lines in sky */
    /* Pre-process before detection */
    threshold = fabs(cpl_vector_get_mean(sky) + cpl_vector_get_stdev(sky)) ;
    tmp_vec = cpl_vector_duplicate(sky) ;
    for (i=0 ; i<cpl_vector_get_size(tmp_vec) ; i++) {
        if (cpl_vector_get(tmp_vec, i) < threshold) 
            cpl_vector_set(tmp_vec, i, 0.0);
    }
    if ((sky_lines = irplib_spectrum_detect_peaks(tmp_vec, fwhm,
                    kappa, 0, NULL, NULL)) == NULL) {
        cpl_msg_error(cpl_func, "Cannot detect peaks from sky") ;
        cpl_vector_delete(obj_lines) ;
        cpl_vector_delete(tmp_vec) ;
        return NULL ;
    }
    cpl_vector_delete(tmp_vec) ;

    cpl_msg_debug(cpl_func, "Detected %"CPL_SIZE_FORMAT" lines from obj", 
            cpl_vector_get_size(obj_lines));
    cpl_msg_debug(cpl_func, "Detected %"CPL_SIZE_FORMAT" lines from sky", 
            cpl_vector_get_size(sky_lines));

    /* Obj lines need to be separated - Remove double lines */
    cpl_vector_sort(obj_lines, CPL_SORT_ASCENDING) ;
    nb_lines = cpl_vector_get_size(obj_lines) ;
    pobj_lines = cpl_vector_get_data(obj_lines) ;
    obj_lines_arr = cpl_malloc(nb_lines * sizeof(double)) ;
    /* Always keep the first line */
    obj_lines_arr[0] = pobj_lines[0] ;
    nb_found = 1 ;
    for (i=1 ; i<nb_lines ; i++) {
        /* If the lines positions are too close to neighbor, remove them */
        if (fabs(pobj_lines[i]-pobj_lines[i-1]) > min_gap) {
            obj_lines_arr[nb_found] = pobj_lines[i] ;
            nb_found++ ;
        }
    }
    /* Create the new obj_lines */
    cpl_vector_delete(obj_lines) ;
    obj_lines = cpl_vector_new(nb_found) ;
    pobj_lines = cpl_vector_get_data(obj_lines) ;
    for (i=0 ; i<nb_found ; i++) pobj_lines[i] = obj_lines_arr[i] ;
    cpl_free(obj_lines_arr) ;
    nb_lines = cpl_vector_get_size(obj_lines) ;

    cpl_msg_debug(cpl_func, 
            "Detected %"CPL_SIZE_FORMAT" separated lines from obj", 
            cpl_vector_get_size(obj_lines));

    /* Associate obj and sky lines */
    obj_lines_arr = cpl_malloc(nb_lines * sizeof(double)) ;
    sky_lines_arr = cpl_malloc(nb_lines * sizeof(double)) ;
    nb_found = 0 ;
    cpl_vector_sort(sky_lines, CPL_SORT_ASCENDING) ;
    for (i=0 ; i<nb_lines ; i++) {
        obj_pos = cpl_vector_get(obj_lines, i) ;
        sky_idx = cpl_vector_find(sky_lines, obj_pos) ;
        sky_pos = cpl_vector_get(sky_lines, sky_idx) ;
        /* If the lines positions are close enough, keep them */
        if (fabs(obj_pos-sky_pos) < min_gap) {
            obj_lines_arr[nb_found] = obj_pos ;
            sky_lines_arr[nb_found] = sky_pos ;
            nb_found++ ;
        }
    }

    /* Create the new lines vectors */
    cpl_vector_delete(obj_lines) ;
    cpl_vector_delete(sky_lines) ;
    obj_lines = cpl_vector_new(nb_found) ;
    sky_lines = cpl_vector_new(nb_found) ;
    nb_lines = cpl_vector_get_size(obj_lines) ;
    pobj_lines = cpl_vector_get_data(obj_lines) ;
    psky_lines = cpl_vector_get_data(sky_lines) ;
    for (i=0 ; i<nb_found ; i++) {
        pobj_lines[i] = obj_lines_arr[i] ;
        psky_lines[i] = sky_lines_arr[i] ;
    }
    cpl_free(obj_lines_arr) ;
    cpl_free(sky_lines_arr) ;
 
    /* Remove outliers */
    if (remove_outliers) {
        diff = cpl_vector_duplicate(obj_lines) ;
        cpl_vector_subtract(diff, sky_lines) ;
        threshold = fabs(cpl_vector_get_median_const(diff)) + 
            2 * cpl_vector_get_stdev(diff) ;
        nb_found = 0 ;
        for (i=0 ; i<cpl_vector_get_size(diff) ; i++) 
            if (fabs(cpl_vector_get(diff, i)) < threshold) nb_found++ ; 
        obj_lines_clean = cpl_vector_new(nb_found) ;
        sky_lines_clean = cpl_vector_new(nb_found) ;
        nb_found = 0 ;
        for (i=0 ; i<cpl_vector_get_size(diff) ; i++) 
            if (fabs(cpl_vector_get(diff, i)) < threshold) {
                cpl_vector_set(obj_lines_clean, nb_found, 
                        cpl_vector_get(obj_lines, i)) ;
                cpl_vector_set(sky_lines_clean, nb_found, 
                        cpl_vector_get(sky_lines, i)) ;
                nb_found++ ;
            }
        cpl_vector_delete(diff) ;
        cpl_vector_delete(obj_lines) ;
        cpl_vector_delete(sky_lines) ;
        obj_lines = obj_lines_clean ;
        sky_lines = sky_lines_clean ;
    }

    /* Check if there are enough matched lines */
    if (nb_found <= degree) {
        cpl_msg_error(cpl_func, "Not enough match for the fit") ;
        cpl_vector_delete(obj_lines) ;
        cpl_vector_delete(sky_lines) ;
        return NULL ;
    }

    cpl_msg_debug(cpl_func, "Matched %"CPL_SIZE_FORMAT" lines", 
            cpl_vector_get_size(obj_lines));

    return cpl_bivector_wrap_vectors(obj_lines, sky_lines); 
}

