/*
 * 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 "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 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 cpl_imagelist * kmos_sky_stretch_apply(cpl_imagelist *, cpl_vector *, 
        cpl_polynomial *) ;
static int kmos_sky_stretch_verification(cpl_bivector *, cpl_bivector *,
        cpl_polynomial *, int) ;
static cpl_polynomial * kmos_priv_create_init_poly(double, double, int, int) ;
static int kmos_priv_sky_plot(cpl_imagelist *, cpl_imagelist *, cpl_imagelist *,
        int, cpl_vector *, cpl_image *) ;
static cpl_imagelist * kmos_priv_sky_stretch(cpl_imagelist *, cpl_imagelist *,
        cpl_vector *, cpl_image *, int, int) ;
static int kmos_sky_tweak_identify_lines_cont(cpl_vector *, int *, int *, 
        int *, int *) ;
static void kmos_plot_bivectors(cpl_bivector *, cpl_bivector *, char *) ;
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_bivector * kmos_sky_tweak_correct_vibrational_trans(
        cpl_bivector *, cpl_bivector *, int, int) ;

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

/**@{*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Main sky tweaking function
  @param    object  Object cube
  @param    sky     Sky cube
  @param    header  NAXIS3 / CRVAL3 / CDELT3 / CRPIX3
  @param    min_frac
  @param    tbsub
  @param    ifu_nr
  @param    skip_last
  @param    stretch : apply stretching (0-no, 1-yes, 2-force)
  @param    display
  @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.
 */
