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

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

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

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

#include <cpl.h>

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

#include "kmo_error.h"
#include "kmo_priv_extract_spec.h"
#include "kmo_priv_lcorr.h"
#include "kmo_priv_fit_profile.h"

FILE *IDL = NULL;

/*----------------------------------------------------------------------------*/
/**
    @defgroup kmos_priv_lcorr     Helper functions for lambda correction.
    @{
 */
/*----------------------------------------------------------------------------*/

int print_info_once_oh_ref = TRUE;

/*----------------------------------------------------------------------------*/
/**
    @brief
        Main function to create the lcal-correction

    @param cube      input data cube
    @param header    the header of @c cube
    @param header    the header of @c cube

   @return The lcal-correction-polynomial to be used with @c kmo_reconstruct_sci().

    The returned polynomial has to be deallocated with cpl_polynomial_delete().
*/
/*----------------------------------------------------------------------------*/
cpl_polynomial *kmo_lcorr_get(const cpl_imagelist *cube,
                              const cpl_propertylist *header,
                              const cpl_frame *ref_spectrum_frame,
                              const gridDefinition gd,
                              const char *filter_id,
                              int ifu_id)
{
    cpl_polynomial      *lcorr_coeffs   = NULL;
    cpl_bivector        *obj_spectrum   = NULL,
                        *ref_spectrum   = NULL;
    cpl_vector          *peaks          = NULL,
                        *range          = NULL;
    int                 ic              = 0;
    const int           format_width    = 14,
                        max_coeffs      = 6;
    char                *coeff_string   = NULL,
                        coeff_dump[format_width * max_coeffs + 1];
    const char          *ref_filename   = NULL;
    cpl_size            pows[1];

    KMO_TRY
    {
        KMO_TRY_ASSURE((cube != NULL) &&
                       (header != NULL) &&
                       (ref_spectrum_frame != NULL),
                       CPL_ERROR_NULL_INPUT,
                       "Not all input data is provided!");

        KMO_TRY_EXIT_IF_NULL(
            range = cpl_vector_new(2));
        KMO_TRY_EXIT_IF_ERROR(
            cpl_vector_set(range, 0, gd.l.start));
        KMO_TRY_EXIT_IF_ERROR(
            cpl_vector_set(range, 1, gd.l.start + gd.l.delta * gd.l.dim));

        KMO_TRY_EXIT_IF_NULL(
            ref_filename = cpl_frame_get_filename(ref_spectrum_frame));
        KMO_TRY_EXIT_IF_NULL(
            ref_spectrum = kmo_lcorr_read_reference_spectrum(ref_filename, NULL));
        KMO_TRY_EXIT_IF_NULL(
            peaks = kmo_lcorr_get_peak_lambdas(ref_spectrum, 0.2, range));

        KMO_TRY_EXIT_IF_NULL(
            obj_spectrum = kmo_lcorr_extract_spectrum(cube, header, 0.8, NULL));

        KMO_TRY_EXIT_IF_NULL(
            lcorr_coeffs = kmo_lcorr_crosscorrelate_spectra(obj_spectrum, ref_spectrum,
                                                            peaks, filter_id));

        // debug stuff
        coeff_dump[0] = 0;
        for (ic = 0; ic <= cpl_polynomial_get_degree(lcorr_coeffs) && ic < max_coeffs; ic++) {
            pows[0] = ic;
            coeff_string = cpl_sprintf(" %*g,", format_width-2, cpl_polynomial_get_coeff(lcorr_coeffs,pows));
            strncat(coeff_dump, coeff_string, format_width);
            cpl_free(coeff_string); coeff_string = NULL;
        }
        cpl_msg_debug(cpl_func,"Lambda correction coeffs for IFU %d %s", ifu_id, coeff_dump);
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        cpl_polynomial_delete(lcorr_coeffs); lcorr_coeffs = NULL;
    }

    cpl_vector_delete(range); range = NULL;
    cpl_vector_delete(peaks); peaks = NULL;
    cpl_bivector_delete(ref_spectrum); ref_spectrum = NULL;
    cpl_bivector_delete(obj_spectrum); obj_spectrum = NULL;
    cpl_free(coeff_string); coeff_string = NULL;

    return lcorr_coeffs;
}

/*----------------------------------------------------------------------------*/
/**
    @brief
        Creates a vector containing the spectrum of all marked spaxels

    @param cube      input cube
    @param obj_mask  image holding weights for all spaxels
                            0.0:   spaxels is disregarded
                            1.0:   spaxel is taken

   @return A vector in case of success, NULL otherwise.

    The returned vector has to be deallocated with cpl_vector_delete().
*/
/*----------------------------------------------------------------------------*/
cpl_bivector *kmo_lcorr_extract_spectrum (const cpl_imagelist *cube,
        const cpl_propertylist *header,
        const double min_frac,
        const cpl_vector *range) {

    cpl_bivector *result = NULL;
    cpl_vector *spectrum = NULL,
               *lambda   = NULL;
    cpl_image *obj_mask = NULL;
    KMO_TRY
    {
        KMO_TRY_ASSURE(cube != NULL && header != NULL,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");

        KMO_TRY_EXIT_IF_NULL(
                lambda = kmo_lcorr_create_lambda_vector(header));

        if (range != NULL) {
            KMO_TRY_EXIT_IF_NULL(
                    obj_mask = kmo_lcorr_create_object_mask(cube, min_frac, lambda, range));
        } else {
            KMO_TRY_EXIT_IF_NULL(
                    obj_mask = kmo_lcorr_create_object_mask(cube, min_frac, NULL, NULL));
        }

        KMO_TRY_EXIT_IF_ERROR(
                kmo_priv_extract_spec(cube, NULL, obj_mask, &spectrum, NULL));

        KMO_TRY_EXIT_IF_NULL(
                result = cpl_bivector_wrap_vectors(lambda, spectrum));
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
    }

    if (obj_mask != NULL) {cpl_image_delete(obj_mask);}
    if (result == NULL) {
        if (lambda != NULL) {cpl_vector_delete(lambda);}
        if (spectrum != NULL) {cpl_vector_delete(spectrum);}
    }
    return result;
}

/*----------------------------------------------------------------------------*/
/**
    @brief
        Creates a mask of the cube

    For every spaxel an averaged (using median) flux is calculated. If both a lambda vector and
    wavelength range(s) are specified only those ranges are considered.

    Both the lambda vector and the wavelength range vector must be specified or must be NULL.

    Each entry in the lambda vector corresponds to the same index in the z-axis of the cube. So
    the lambda vector must have the same number of elements than the number of images in the cube.

    The wavelength range vector contains one or more ranges as a sequence of value pairs:
    lower range value (inclusive), upper range value (inclusive)


    @param cube      input cube
    @param min_frac  fraction of spaxels sorted according the average flux to be used
    @param lambda    wavelength vector, each entry corresponds  to the z-direction of the input cube
                     or NULL
    @param range     wavelength ranges to be considered
                     or NULL

   @return An image holding the mask in case of success, NULL otherwise

    The returned image has to be deallocated with cpl_image_delete().
*/
/*----------------------------------------------------------------------------*/
cpl_image *kmo_lcorr_create_object_mask (const cpl_imagelist *cube,
        float min_frac,
        const cpl_vector *lambda,
        const cpl_vector *range) {

    cpl_image *mask      = NULL;
    cpl_imagelist *rcube = NULL;
    const cpl_image *slice = NULL;
    const cpl_imagelist *icube = NULL;
    cpl_image *tmp_img   = NULL,
                    *m_img = NULL;
    cpl_vector *sum      = NULL,
               *m_vector = NULL,
               *sorted_sum = NULL;
    cpl_mask         *empty_mask     = NULL;

    int x, y, z,  r;
    int nx, ny, nz, nr, px, cnz= 0;
    int check4range = FALSE;
    const double  *lambda_data = NULL;
    double l_low, l_up = 0.0;
    double max_value = 0.0;
    int *cnts = NULL;
    const double *m_data   = NULL;
    float        *mask_data = NULL;

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

        KMO_TRY_ASSURE(((lambda != NULL) && (range != NULL)) ||
                       ((lambda == NULL) && (range == NULL)) ,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");
        if (range != NULL) {
            check4range = TRUE;
            KMO_TRY_ASSURE(cpl_vector_get_size(range)%2 == 0,
                    CPL_ERROR_ILLEGAL_INPUT,
                    "Range vector size must be a multiple of 2");

            nr = cpl_vector_get_size(range) / 2;
        }

        if (lambda != NULL) {
            KMO_TRY_ASSURE(cpl_vector_get_size(lambda) == cpl_imagelist_get_size(cube),
                    CPL_ERROR_ILLEGAL_INPUT,
                    "Size of lambda vector must be the number of images in the input cube");
            KMO_TRY_EXIT_IF_NULL(
                    lambda_data = cpl_vector_get_data_const(lambda));
        }

        KMO_TRY_EXIT_IF_NULL(
                slice = cpl_imagelist_get_const(cube, 0));
        nx = cpl_image_get_size_x(slice);
        ny = cpl_image_get_size_y(slice);
        nz = cpl_imagelist_get_size(cube);

        // if wavelength range are specified create a new imagelist holding only those lambdas
        // otherwise take original input cube
        if (check4range) {
            cnz = 0;
            KMO_TRY_EXIT_IF_NULL(
                    rcube = cpl_imagelist_new());
            for (z=0; z<nz; z++) {
                int checkOK = FALSE;
                for (r=0; r<nr; r++) {
                    l_low = cpl_vector_get(range, r*2);
                    l_up  = cpl_vector_get(range, r*2+1);
                    if ((lambda_data[z] >= l_low) && (lambda_data[z] <= l_up)) {
                        checkOK = TRUE;
                        break;
                    }
                }
                if (checkOK) {
                    KMO_TRY_EXIT_IF_NULL(
                            slice = cpl_imagelist_get_const(cube, z));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_imagelist_set(rcube, cpl_image_duplicate(slice), cnz));
                    cnz++;
                }
            }
            icube = rcube;
        } else {
            icube = cube;
        }

        KMO_TRY_EXIT_IF_NULL(
                tmp_img =  cpl_imagelist_collapse_median_create(icube));
        KMO_TRY_EXIT_IF_NULL(
                m_img =  cpl_image_cast(tmp_img, CPL_TYPE_DOUBLE));
       KMO_TRY_EXIT_IF_NULL(
                m_data = cpl_image_get_data_double_const(m_img));
        KMO_TRY_EXIT_IF_NULL(
                m_vector = cpl_vector_wrap(nx * ny, (double *) m_data));
        KMO_TRY_EXIT_IF_NULL(
                sorted_sum = cpl_vector_duplicate(m_vector));
        cpl_vector_sort(sorted_sum, CPL_SORT_ASCENDING);
        max_value = cpl_vector_get(sorted_sum, nx*ny*min_frac);

        mask = cpl_image_new(nx,ny,CPL_TYPE_FLOAT);
        KMO_TRY_EXIT_IF_NULL(
                mask_data = cpl_image_get_data_float(mask));
        for (x=0; x<nx; x++) {
            for (y=0; y<ny; y++) {
                px = x+y*nx;
                if (m_data[px] <= max_value) {
                    mask_data[px] = 1.0;
                } else {
                    mask_data[px] = 0.0;
                }
            }
        }

    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        if (mask != NULL) {cpl_image_delete(mask);}
        mask = NULL;
    }
    if (rcube != NULL) {cpl_imagelist_delete(rcube);}
    if (tmp_img != NULL) {cpl_image_delete(tmp_img);}
    if (m_img != NULL) {cpl_image_delete(m_img);}
    if (m_vector != NULL) {cpl_vector_unwrap(m_vector);}
    if (sum != NULL) {cpl_vector_delete(sum);}
    if (cnts != NULL) {cpl_free(cnts);}
    if (sorted_sum != NULL) {cpl_vector_delete(sorted_sum);}
    if (empty_mask != NULL) {cpl_mask_delete(empty_mask);}

    return mask;
}