/*----------------------------------------------------------------------------*/
cpl_imagelist * kmos_priv_sky_tweak(
        cpl_imagelist           *   obj, 
        cpl_imagelist           *   sky,
        const cpl_propertylist  *   header, 
        float                       min_frac, 
        int                         tbsub,
        int                         ifu_nr,
        int                         skip_last,
        int                         stretch,
        int                         plot) 
{
    cpl_imagelist   *   result ;
    cpl_image       *   mask ;
    cpl_imagelist   *   sky_stretched ;
    cpl_bivector    *   obj_spec ;
    cpl_bivector    *   sky_stretched_spec ;
    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 ;
    }

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

    /* Stretch the sky to align the lineѕ */
    if (stretch > 0) {
        if ((sky_stretched = kmos_priv_sky_stretch(obj, sky, lambda, mask,
                        stretch, plot)) == NULL) {
            cpl_msg_warning(__func__, "Cannot stretch the sky - skip") ;
            sky_stretched = cpl_imagelist_duplicate(sky) ;
        }
    } else {
        sky_stretched = cpl_imagelist_duplicate(sky) ;
    }

    /* Get the background spectrum (using mask) from obj and sky cubes */
    kmos_sky_tweak_get_spectra(obj, sky_stretched, lambda, mask, 1, 
            &obj_spec, &sky_stretched_spec, &first_nonan);
   
    /* Plot */
    if (plot) kmos_plot_bivectors(obj_spec, sky_stretched_spec,
            "t 'Obj and Stretched sky' w lines") ;

    /* Get the corrections factors for each sub-band */
    vscales = kmos_sky_tweak_correct_vibrational_trans(obj_spec,
                sky_stretched_spec, skip_last, plot);
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) 
        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) 
        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(sky_stretched,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(sky_stretched,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(sky_stretched,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_stretched_spec);
    if (thermal_backg != NULL) cpl_bivector_delete(thermal_backg);
    cpl_bivector_delete(vscales);

    /* Plot the results */
    if (plot)
        kmos_priv_sky_plot(obj, sky, sky_stretched, ifu_nr, lambda, mask) ;
    cpl_vector_delete(lambda);
    cpl_image_delete(mask);
    cpl_imagelist_delete(sky_stretched);

    return result;
}

/** @} */

/*----------------------------------------------------------------------------*/
/**
  @brief  
  @param
  @return
 */
/*----------------------------------------------------------------------------*/
static int kmos_priv_sky_plot(
        cpl_imagelist           *   obj, 
        cpl_imagelist           *   sky,
        cpl_imagelist           *   sky_stretched_scaled,
        int                         ifu_nr,
        cpl_vector              *   lambda,
        cpl_image               *   mask) 
{
    cpl_bivector    *   obj_spec ;
    cpl_bivector    *   sky_spec ;
    cpl_bivector    *   sky_stretched_scaled_spec ;
    cpl_imagelist   *   sky_ref ;
    /* cpl_bivector    *   sky_ref_spec ; */
    int                 first_nonan ;

    /* Dev :  Load Reference Sky */
    /*
    sky_ref=kmclipm_imagelist_load(
            "/home/yjung/P_kmos/kmos_test_5915/sky13-11-14T00_49_48.550_sci_reconstructed_flat_dc_KMOS.2013-11-14T00_55_04.459.fits", 
            CPL_TYPE_FLOAT, ifu_nr);
    */

    /* Get the background spectrum from obj and sky cubes */
    kmos_sky_tweak_get_spectra(obj, sky, lambda, mask, 1, &obj_spec,
            &sky_spec, &first_nonan);
    cpl_bivector_delete(obj_spec) ;
    /*
    kmos_sky_tweak_get_spectra(obj, sky_ref, lambda, mask, 1, &obj_spec, 
            &sky_ref_spec, &first_nonan);
    cpl_bivector_delete(obj_spec) ;
    */
    kmos_sky_tweak_get_spectra(obj, sky_stretched_scaled, lambda, mask, 1, 
            &obj_spec, &sky_stretched_scaled_spec, &first_nonan);

    /* Plot obj-sky */
    kmos_plot_bivectors(obj_spec, sky_spec, "t 'Obj and Input Sky' w lines") ;
    /* Plot obj-ref_sky */
    /*
    kmos_plot_bivectors(obj_spec, sky_ref_spec, "t 'Obj and Ric Sky' w lines") ;
    */
    /* Plot obj-sky_stretched_scaled */
    kmos_plot_bivectors(obj_spec, sky_stretched_scaled_spec,
            "t 'Obj and Stretched Scaled Sky' w lines") ;

    cpl_bivector_delete(obj_spec) ;
    cpl_bivector_delete(sky_spec) ;
    /* cpl_bivector_delete(sky_ref_spec) ; */
    cpl_bivector_delete(sky_stretched_scaled_spec) ;
    cpl_imagelist_delete(sky_ref);
    return 0 ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Compute the polynomial from boundaries
  @param    wmin    First pixel wavelength
  @param    wmax    Last pixel wavelength
  @param    pix_min First pixel id
  @param    pix_max Last pixel id
  @return   the polynomial or NULL in error case
  The returned polynomial must be deallocated with cpl_polynomial_delete()
 */
/*----------------------------------------------------------------------------*/
static cpl_polynomial * kmos_priv_create_init_poly(
        double          wmin,
        double          wmax,
        int             pix_min,
        int             pix_max)
{
    cpl_polynomial      *   poly ;
    cpl_size                power ;
    double                  a, b ;

    /* Test entries */
    if (wmin <= 0.0) return NULL ;
    if (wmax <= 0.0) return NULL ;
    if (wmax <= wmin) return NULL ;

    /* Compute polynomial coeffs */
    a = (wmax - wmin) / (pix_max - pix_min) ;
    b = wmin - a * pix_min ;

    /* Create polynomial */
    poly = cpl_polynomial_new(1) ;
    power = 0 ;
    cpl_polynomial_set_coeff(poly, &power, b) ;
    power = 1 ;
    cpl_polynomial_set_coeff(poly, &power, a) ;

    return poly ;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Ѕtretch the sky cube to align the sky spectrum on the one
              from the object cube
  @param    obj     Object cube
  @param    sky     Sky cube
  @param    lambda  Vector with wavelength values (same size as obj&sky)
  @param    mask    Mask identifiing the background pixels
  @param    stretch 0-no, 1-yes, 2-force
  @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. Many stretches are tried and the best one is retained.
  The best stretching polynomial is used to stretch the input sky cube
  and return it.
    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,
        cpl_vector              *   lambda,
        cpl_image               *   mask, 
        int                         stretch,
        int                         plot)
{
    cpl_imagelist   *   new_sky ;
    cpl_bivector    *   obj_spec ;
    cpl_bivector    *   sky_spec ;
    cpl_vector      *   wl_errors ;
    double              wl_error ;
    int                 nsamples ;
    cpl_polynomial  *   xcdisp ;
    cpl_polynomial  *   xcdisp2 ;
    cpl_polynomial  *   init_poly ;
    double              slit_width, fwhm, xc ;
    cpl_vector      *   xcorrs ;
    cpl_bivector    *   obj_spec_extended ;
    double              *obj_spec_extended_x, *obj_spec_extended_y,
                        *obj_spec_x, *obj_spec_y ;   
    int                 poly_deg, i, spec_size, wl_error_pix, first_nonan ;

    /* Initialize */
    slit_width = 2.0 ;
    fwhm = 2.0 ;
    xcorrs = NULL ;
    nsamples = 5 ;
    poly_deg = 2 ;
    wl_error_pix = 4 ;

    /* Check inputs */
    if (stretch <= 0) return NULL;

    /* Get the background spectrum from obj and sky cubes using mask */
    kmos_sky_tweak_get_spectra(obj, sky, lambda, mask, 1, &obj_spec,
            &sky_spec, &first_nonan);
    spec_size = cpl_bivector_get_size(obj_spec) ;

    /* Convert the WL error from pixels to wavelength */
    wl_error=cpl_vector_get(lambda, wl_error_pix) -
        cpl_vector_get(lambda, 0) ;
    cpl_msg_debug(__func__, "ERROR pix / wl: %d / %g\n",wl_error_pix,wl_error) ;

    /* Increase the size of the obj bivector for the Correlation */
    obj_spec_extended = cpl_bivector_new(spec_size+2*wl_error_pix) ;
    obj_spec_extended_x = cpl_bivector_get_x_data(obj_spec_extended) ;
    obj_spec_extended_y = cpl_bivector_get_y_data(obj_spec_extended) ;
    obj_spec_x = cpl_bivector_get_x_data(obj_spec) ;
    obj_spec_y = cpl_bivector_get_y_data(obj_spec) ;
    for (i=wl_error_pix ; i<spec_size+wl_error_pix ; i++) {
        obj_spec_extended_x[i] = obj_spec_x[i-wl_error_pix] ;
        obj_spec_extended_y[i] = obj_spec_y[i-wl_error_pix] ;
    }
    for (i=0 ; i<wl_error_pix ; i++) {
        obj_spec_extended_x[wl_error_pix-i-1] = obj_spec_x[0]-
            (obj_spec_x[1]-obj_spec_x[0])*(i+1);
        obj_spec_extended_y[i] = obj_spec_y[0] ;
    }
    for (i=spec_size+wl_error_pix ; i<spec_size+2*wl_error_pix ; i++) {
        obj_spec_extended_x[i] = obj_spec_x[spec_size-1] +    
            (obj_spec_x[spec_size-1]-obj_spec_x[spec_size-2])*
            (i-spec_size+wl_error_pix+1);
        obj_spec_extended_y[i] = obj_spec_y[spec_size-1] ;
    }

    /* Create Initial Polynomial */
    init_poly = kmos_priv_create_init_poly(
            cpl_vector_get(cpl_bivector_get_x_const(sky_spec), 0),
            cpl_vector_get(cpl_bivector_get_x_const(sky_spec), spec_size-1),
            0, spec_size-1) ;
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) 
        cpl_polynomial_dump(init_poly, stdout); 

    /* Best Polynomial search 1. pass */
    cpl_msg_info(__func__, "Search for the best stretching polynomial") ;
    cpl_msg_indent_more() ;
    wl_errors = cpl_vector_new(poly_deg + 1) ;
    cpl_vector_fill(wl_errors, wl_error) ;
    xcdisp=irplib_wlxcorr_best_poly(cpl_bivector_get_y(sky_spec), 
            obj_spec_extended, poly_deg, init_poly, wl_errors, nsamples, 
            slit_width, fwhm, &xc, NULL, &xcorrs) ;
    cpl_msg_indent_less() ;
    cpl_polynomial_delete(init_poly) ;
    cpl_vector_delete(wl_errors) ;
    if (xcorrs != NULL) cpl_vector_delete(xcorrs) ;
    if (xcdisp == NULL) {
        cpl_msg_error(__func__, "Cannot Find the best stretching polynomial") ;
        cpl_bivector_delete(obj_spec_extended); 
        cpl_bivector_delete(obj_spec); 
        cpl_bivector_delete(sky_spec); 
        cpl_error_reset() ;
        return NULL ;
    }
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) {
        cpl_polynomial_dump(xcdisp, stdout); 
        cpl_msg_info(__func__, "Cross-Correlation factor: %g", xc) ;
        kmos_sky_stretch_verification(obj_spec, sky_spec, xcdisp, plot) ;
    }

    /* Best Polynomial search 2. pass */
    nsamples = 10 ;
    cpl_msg_indent_more() ;
    wl_errors = cpl_vector_new(poly_deg + 1) ;
    cpl_vector_fill(wl_errors, wl_error/2.) ;
    xcdisp2=irplib_wlxcorr_best_poly(cpl_bivector_get_y(sky_spec), 
            obj_spec_extended, poly_deg, xcdisp, wl_errors, nsamples, 
            slit_width, fwhm, &xc, NULL, &xcorrs) ;
    cpl_msg_indent_less() ;
    cpl_bivector_delete(obj_spec_extended); 
    cpl_polynomial_delete(xcdisp) ;
    cpl_vector_delete(wl_errors) ;
    if (xcdisp2 == NULL) {
        cpl_msg_error(__func__, "Cannot Find the best stretching polynomial") ;
        cpl_bivector_delete(obj_spec); 
        cpl_bivector_delete(sky_spec); 
        if (xcorrs != NULL) cpl_vector_delete(xcorrs) ;
        cpl_error_reset() ;
        return NULL ;
    }
    if (cpl_msg_get_level() == CPL_MSG_DEBUG) {
        cpl_polynomial_dump(xcdisp2, stdout); 
        cpl_msg_info(__func__, "Cross-Correlation factor: %g", xc) ;
        kmos_sky_stretch_verification(obj_spec, sky_spec, xcdisp2, plot) ;
    }
    cpl_bivector_delete(obj_spec) ;
    cpl_bivector_delete(sky_spec) ;

    /* Check XC factor */
    if (xc < 0.9 && stretch == 1) {
        cpl_msg_error(__func__, "Failed to properly correlate") ;
        if (xcorrs != NULL) cpl_vector_delete(xcorrs) ;
        cpl_polynomial_delete(xcdisp2) ;
        return NULL ;
    }

    /* Plot the correlation values */
    if (plot) {
        cpl_plot_vector("set grid;",
                "t 'Correlation values for the linear solution search' w lines",
                "", xcorrs) ;
    }
    if (xcorrs != NULL) cpl_vector_delete(xcorrs) ;

    /* The polynomial is computed on a truncated signal (first values */
    /* are nan). It needs to be shifted to be valid on a 2048 bins cube */
    cpl_polynomial_shift_1d(xcdisp2, 0, -1*first_nonan) ;

    /* Apply the Sky cube stretching */
    new_sky = kmos_sky_stretch_apply(sky, lambda, xcdisp2);
    cpl_polynomial_delete(xcdisp2) ;
    return new_sky ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   Apply the stretching to the sky spectrum and see the difference
  @param    sky         Sky cube
  @param    xcdisp      Stretching polynomial
  @return   The stretched sky
 */
/*----------------------------------------------------------------------------*/
static cpl_imagelist * kmos_sky_stretch_apply(
        cpl_imagelist   *   sky, 
        cpl_vector      *   lambdas,
        cpl_polynomial  *   xcdisp)
{
    cpl_imagelist   *   out_sky ;
    cpl_vector      **  sky_specs ;
    cpl_vector      **  resampled_sky_specs ;
    cpl_vector      *   new_lambdas ;
    float           *   image_data ;
    cpl_bivector    *   ref_spec ;
    cpl_bivector    *   resampled_spec ;
    int                 spec_size, nx, ny, i, j, k ;

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

    /* Create the sky spectra */
    sky_specs = cpl_malloc(nx*ny*sizeof(cpl_vector*)) ;
    for (i=0 ; i<nx ; i++)
        for (j=0 ; j<ny ; j++)
            sky_specs[i+j*nx] = cpl_vector_new(spec_size) ;

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

    /* Create the New lambdas vector */
    new_lambdas = cpl_vector_new(spec_size) ;
    cpl_vector_fill_polynomial(new_lambdas, xcdisp, 1, 1) ;

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

    resampled_sky_specs = 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_sky_specs[i+j*nx] = cpl_vector_new(spec_size) ;

            /* Wrap the bivectors */
            ref_spec = cpl_bivector_wrap_vectors(new_lambdas,sky_specs[i+j*nx]);
            resampled_spec = cpl_bivector_wrap_vectors(lambdas, 
                    resampled_sky_specs[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_lambdas) ;

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

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

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

/*----------------------------------------------------------------------------*/
/**
  @brief   Apply the stretching to the sky spectrum and see the difference
  @param    obj_spec    Sky spectrum from the object
  @param    sky_spec    Sky spectrum from the sky
  @param    xcdisp      Stretching polynomial
  @param    plot        Flag to enable plotting
  @return   0 if ok, -1 otherwise
 */
/*----------------------------------------------------------------------------*/
static int kmos_sky_stretch_verification(
        cpl_bivector    *   obj_spec, 
        cpl_bivector    *   sky_spec, 
        cpl_polynomial  *   xcdisp, 
        int                 plot) 
{
    cpl_bivector    *   new_sky_spec ;
    cpl_bivector    *   resampled_sky_spec ;
    double          *   lambdas1 ;
    double          *   lambdas2 ;
    int                 spec_size ;

    /* Initialize */
    spec_size = cpl_bivector_get_size(obj_spec) ;

    /* Apply the Wavelength Transformation to the sky spectrum */
    new_sky_spec = cpl_bivector_duplicate(sky_spec) ;
    cpl_vector_fill_polynomial(cpl_bivector_get_x(new_sky_spec), xcdisp, 1, 1) ;

    /* Resample y */
    resampled_sky_spec = cpl_bivector_duplicate(sky_spec) ;

    /* new_sky_spec wmin/max outside sky_spec wmin/max for the resampling  */
    lambdas1 = cpl_bivector_get_x_data(new_sky_spec) ;
    lambdas2 = cpl_bivector_get_x_data(sky_spec) ;
    if (lambdas1[0] > lambdas2[0]) 
        lambdas1[0] = lambdas2[0] ;
    if (lambdas1[spec_size-1] < lambdas2[spec_size-1]) 
        lambdas1[spec_size-1] = lambdas2[spec_size-1] ;

    /* Apply the resampling */
    cpl_bivector_interpolate_linear(resampled_sky_spec, new_sky_spec) ;
    if (cpl_error_get_code() != CPL_ERROR_NONE) {
        cpl_msg_error(__func__, "Cannot resample") ;
        cpl_bivector_delete(new_sky_spec); 
        cpl_bivector_delete(resampled_sky_spec); 
        return -1 ;
    }
    cpl_bivector_delete(new_sky_spec); 

    /* Plot */
    if (plot) {
        kmos_plot_bivectors(obj_spec, sky_spec, "t 'before' w lines") ;
        kmos_plot_bivectors(obj_spec, resampled_sky_spec, "t 'after' w lines") ;
    }
    cpl_bivector_delete(resampled_sky_spec); 
    return 0 ;
}

/*----------------------------------------------------------------------------*/
/**
  @brief   
  @param    
  @return
 */
/*----------------------------------------------------------------------------*/
static void kmos_plot_bivectors(
        cpl_bivector    *   obj, 
        cpl_bivector    *   sky,
        char            *   title)
{
    cpl_vector      **  vectors ;
    cpl_vector      *   diff_vector ;
    double              max, mean, min ;
    int                 i ;

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

    /* Create Vectors */
    vectors = cpl_malloc(3*sizeof(cpl_vector*)) ;
    vectors[0] = cpl_bivector_get_x(obj) ;
    vectors[1] = cpl_bivector_get_y(obj) ;
    vectors[2] = cpl_bivector_get_y(sky) ;

    /*
    */
    cpl_plot_vectors("set grid;set xlabel 'Wavelngth (microns)';",
            title, "", (const cpl_vector **)vectors, 3);
    cpl_free(vectors) ;

    /* Create Difference */
    diff_vector = cpl_vector_duplicate(cpl_bivector_get_y(obj)) ;
    cpl_vector_subtract(diff_vector, cpl_bivector_get_y(sky)) ;

    /* Set max to 0 for display */
    max = cpl_vector_get_max(diff_vector) ;
    min = cpl_vector_get_min(diff_vector) ;
    mean = cpl_vector_get_mean(diff_vector) ;

    for (i=0 ; i<cpl_vector_get_size(diff_vector) ; i++) {
        if (cpl_vector_get(diff_vector, i) > 0.99*max)
            cpl_vector_set(diff_vector, i, mean) ;
        if (cpl_vector_get(diff_vector, i) < 0.0)
            cpl_vector_set(diff_vector, i, mean) ;
    }

    /* Plot the difference */
    cpl_plot_vector("set grid;set xlabel 'Wavlength (microns)';",
            title, "", diff_vector);
    
    /* Free */
    cpl_vector_delete(diff_vector) ;
    return ;
}

/*----------------------------------------------------------------------------*/
/**
  @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_plot_vectors("set grid;set xlabel 'Wavelength (microns)';",
            "t 'Obj and Stretched Sky sub-band' w lines", "", 
            (const cpl_vector **)vectors, 3);

    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(
        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;

    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);
    *first_nonan = -1 ;

    /* Check entries */
    if (nx!=snx || ny!=sny || nz!=snz || first_nonan==NULL) {
        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 ;
    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 'Stretched 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 stretched 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         *   cont_size, 
        int         *   lines_size) 
{
    int     current_line_size, nr_samples, i ;

    /* 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;
    *cont_size = 0;
    *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;
            (*cont_size)++;
            *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, *cont_size, *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 !!!!!!! */