/*----------------------------------------------------------------------------*/
/**
    @brief
        Creates a wavelength vector from an input cube

    Using the WCS properties of the third axis a lambda vector is created.

    @param propertylist property list of input cube

   @return A vector in case of success, NULL otherwise.

    The returned vector has to be deallocated with cpl_vector_delete().
*/
/*----------------------------------------------------------------------------*/

cpl_vector *kmo_lcorr_create_lambda_vector(const cpl_propertylist *header) {

    cpl_vector *lambda_vector = NULL;
    int naxis3, z = 0;
    double  crval3, cdelt3, crpix3 = 0.0;
    double *lambda = NULL;

    KMO_TRY {
        KMO_TRY_ASSURE(header != NULL,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");
        KMO_TRY_ASSURE(cpl_propertylist_has(header,NAXIS3) &&
                cpl_propertylist_has(header,CRVAL3) &&
                cpl_propertylist_has(header,CDELT3) &&
                cpl_propertylist_has(header,CRPIX3),
                CPL_ERROR_ILLEGAL_INPUT,
                "missing WCS keywords in header");

        naxis3 = cpl_propertylist_get_int(header, NAXIS3);
        crval3 = cpl_propertylist_get_double(header, CRVAL3);
        cdelt3 = cpl_propertylist_get_double(header, CDELT3);
        crpix3 = cpl_propertylist_get_double(header, CRPIX3);

        KMO_TRY_EXIT_IF_NULL(
                lambda_vector = cpl_vector_new(naxis3));
        KMO_TRY_EXIT_IF_NULL(
                lambda = cpl_vector_get_data(lambda_vector));

        for (z=0; z<naxis3; z++) {
            lambda[z] = (z+1 - crpix3) * cdelt3 + crval3;
        }
     }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        if (lambda_vector != NULL) {
            cpl_vector_delete(lambda_vector);
            lambda_vector = NULL;
        }
    }

    return lambda_vector;
}

/*----------------------------------------------------------------------------*/
/**
    @brief
        Creates a spectrum bivector (x: lambda) from an input FITS file

    Reads a spectrum from a FITS file. If lambda is specified the spectrum is mapped on the
    specified lambda values using cubic splines. Otherwise the native lambda resolution is returned.

    @param filename    File name of input FITS file holding the reference spectrum
    @param lambda      A vector holding the wavelength values for the output bivector or NULL

   @return A bivector in case of success, NULL otherwise.

    The returned bivector has to be deallocated with cpl_bivector_delete().
*/
/*----------------------------------------------------------------------------*/
cpl_bivector *kmo_lcorr_read_reference_spectrum(const char *filename, const cpl_vector *lambda) {

    cpl_bivector *result = NULL;
    cpl_vector *file_spec = NULL,
               *lambda_out  = NULL,
               *spec_out    = NULL;
    cpl_propertylist *header = NULL;
    double *file_data   = NULL,
           *spec_data   = NULL,
           *lambda_data = NULL;
    double crval, cdelt = 0.0;
    int naxis1, vsize, crpix = 0;

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

        if (print_info_once_oh_ref) {
            cpl_msg_info(__func__,
                    "Using file %s as OH reference spectrum for lambda correction",filename);
            print_info_once_oh_ref = FALSE;
        }

        file_spec = cpl_vector_load(filename, 1);
        if (cpl_error_get_code() != CPL_ERROR_NONE) {
            if (cpl_error_get_code() == CPL_ERROR_FILE_IO) {
                cpl_msg_error("", "File not found: %s", filename);
            } else {
                cpl_msg_error("", "Problem loading file '%s' (%s --> Code %d)",
                              filename, cpl_error_get_message(),
                              cpl_error_get_code());
            }
        }

        KMO_TRY_EXIT_IF_NULL(
                header = cpl_propertylist_load(filename,1));

        naxis1 = cpl_propertylist_get_int(header, NAXIS1);
        KMO_TRY_CHECK_ERROR_STATE();
        crval = cpl_propertylist_get_double(header, CRVAL1);
        KMO_TRY_CHECK_ERROR_STATE();
        cdelt = cpl_propertylist_get_double(header, CDELT1);
        KMO_TRY_CHECK_ERROR_STATE();
        switch (cpl_propertylist_get_type(header, CRPIX1)) {
        case CPL_TYPE_INT:
            crpix = cpl_propertylist_get_int(header, CRPIX1);
            break;
        case CPL_TYPE_DOUBLE:
            crpix = cpl_propertylist_get_double(header, CRPIX1);
            break;
        case CPL_TYPE_FLOAT:
            crpix = cpl_propertylist_get_float(header, CRPIX1);
            break;
        default:
            KMO_TRY_ASSURE(1 == 0,
                           CPL_ERROR_ILLEGAL_INPUT,
                           "CRPIX1 is of wrong type!");
        }
        KMO_TRY_CHECK_ERROR_STATE();

        if (lambda == NULL) {
            KMO_TRY_EXIT_IF_NULL(
                    lambda_out = cpl_vector_new(naxis1));
            KMO_TRY_EXIT_IF_NULL(
                    lambda_data = cpl_vector_get_data(lambda_out));
            int i = 0;
            for (i = 0; i < naxis1; i++) {
                lambda_data[i] = crval + (i + 1-crpix)*cdelt;
            }

            KMO_TRY_EXIT_IF_NULL(
                    spec_out = cpl_vector_duplicate(file_spec));
        } else {
            KMO_TRY_EXIT_IF_NULL(
                    file_data = cpl_vector_get_data(file_spec));

            vsize = cpl_vector_get_size(lambda);

            spec_data = cubicspline_reg_irreg(naxis1, crval + (1-crpix)*cdelt, cdelt, file_data,
                vsize, (double *) cpl_vector_get_data_const(lambda), NATURAL);

            KMO_TRY_EXIT_IF_NULL(
                spec_out = cpl_vector_wrap(vsize, spec_data));

            KMO_TRY_EXIT_IF_NULL(
                    lambda_out = cpl_vector_duplicate(lambda));
        }

        KMO_TRY_EXIT_IF_NULL(
                result = cpl_bivector_wrap_vectors(lambda_out, spec_out));
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
    }

    if (file_spec != NULL) {cpl_vector_delete(file_spec);}
    if (header != NULL) {cpl_propertylist_delete(header);}

    return result;

}


cpl_array *kmo_lcorr_get_peak_positions(const cpl_bivector *spectrum,
        double min_frac,
        cpl_vector *range) {

    cpl_array *positions = NULL;

    cpl_vector *copied_spectrum = NULL;

    int             spec_size       = 0,
                    peak_cnt        = 0,
                    nr              = 0,
                    i               = 0,
                    r               = 0;
    double          min             = 0.0,
                    l_low           = 0.0,
                    l_up            = 0.0;
    double          *spec_data      = NULL,
                    *diff_data      = NULL;
    const double    *lambda_data    = NULL;

    KMO_TRY {
        KMO_TRY_ASSURE(spectrum != NULL && cpl_bivector_get_y_const(spectrum) != NULL,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");

       if (range != NULL) {
           KMO_TRY_ASSURE(cpl_bivector_get_x_const(spectrum) != NULL,
                   CPL_ERROR_NULL_INPUT,
                   "Not all input data is provided!");
            KMO_TRY_ASSURE(cpl_vector_get_size(range)%2 == 0,
                    CPL_ERROR_ILLEGAL_INPUT,
                    "Range vector size must be a multiple of 2");
            KMO_TRY_EXIT_IF_NULL(
                    lambda_data = cpl_vector_get_data_const(cpl_bivector_get_x_const(spectrum)));
             nr = cpl_vector_get_size(range) / 2;
        }

        KMO_TRY_EXIT_IF_NULL(
                copied_spectrum = cpl_vector_duplicate(cpl_bivector_get_y_const(spectrum)));

        KMO_TRY_EXIT_IF_NULL(
                spec_data = cpl_vector_get_data(copied_spectrum));

        spec_size = cpl_vector_get_size(copied_spectrum);

        if (range != NULL) {
            for (i = 0; i < (spec_size-1); i++) {
                int checkOK = FALSE;
                for (r = 0; r < nr; r++) {
                    l_low = cpl_vector_get(range, r*2);
                    l_up  = cpl_vector_get(range, r*2+1);
                    if ((lambda_data[i] >= l_low) && (lambda_data[i] <= l_up)) {
                        checkOK = TRUE;
                        break;
                    }
                }
                if (! checkOK) {
                    spec_data[i] = 0.;
                }
            }
        }

        KMO_TRY_EXIT_IF_NULL(
                diff_data = cpl_malloc((spec_size-1) * sizeof(double)));

        KMO_TRY_EXIT_IF_NULL(
                positions = cpl_array_new(spec_size, CPL_TYPE_INT));

        min = cpl_vector_get_max(copied_spectrum) * min_frac;

        if (spec_data[0] < min) {
            spec_data[0] = 0.;
        }
        for (i = 0; i < (spec_size-1); i++) {
            if (spec_data[i+1] < min) {
                spec_data[i+1] = 0.;
            }
            diff_data[i] = spec_data[i+1] - spec_data[i];
        }

        peak_cnt = 0;
        for (i = 0; i < (spec_size-2); i++) {
            if ((diff_data[i] > 0) && (diff_data[i+1] < 0)) {
                KMO_TRY_EXIT_IF_ERROR (
                        cpl_array_set_int(positions, peak_cnt, i+1));
                peak_cnt++;
            }
        }
        KMO_TRY_EXIT_IF_ERROR (
                cpl_array_set_size(positions, peak_cnt));
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        if (positions != NULL) {cpl_array_delete(positions);}
        positions = NULL;
    }

    if (diff_data != NULL) {cpl_free(diff_data);}
    if (copied_spectrum != NULL) {cpl_vector_delete(copied_spectrum);}

    return positions;
}

cpl_vector *kmo_lcorr_get_peak_lambdas (const cpl_bivector *spectrum,
        double min_frac,
        cpl_vector *range) {

    cpl_vector *peak_lambdas = NULL;
    cpl_array *positions = NULL;
    int pos_size = 0,
        spec_size = 0;
    const int *pos_data = NULL;
    const double *spec_data =  NULL;
    double *lambda_data =  NULL;

    KMO_TRY {
        KMO_TRY_ASSURE(spectrum != NULL &&
                cpl_bivector_get_x_const(spectrum) != NULL &&
                cpl_bivector_get_y_const(spectrum) != NULL,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");

        KMO_TRY_EXIT_IF_NULL(
                positions = kmo_lcorr_get_peak_positions(spectrum, min_frac, range));
        pos_size = cpl_array_get_size(positions);

        KMO_TRY_EXIT_IF_NULL(
                peak_lambdas = cpl_vector_new(pos_size));

        KMO_TRY_EXIT_IF_NULL(
                pos_data = cpl_array_get_data_int_const(positions));
        KMO_TRY_EXIT_IF_NULL(
                spec_data = cpl_vector_get_data_const(cpl_bivector_get_x_const(spectrum)));
        KMO_TRY_EXIT_IF_NULL(
                lambda_data = cpl_vector_get_data(peak_lambdas));

        spec_size = cpl_bivector_get_size(spectrum);

        int i = 0;
        for (i = 0; i < pos_size; i++) {
            if (pos_data[i] >= spec_size) {
                KMO_TRY_EXIT_WITH_ERROR(CPL_ERROR_ACCESS_OUT_OF_RANGE);
            }
            lambda_data[i] = spec_data[pos_data[i]];
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        if (peak_lambdas != NULL) {cpl_vector_delete(peak_lambdas);}
        peak_lambdas = NULL;
    }

    if (positions != NULL) {cpl_array_delete(positions);}
    return peak_lambdas;

}

int gauss1d_fnc(const double x[], const double a[], double *result)
{
    double peak   = a[0];
    double mean   = a[1];
    double sigma  = a[2];
    double offset = a[3];

    if (sigma == 0.0) {
        return 1;
    }
    double z = (x[0]- mean)/sigma;
    double ez = exp(-z*z/2.);
    *result = offset + peak * ez;
    return 0;
}

int gauss1d_fncd(const double x[], const double a[], double result[])
{
    double peak   = a[0];
    double mean   = a[1];
    double sigma  = a[2];
//    double offset = a[3];

    if (sigma == 0.0) {
        return 1;
    }
    double z = (x[0]- mean)/sigma;
    double ez = exp(-z*z/2.);

   // derivative in peak
    result[0] = ez;

    // derivative in mean
    result[1] = peak * ez * z/sigma;

    // derivative in sigma
    result[2] =  result[1] * z;

    // derivative in offset
    result[3] = 1.;

    return 0;
}

double fit_peak (const cpl_bivector *spectrum, const cpl_size size, double lambda_in, int half_width, double *sigma_par) {

    double error = -1.0;
    double x0 = 0., sigma = 0., area = 0., offset = 0., mse = 0.;
    cpl_size pos_found = 0, low_bound = 0, high_bound = 0;
    cpl_vector *vx = NULL,
               *vy = NULL;
    cpl_fit_mode fit_mode = CPL_FIT_ALL;
    cpl_error_code cpl_err;

    const double sqrt2pi = sqrt(2. * 3.14159265358979323846);

    sigma = *sigma_par;
    if (IDL !=NULL ) fprintf(IDL, "%20s:  %f  %d %d %f\n","fitpeak input",  lambda_in, half_width, size, sigma);

    if ((lambda_in < cpl_vector_get(cpl_bivector_get_x_const(spectrum),  0)) ||
        (lambda_in > cpl_vector_get(cpl_bivector_get_x_const(spectrum),size-1))) {
        return error;
    }
    pos_found = cpl_vector_find(cpl_bivector_get_x_const(spectrum), lambda_in);
    if (pos_found < 0) {
        return error;
    }
    low_bound = pos_found - half_width;
    high_bound = pos_found + half_width;
    if (low_bound < 0) {
        low_bound = 0;
    }
    if (high_bound >= size) {
        high_bound = size - 1;
    }

    vx = cpl_vector_extract(cpl_bivector_get_x_const(spectrum), low_bound, high_bound, 1);
    vy = cpl_vector_extract(cpl_bivector_get_y_const(spectrum), low_bound, high_bound, 1);
//    printf("low: %5d   high: %5d    lamda_in: %f\n", low_bound, high_bound, lambda_in);
    if (IDL !=NULL ) {
        fprintf(IDL, "%20s:  %d %d %d\n", "fitpeak selection",  low_bound, high_bound, cpl_vector_get_size(vx));
        fprintf(IDL, "%20s: %d", "fitpeak vx",cpl_vector_get_size(vx)); cpl_vector_dump(vx, IDL);
        fprintf(IDL, "%20s: %d", "fitpeak vy",cpl_vector_get_size(vx)); cpl_vector_dump(vy, IDL);
    }
    if (high_bound-low_bound < 4) {
        if (vx != NULL) {cpl_vector_delete(vx);}
        if (vy != NULL) {cpl_vector_delete(vy);}
       return error;
    }

    if (sigma == 0.0) {
        fit_mode = CPL_FIT_ALL;
    } else {
        fit_mode = CPL_FIT_CENTROID | CPL_FIT_AREA | CPL_FIT_OFFSET;
    }

    cpl_err = cpl_vector_fit_gaussian(vx, NULL, vy, NULL,
            fit_mode, &x0, &sigma, &area, &offset, &mse, NULL, NULL);
//    printf("vector fit               error: %d  offset: %f    x0: %f   sigma: %f    area: %f    offset: %f      mse: %f\n",
//            cpl_err, lambda_in-x0, x0, sigma, area/sigma/sqrt2pi, offset, mse);
    if (cpl_err != CPL_ERROR_NONE) {
        cpl_error_reset();
    }
    if (IDL !=NULL ) fprintf(IDL, "%20s: %d %f  %f %f %f\n", "fitpeak vectorfit",  cpl_err, area/sigma/sqrt2pi, x0, sigma, offset);


    if (IDL !=NULL ) {
        cpl_matrix *mx = cpl_matrix_wrap(cpl_vector_get_size(vx), 1, cpl_vector_get_data(vx));
        cpl_vector *fit_par = cpl_vector_new(4);
        cpl_vector_set(fit_par,0,cpl_vector_get_max(vy));
        cpl_vector_set(fit_par,1,lambda_in);
        cpl_vector_set(fit_par,2,(cpl_vector_get(vx,cpl_vector_get_size(vx)-1)-cpl_vector_get(vx,0))/9.);
        cpl_vector_set(fit_par,3,cpl_vector_get_min(vy));
        cpl_error_code cpl_err_tmp = cpl_fit_lvmq(mx, NULL,
                vy, NULL,
                fit_par, NULL,
                &gauss1d_fnc, &gauss1d_fncd,
                CPL_FIT_LVMQ_TOLERANCE / 10000.,        // 0.01
                CPL_FIT_LVMQ_COUNT,            // 5
                CPL_FIT_LVMQ_MAXITER * 1000,          // 1000
                &mse,
                NULL,
                NULL);
        if (cpl_err_tmp != CPL_ERROR_NONE) {
            cpl_error_reset();
        }
        //    printf("lvmq fit                 error: %d  offset: %f    x0: %f   sigma: %f    peak: %f    offset: %f      mse: %f\n",
        //            cpl_err,
        //            lambda_in-cpl_vector_get(fit_par,1),
        //            cpl_vector_get(fit_par,1),
        //            cpl_vector_get(fit_par,2),
        //            cpl_vector_get(fit_par,0),
        //            cpl_vector_get(fit_par,3),
        //            mse);
        fprintf(IDL, "%20s: %d %f  %f %f %f\n", "fitpeak LVMQfit",
                cpl_err_tmp, cpl_vector_get(fit_par,0), cpl_vector_get(fit_par,1),
                cpl_vector_get(fit_par,2),cpl_vector_get(fit_par,3));
        if (mx != NULL) cpl_matrix_unwrap(mx);
        if (fit_par != NULL) cpl_vector_delete(fit_par);
    }
//    printf("\n");
    cpl_vector_delete(vx);
    cpl_vector_delete(vy);

    if (cpl_err != CPL_ERROR_NONE) {
        if (cpl_err != CPL_ERROR_CONTINUE) {
            cpl_msg_error("","%s\n",cpl_error_get_message_default(cpl_err));
        }
        return error;
    }
    *sigma_par = sigma;
    return x0;

}

cpl_polynomial *kmo_lcorr_crosscorrelate_spectra(
        cpl_bivector *object,
        cpl_bivector *reference,
        cpl_vector *peaks,
        const char *band_id) {

    cpl_vector       *lambda_fit = NULL,
                     *ref_fit = NULL,
                     *sigma_fit = NULL,
                     *lambda_ref = NULL,
                     *lambda_tmp = NULL,
                     *sigma_tmp = NULL,
                     *final_obj_in = NULL,
                     *final_obj_fit = NULL,
                     *final_obj_diff = NULL;
    cpl_polynomial *coeffs = NULL;
    cpl_matrix * samppos = NULL;

    int halfwidth_obj = 0,
        halfwidth_ref = 0,
        i             = 0;
    cpl_size obj_size = 0,
        ref_size = 0,
        no_peaks = 0,
        no_valid_peaks = 0,
        peak_idx = 0,
        v_idx    = 0;
    int *invalid_index = NULL;

    double lambda_in = 0.0,
           lambda_out = 0.0,
           sigma = 0.0,
           l_median = 0.0,
           l_stdev = 0.0,
           s_median = 0.0,
           s_stdev = 0.0,
           l_limit = 0.0,
           s_limit = 0.0,
           delta_lambda = 0.0,
           resolution;

    const char *stype = NULL;

    KMO_TRY {
        KMO_TRY_ASSURE(object != NULL &&
                reference != NULL &&
                peaks != NULL,
                CPL_ERROR_NULL_INPUT,
                "Not all input data is provided!");

        KMO_TRY_ASSURE((strcmp(band_id, "IZ") == 0) ||
                       (strcmp(band_id, "YJ") == 0) ||
                       (strcmp(band_id, "H") == 0) ||
                       (strcmp(band_id, "K") == 0) ||
                       (strcmp(band_id, "HK") == 0),
                       CPL_ERROR_ILLEGAL_INPUT,
                       "Band ID for OH lambda correction must "
                       "be either \"IZ\", \"YJ\", \"H\", \"K\" or "
                       "\"HK\" (is \"%s\")!", band_id);

        if (strcmp(band_id, "H") == 0) {
            resolution = 4000.;
        } else if (strcmp(band_id, "K") == 0) {
            resolution = 4200.;
        } else if (strcmp(band_id, "HK") == 0) {
            resolution = 1800.;
        } else if (strcmp(band_id, "IZ") == 0) {
            resolution = 3200.;
        } else if (strcmp(band_id, "YJ") == 0) {
            resolution = 3200.;
        }

        obj_size = cpl_bivector_get_size(object);
        ref_size = cpl_bivector_get_size(reference);
        no_peaks = cpl_vector_get_size(peaks);
        if (IDL !=NULL ) {
            fprintf(IDL, "%20s: %d %d %d %f\n", "xcorr input", obj_size, ref_size, no_peaks, resolution);
            fprintf(IDL, "%20s: %d ", "xcorr object_l", obj_size); cpl_vector_dump(cpl_bivector_get_x_const(object), IDL);
            fprintf(IDL, "%20s: %d ", "xcorr object_v", obj_size); cpl_vector_dump(cpl_bivector_get_y_const(object), IDL);
            fprintf(IDL, "%20s: %d ", "xcorr reference_l", ref_size); cpl_vector_dump(cpl_bivector_get_x_const(reference), IDL);
            fprintf(IDL, "%20s: %d ", "xcorr reference_v", ref_size); cpl_vector_dump(cpl_bivector_get_y_const(reference), IDL);
            fprintf(IDL, "%20s: %d ", "xcorr peaks", no_peaks); cpl_vector_dump(peaks, IDL);
        }

        KMO_TRY_ASSURE(obj_size > 1 && ref_size > 1 ,
               CPL_ERROR_ILLEGAL_INPUT,
                "Input spectra must have more than one value");

        KMO_TRY_EXIT_IF_NULL(
                invalid_index = cpl_calloc(no_peaks, sizeof(int)));

        delta_lambda = 2. *
                cpl_vector_get(cpl_bivector_get_x_const(object), cpl_bivector_get_size(object) / 2) /
                resolution;


        halfwidth_obj =  delta_lambda / (cpl_vector_get(cpl_bivector_get_x_const(object),1) -
                                     cpl_vector_get(cpl_bivector_get_x_const(object),0));

        halfwidth_ref = delta_lambda / (cpl_vector_get(cpl_bivector_get_x_const(reference),1) -
                                     cpl_vector_get(cpl_bivector_get_x_const(reference),0));

        // scan for peaks which are too close
        for (i = 0; i < no_peaks-1; i++) {
            if ((cpl_vector_get(peaks,i+1) - cpl_vector_get(peaks,i)) < 2. * delta_lambda) {
                invalid_index[i] = 1;
                invalid_index[i+1] = 1;
                cpl_msg_debug(cpl_func,
                        "Pair of peaks are rejected because they are too close: %d - %d, %f - %f",
                        i, i+1, cpl_vector_get(peaks,i), cpl_vector_get(peaks,i+1));
            }
        }

        // first scan reference spectrum
        // find exact peak location by fitting a gaussian
        // detect and withdraw outliers
        KMO_TRY_EXIT_IF_NULL(
                ref_fit = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                sigma_fit = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                lambda_ref = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                lambda_tmp = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                sigma_tmp = cpl_vector_new(no_peaks));

        peak_idx = 0;
        stype="reference";
        if (IDL !=NULL ) fprintf(IDL, "%20s: %s\n", "xcorr spectrum type", stype);
        for (i = 0 ; i < no_peaks; i++) {
            lambda_in = cpl_vector_get(peaks,i);
            sigma = 0.0;
            lambda_out = fit_peak (reference, ref_size, lambda_in, halfwidth_ref, &sigma);
            if (invalid_index[i] == 0) {
                if (lambda_out > 0.) {
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(ref_fit, i, lambda_out));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(sigma_fit, i, sigma));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(lambda_ref, peak_idx, lambda_in));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(lambda_tmp, peak_idx, lambda_out));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(sigma_tmp, peak_idx, sigma));
                    peak_idx++;
                } else {
                    invalid_index[i] = 2;
                    cpl_msg_debug(cpl_func,
                            "Gaussian fit failed for %s spectrum, wavelenght %f, peak index %d",
                            stype, lambda_in, i);
                }
            }
        }
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(lambda_ref,peak_idx));
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(lambda_tmp,peak_idx));
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(sigma_tmp,peak_idx));
        if (IDL !=NULL ) {
            fprintf(IDL, "%20s: %d ", "xcorr lambda_ref", peak_idx); cpl_vector_dump(lambda_ref, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr lambda_tmp", peak_idx); cpl_vector_dump(lambda_tmp, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr sigma_tmp", peak_idx); cpl_vector_dump(sigma_tmp, IDL);
        }

        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_subtract(lambda_tmp,lambda_ref));
        l_median = cpl_vector_get_median_const(lambda_tmp);
        l_stdev = cpl_vector_get_stdev(lambda_tmp);
        s_median = cpl_vector_get_median_const(sigma_tmp);
        s_stdev = cpl_vector_get_stdev(sigma_tmp);
        l_limit = 3. * l_stdev;
        s_limit = 3. * s_stdev;
        cpl_vector_delete(lambda_ref);
        cpl_vector_delete(lambda_tmp);
        cpl_vector_delete(sigma_tmp);
        if (IDL !=NULL ) fprintf(IDL, "%20s: %f %f %f %f %f %f\n", "xcorr limits", l_median, l_stdev, l_limit, s_median, s_stdev, s_limit);

        for (i = 0; i < no_peaks; i++){
           if (invalid_index[i] == 0) {
               double tmp_lambda = cpl_vector_get(ref_fit,i) - cpl_vector_get(peaks,i);
               double tmp_sigma =  cpl_vector_get(sigma_fit,i);
               if (fabs(tmp_lambda - l_median) >  l_limit) {
                   invalid_index[i] = 3;
                   cpl_msg_debug(cpl_func,
                           "Peak rejected due lambda outlier, %s spectrum, wavelength %f, index %d, limit %f, value %f",
                           stype, cpl_vector_get(peaks,i), i, l_limit, fabs(tmp_lambda - l_median));
               } else if (fabs(tmp_sigma - s_median) >  s_limit) {
                   cpl_msg_debug(cpl_func,
                           "Peak rejected due sigma outlier, %s spectrum, wavelength %f, index %d, limit %f, value %f",
                           stype, cpl_vector_get(peaks,i), i, s_limit,fabs(tmp_sigma - s_median));
                   invalid_index[i] = 4;
               }
           }
       }
       cpl_vector_delete(sigma_fit);

       // second scan object spectrum
       // find exact peak location by fitting a gaussian
       // detect and withdraw outliers
        KMO_TRY_EXIT_IF_NULL(
                lambda_fit = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                sigma_fit = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                lambda_ref = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                lambda_tmp = cpl_vector_new(no_peaks));
        KMO_TRY_EXIT_IF_NULL(
                sigma_tmp = cpl_vector_new(no_peaks));

        peak_idx = 0;
        stype="object";
        if (IDL !=NULL ) fprintf(IDL, "%20s: %s\n", "xcorr spectrum type", stype);
        for (i = 0; i < no_peaks; i++) {
            lambda_in = cpl_vector_get(peaks,i);
            sigma = 0.0;
            lambda_out = fit_peak (object, obj_size, lambda_in, halfwidth_obj, &sigma);
            if (invalid_index[i] == 0) {
                if (lambda_out > 0.) {
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(lambda_fit, i, lambda_out));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(sigma_fit, i, sigma));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(lambda_ref, peak_idx, lambda_in));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(lambda_tmp, peak_idx, lambda_out));
                    KMO_TRY_EXIT_IF_ERROR(
                            cpl_vector_set(sigma_tmp, peak_idx, sigma));
                    peak_idx++;
                } else {
                    invalid_index[i] = 5;
                    cpl_msg_debug(cpl_func,
                            "Gaussian fit failed for %s spectrum, wavelenght %f, peak index %d",
                            stype, lambda_in, i);
                }
            }
        }
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(lambda_ref,peak_idx));
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(lambda_tmp,peak_idx));
        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_set_size(sigma_tmp,peak_idx));
        if (IDL !=NULL ) {
            fprintf(IDL, "%20s: %d ", "xcorr lambda_ref", peak_idx); cpl_vector_dump(lambda_ref, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr lambda_tmp", peak_idx); cpl_vector_dump(lambda_tmp, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr sigma_tmp", peak_idx); cpl_vector_dump(sigma_tmp, IDL);
        }

        KMO_TRY_EXIT_IF_ERROR(
                cpl_vector_subtract(lambda_tmp,lambda_ref));
        l_median = cpl_vector_get_median_const(lambda_tmp);
        l_stdev = cpl_vector_get_stdev(lambda_tmp);
        s_median = cpl_vector_get_median_const(sigma_tmp);
        s_stdev = cpl_vector_get_stdev(sigma_tmp);
        l_limit = 2. * l_stdev;
        s_limit = 2. * s_stdev;
        cpl_vector_delete(lambda_ref);
        cpl_vector_delete(lambda_tmp);
        cpl_vector_delete(sigma_tmp);
        if (IDL !=NULL ) fprintf(IDL, "%20s: %f %f %f %f %f %f\n", "xcorr limits", l_median, l_stdev, l_limit, s_median, s_stdev, s_limit);

        for (i = 0; i < no_peaks; i++){
            if (invalid_index[i] == 0) {
                double tmp_lambda = cpl_vector_get(lambda_fit,i) - cpl_vector_get(peaks,i);
                double tmp_sigma =  cpl_vector_get(sigma_fit,i);
                if (fabs(tmp_lambda - l_median) >  l_limit) {
                    invalid_index[i] = 6;
                    cpl_msg_debug(cpl_func,
                            "Peak rejected due lambda outlier, %s spectrum, wavelength %f, index %d, limit %f, value %f",
                            stype, cpl_vector_get(peaks,i), i, l_limit, fabs(tmp_lambda - l_median));
                } else if (fabs(tmp_sigma - s_median) >  s_limit) {
                    cpl_msg_debug(cpl_func,
                            "Peak rejected due sigma outlier, %s spectrum, wavelength %f, index %d, limit %f, value %f",
                            stype, cpl_vector_get(peaks,i), i, s_limit,fabs(tmp_sigma - s_median));
                    invalid_index[i] = 7;
                }
            }
        }
        cpl_vector_delete(sigma_fit);

        no_valid_peaks = 0;
	char *dbg_msg;
        KMO_TRY_EXIT_IF_NULL(
			     dbg_msg = cpl_malloc((no_peaks * 2 + 1) * sizeof(char)));
	dbg_msg[no_peaks*2] = '\0';
        if (IDL !=NULL ) fprintf(IDL, "%20s: %d", "xcorr invalid peaks", no_peaks);
        for (i = 0; i < no_peaks; i++){
	    sprintf(&dbg_msg[2*i], "%2d", invalid_index[i]);
            if (invalid_index[i] == 0) {
                no_valid_peaks++;
            }
            if (IDL !=NULL ) fprintf(IDL, " %d", invalid_index[i]);
        }
        if (IDL !=NULL ) {
            fprintf(IDL, "\n");
        }
	cpl_msg_debug(__func__,"%d valid ones of %d peaks: %s", (int) no_valid_peaks, (int) no_peaks, dbg_msg);
	cpl_free(dbg_msg);

        KMO_TRY_EXIT_IF_NULL(
                final_obj_in = cpl_vector_new(no_valid_peaks));
        KMO_TRY_EXIT_IF_NULL(
                final_obj_fit = cpl_vector_new(no_valid_peaks));
        KMO_TRY_EXIT_IF_NULL(
                final_obj_diff = cpl_vector_new(no_valid_peaks));
        v_idx = 0;
        for (i = 0; i < no_peaks; i++){
            if (invalid_index[i] == 0) {
                double in = cpl_vector_get(ref_fit,i);
                double fit = cpl_vector_get(lambda_fit,i);
                KMO_TRY_EXIT_IF_ERROR(
                        cpl_vector_set(final_obj_in, v_idx, in));
                KMO_TRY_EXIT_IF_ERROR(
                        cpl_vector_set(final_obj_fit, v_idx, fit));
                KMO_TRY_EXIT_IF_ERROR(
                        cpl_vector_set(final_obj_diff, v_idx, fit-in));
                v_idx++;
            }
        }
        if (IDL !=NULL ) {
            fprintf(IDL, "%20s: %d ", "xcorr final_obj_in", no_valid_peaks); cpl_vector_dump(final_obj_in, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr final_obj_fit", no_valid_peaks); cpl_vector_dump(final_obj_fit, IDL);
            fprintf(IDL, "%20s: %d ", "xcorr final_obj_diff", no_valid_peaks); cpl_vector_dump(final_obj_diff, IDL);
        }

        cpl_size degree[1] = {2};
        KMO_TRY_EXIT_IF_NULL(
                coeffs = cpl_polynomial_new(1));

        if (cpl_vector_get_size(final_obj_in) > (degree[0] + 3)) {
            KMO_TRY_EXIT_IF_NULL(
                    samppos = cpl_matrix_wrap(1,cpl_vector_get_size(final_obj_in),
                            cpl_vector_get_data(final_obj_in)));
            KMO_TRY_EXIT_IF_ERROR(
                    cpl_polynomial_fit(coeffs, samppos, NULL, final_obj_diff, NULL, CPL_FALSE,
                            NULL, degree));
        } else {
            cpl_msg_warning(NULL,"Too few valid peaks for lambda correction using OH lines");
            // create polynomial a0=0
            cpl_size pows[1];
            pows[0] = 0;
            KMO_TRY_EXIT_IF_ERROR(
                    cpl_polynomial_set_coeff (coeffs, pows, 0.));
        }

        if (IDL !=NULL ) {
            cpl_size pows[1];
            fprintf(IDL, "%20s: %d", "xcorr polyfit", cpl_polynomial_get_degree(coeffs));
            for (i = 0; i <= cpl_polynomial_get_degree(coeffs); i++) {
                pows[0] = i;
                fprintf(IDL, " %f", cpl_polynomial_get_coeff(coeffs,pows));
            }
            fprintf(IDL, "\n");
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        if (coeffs != NULL) {
            cpl_polynomial_delete(coeffs);
            coeffs = NULL;
        }
    }

    if (ref_fit != NULL) cpl_vector_delete(ref_fit);
    if (lambda_fit != NULL) cpl_vector_delete(lambda_fit);
    if (final_obj_in != NULL) cpl_vector_delete(final_obj_in);
    if (final_obj_fit != NULL) cpl_vector_delete(final_obj_fit);
    if (final_obj_diff != NULL) cpl_vector_delete(final_obj_diff);
    if (invalid_index != NULL) cpl_free(invalid_index);
    if (samppos != NULL) cpl_matrix_unwrap(samppos);

    return coeffs;
}

void kmo_lcorr_open_debug_file (char* filename) {
    IDL = fopen(filename,"w");
}
void kmo_lcorr_close_debug_file () {
    fclose(IDL);
    IDL = NULL;
}
