/* -*- Mode: C; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim:set sw=2 sts=2 et cin: */
/*
 * This file is part of the MUSE Instrument Pipeline
 * Copyright (C) 2008-2014 European Southern Observatory
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */

/* This is the old code that works with lsf_param */

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

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

#include "muse_sky.h"
#include "muse_sky_old.h"
#include "muse_instrument.h"
#include "muse_lsf_params.h"
#include "muse_optimize.h"
#include "muse_utils.h"
#include "muse_data_format_z.h"

/** @addtogroup muse_skysub 
    @{
  */

/*----------------------------------------------------------------------------*/
/**
   @brief    Structure definition of sky parameters.

   This structure is used to hold the sky parameters for the master sky
   calculation.
 */
/*----------------------------------------------------------------------------*/
typedef struct {
  /** @brief Intensities of lines, sorted by line group */
  cpl_array *line_strength;
  /** @brief Parametrization of the continuum */
  cpl_array *continuum;
  /** @brief LSF parameters when applied to MASTER */
  muse_lsf_params *lsf;
} muse_sky_master_params;


/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Create a new sky_params structure.
   @param n_groups Number of group intensities.
   @param n_continuum Order for continuum parametrization.
   @return sky_params Structure to hold the data
 */
/*----------------------------------------------------------------------------*/
static muse_sky_master_params *
muse_sky_master_params_new(cpl_size n_groups, cpl_size n_continuum) {
  muse_sky_master_params *res = cpl_calloc(1, sizeof(muse_sky_master_params));
  res->line_strength = cpl_array_new(n_groups, CPL_TYPE_DOUBLE);
  cpl_array_fill_window_double(res->line_strength, 0, n_groups, 1.0);
  res->continuum = cpl_array_new(n_continuum, CPL_TYPE_DOUBLE);
  cpl_array_fill_window_double(res->continuum, 0, n_continuum, 0.0);
  return res;
}

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Delete an allocated muse_sky_master_params structure.
   @param aParams Structure to delete.
 */
/*----------------------------------------------------------------------------*/
static void 
muse_sky_master_params_delete(muse_sky_master_params *aParams) {
  if (aParams != NULL) {
    cpl_array_delete(aParams->line_strength);
    cpl_array_delete(aParams->continuum);
    muse_lsf_params_delete(aParams->lsf);
    cpl_free(aParams);
  }
}

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Convert a parametrization to sky parameters.
   @param aPar Parametrization values
   @param offset Pointer to offset of the first parametrization value index. 
                 Will be increased by the number of used values.
   @param ngroups Number of line groups.
   @return Pointer to a newly allocates sky parameter structure.

   This is the parametrization used in the SKY MASTER procedure.
 */
/*----------------------------------------------------------------------------*/
static muse_sky_master_params *
muse_sky_master_apply_sky_parametrization(const cpl_array *aPar,
                                          cpl_size *offset,
                                          int ngroups) {
  muse_sky_master_params *p = muse_sky_master_params_new(ngroups, 0);

  int i;
  for (i = 0; i < ngroups; i++) {
    double s = (cpl_array_get(aPar, (*offset)++, NULL));
    cpl_array_set(p->line_strength, i, s*s);
  }
  return p;
}

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Return the parametrization values of the first guess.
   @param ngroups Number of line groups.
   @return An array containing the first guess values.

   This array can be converted to sky parametern using 
   muse_sky_master_apply_sky_parametrization().
 */
/*----------------------------------------------------------------------------*/
static cpl_array *
muse_sky_master_sky_firstguess(int ngroups) {
  cpl_array *pars = cpl_array_new(0 + ngroups, CPL_TYPE_DOUBLE);
  cpl_size offset = 0;
  
  // Line strengths
  int i;
  for (i = 0; i < ngroups; i++) {
    cpl_array_set(pars, offset++, 1e-1);
  }

  if (offset != cpl_array_get_size(pars)) {
    cpl_msg_error(__func__, 
                  "inconsistent array: size %li; filled with %li values",
                  (long)cpl_array_get_size(pars), (long)offset);
  }
  return pars;
}

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Convert a parametrization to LSF parameters.
   @param aPar Parametrization values
   @param offset Pointer to offset of the first parametrization value index. 
                 Will be increased by the number of used values.
   @return Pointer to a newly allocates LSF parameter structure.

   This is the parametrization used in the SKY MASTER procedure.
 */
/*----------------------------------------------------------------------------*/
static muse_lsf_params *
muse_sky_master_apply_lsf_parametrization(const cpl_array *aPar,
                                          cpl_size *offset)
{
  muse_lsf_params *lsf = muse_lsf_params_new(1, 3, 1);
  cpl_array_set(lsf->sensitivity, 0, 1.0);
  lsf->offset = cpl_array_get(aPar, (*offset)++, NULL);
  lsf->refraction = 1.0 + cpl_array_get(aPar, (*offset)++, NULL);
  cpl_array_set(lsf->lsf_width, 0, 
                cpl_array_get(aPar, (*offset)++, NULL));
#if 1
  cpl_array_set(lsf->lsf_width, 1, 
                cpl_array_get(aPar, (*offset)++, NULL));
  cpl_array_set(lsf->lsf_width, 2, 
                cpl_array_get(aPar, (*offset)++, NULL));
#endif
  cpl_size i;
  for (i = 0; i < MAX_HERMIT_ORDER; i++) {
    cpl_array_set(lsf->hermit[i], 0, 
                  cpl_array_get(aPar, (*offset)++, NULL));
  }
  return lsf;
}

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Return the parametrization values of the first guess.
   @return An array containing the first guess values.

   This array can be converted to sky parametern using 
   muse_sky_master_apply_lsf_parametrization().
 */
/*----------------------------------------------------------------------------*/
static cpl_array *
muse_sky_master_lsf_firstguess(void) {
  cpl_array *pars = cpl_array_new(5 + MAX_HERMIT_ORDER, CPL_TYPE_DOUBLE);
  cpl_size offset = 0;

  // wavelength calibration offset
  cpl_array_set(pars, offset++, 0.0);
  // Relative refraction - 1
  cpl_array_set(pars, offset++, 0.0);

  // LSF width
  cpl_array_set(pars, offset++, 1.0);
#if 1
  cpl_array_set(pars, offset++, 0);
  cpl_array_set(pars, offset++, 0);
#endif
  // Hermitean coefficients
  cpl_size i;
  for (i = 0; i < MAX_HERMIT_ORDER; i++) {
    cpl_array_set(pars, offset++, 0.0);
  }
  if (offset != cpl_array_get_size(pars)) {
    cpl_msg_error(__func__, 
                  "inconsistent array: size %ld, filled with %ld values",
                  (long)cpl_array_get_size(pars), (long)offset);
  }
  return pars;
}
                                      
/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Convert a parametrization to sky and LSF parameters.
   @param aPar Parametrization values
   @param ngroups Number of line groups.
   @return Pointer to a newly allocates sky parameter structure.

   This is the parametrization used in the SKY MASTER procedure.
 */

/*----------------------------------------------------------------------------*/
static muse_sky_master_params *
muse_sky_master_apply_parametrization(const cpl_array *aPar, int ngroups) {
  cpl_size offset = 0;
  muse_sky_master_params *p = muse_sky_master_apply_sky_parametrization(aPar, &offset,
                                                                 ngroups);
  p->lsf = muse_sky_master_apply_lsf_parametrization(aPar, &offset);

  if (offset != cpl_array_get_size(aPar)) {
    cpl_msg_error(__func__, 
                  "inconsistent array: size %ld, read with %ld values",
                  (long)cpl_array_get_size(aPar), (long)offset);
    cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT);
    muse_sky_master_params_delete(p);
    return NULL;
  }
  return p;
}

/*----------------------------------------------------------------------------*/
/**
   @private
   @brief Simulate the spectrum for a given parameter set.
   @param aLines Sky lines table.
   @param aNgroups Number of groups contained in the sky lines table.
   @param aLambda Wavelength array [Angstrom].
   @param aPar Parameter array for relative line strengts
   @return the simulated spectrum

 */
/*----------------------------------------------------------------------------*/

static cpl_array *
simulate_master_sky_parameters(const cpl_table *aLines,
                               cpl_size aNgroups,
                               const cpl_array *aLambda,
                               const cpl_array *aPar) {
  muse_sky_master_params *p 
    = muse_sky_master_apply_parametrization(aPar, aNgroups);

  cpl_array *continuum = cpl_array_duplicate(aLambda);
  muse_cplarray_poly1d(continuum, p->continuum);

  cpl_table *lines = cpl_table_duplicate(aLines);
  muse_sky_lines_apply_strength(lines, p->line_strength);
  double maxflux = cpl_table_get_column_max(lines, "flux");
  muse_sky_lines_cut(lines, 1e-4 * maxflux); 

  cpl_array *simulated = muse_lsf_params_spectrum(aLambda, lines, p->lsf);
  cpl_array_add(simulated, continuum);

  cpl_array_delete(continuum);
  cpl_table_delete(lines);
  muse_sky_master_params_delete(p);
  return simulated;

}

typedef struct {
  const cpl_array *lambda; // wavelength
  const cpl_array *values; // data values
  const cpl_array *stat; // data statistics
  const cpl_table *sky; // constant sky physics data
  const cpl_size ngroups; // number of groups in the data
} muse_master_fit_struct;


/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Evaluate a given parameter set for the MASTER SKY calc.
   @param aData Data forwarded from the fit algorithm call.
   @param aPar  Current fit parameter.
   @param aRetval Return value vector.
   @return CPL_ERROR_NONE if everything went OK. 
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
muse_sky_master_eval(void *aData, cpl_array *aPar, cpl_array *aRetval) {
  cpl_size size = cpl_array_get_size(aRetval);
  muse_master_fit_struct *data = aData;
  cpl_array *simulated 
    = simulate_master_sky_parameters(data->sky, data->ngroups,
                                     data->lambda, aPar);

  cpl_array_subtract(simulated, data->values);
  cpl_array *dsimulated = muse_cplarray_diff(simulated, 1);
  cpl_array_divide(dsimulated, data->stat);

  cpl_array_fill_window_double(aRetval, 0, size, 0.0);
  memcpy(cpl_array_get_data_double(aRetval),
         cpl_array_get_data_double_const(dsimulated),
         size * sizeof(double));

  cpl_array_delete(simulated);
  cpl_array_delete(dsimulated);

  return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
   @private
   @brief Correct the first guess by data.
   @param aLines Sky lines table.
   @param aNgroups Number of groups contained in the sky lines table.
   @param aLambda Wavelength array [Angstrom].
   @param aData Measured sky spectrum.
   @param aPars Parameter array for relative line strengts
   @return CPL_ERROR_NONE

   This function computes the spectrum from aPars, takes the bin for
   the strongest line for each group and compares it with the data
   value at that bin. Their ratio gives the correction factor for this
   line group strength.
   
 */
/*----------------------------------------------------------------------------*/

static cpl_error_code
muse_sky_master_correct_firstguess(const cpl_table *aLines,
                                   cpl_size aNgroups,
                                   const cpl_array *aLambda,
                                   const cpl_array *aData,
                                   cpl_array *aPars) {

  // evaluate the first guess
  cpl_array *simulated 
    = simulate_master_sky_parameters(aLines, aNgroups, aLambda, aPars);

  cpl_size offset = 0;
  muse_sky_master_params *msp 
    = muse_sky_master_apply_sky_parametrization(aPars, &offset, aNgroups);
  cpl_table *lines = cpl_table_duplicate(aLines);
  muse_sky_lines_apply_strength(lines, msp->line_strength);
  muse_sky_master_params_delete(msp);
  cpl_size i_group;
  double delta = 0.0;
  for (i_group = 0; i_group < aNgroups; i_group++) {
    // take the strongest line of each group
    cpl_table_unselect_all(lines);
    cpl_table_or_selected_int(lines, "group", CPL_EQUAL_TO, i_group);
    cpl_table *gtable = cpl_table_extract_selected(lines);
    cpl_size row;
    cpl_table_get_column_maxpos(gtable, "flux", &row);
    double wavelength = cpl_table_get_double(gtable, "lambda", row, NULL);
    cpl_table_delete(gtable);

    // divide measured data and first guess result
    cpl_size i_lbda1 = muse_cplarray_find_sorted(aLambda, wavelength - 2.0);
    cpl_size i_lbda2 = muse_cplarray_find_sorted(aLambda, wavelength + 2.0);
    double y_data = 0;
    double y_sim = 0;
    double avg_data = 0;
    double avg_sim = 0;
    cpl_size i_lbda;
    for (i_lbda = i_lbda1; i_lbda <= i_lbda2; i_lbda++) {
      double lbda = cpl_array_get(aLambda, i_lbda, NULL);
      double wy_data = cpl_array_get(aData, i_lbda, NULL);
      double wy_sim = cpl_array_get(simulated, i_lbda, NULL);
      y_data += wy_data;
      avg_data += wy_data * lbda;
      y_sim += wy_sim;
      avg_sim += wy_sim * lbda;
    }

    // take this as correction factor
    if (y_sim > 0) {
      cpl_array_set(aPars, i_group, 
                    cpl_array_get(aPars, i_group, NULL)*sqrt(y_data/y_sim));
      avg_data /= y_data;
      avg_sim /= y_sim;
      delta += avg_data - avg_sim;
    }
  }
  cpl_array_set(aPars, aNgroups,
                cpl_array_get(aPars, aNgroups, NULL) + delta/aNgroups);
  cpl_table_delete(lines);
  cpl_array_delete(simulated);

  return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
   @brief Fit all entries of the pixel table to the master sky.
   @param aSpectrum Measured sky spectrum.
   @param aLines Sky lines table.

   @cpl_ensure_code{aSpectrum != NULL, CPL_ERROR_NULL_INPUT}
   @cpl_ensure_code{aLines != NULL, CPL_ERROR_NULL_INPUT}

   @error{set CPL_ERROR_DATA_NOT_FOUND\, return NULL, aStat has no entries}
 */
/*----------------------------------------------------------------------------*/
cpl_error_code
muse_sky_lines_fit_old(cpl_table *aSpectrum, cpl_table *aLines)
{
  cpl_ensure_code(aSpectrum, CPL_ERROR_NULL_INPUT);
  cpl_ensure_code(aLines, CPL_ERROR_NULL_INPUT);

  cpl_array *lambda = muse_cpltable_extract_column(aSpectrum, "lambda");
  cpl_array *data = muse_cpltable_extract_column(aSpectrum, "data");
  cpl_array *stat2 = muse_cpltable_extract_column(aSpectrum, "stat");

  cpl_size nstat = cpl_array_get_size(stat2);
  cpl_ensure_code(nstat > 0, CPL_ERROR_DATA_NOT_FOUND);
  cpl_array *stat = cpl_array_extract(stat2, 0, nstat - 1);
  cpl_array *s2 = cpl_array_extract(stat2, 1, nstat);
  cpl_array_add(stat, s2);
  cpl_array_delete(s2);
  cpl_array_power(stat, 0.5);

  muse_master_fit_struct fit_data = {
    lambda,
    data,
    stat,
    aLines,
    cpl_table_get_column_max(aLines, "group") + 1
  };

  cpl_array *pars = muse_sky_master_sky_firstguess(fit_data.ngroups);
  cpl_array *dpars = muse_sky_master_lsf_firstguess();
  cpl_array_insert(pars, dpars, cpl_array_get_size(pars));
  cpl_array_delete(dpars);

  // run the correction twice: in the first run, mainly the offset for
  // the wavelength is calculated. In the second step, the lines strengts
  // are corrected with a better guess of the window (with the offset from 
  // the first step). Also the wavelength offset is corrected.
  muse_sky_master_correct_firstguess(aLines, fit_data.ngroups, lambda, data, pars);
  muse_sky_master_correct_firstguess(aLines, fit_data.ngroups, lambda, data, pars);
  muse_sky_master_correct_firstguess(aLines, fit_data.ngroups, lambda, data, pars);

  cpl_size size = cpl_array_get_size(lambda);
  // do the fit, ignoring possible errors
  int debug = getenv("MUSE_DEBUG_LSF_FIT")
            && atoi(getenv("MUSE_DEBUG_LSF_FIT")) > 0;
  muse_cpl_optimize_control_t ctrl = {
    -1, -1, -1, -1, // default ftol, xtol, gtol, maxiter
    debug
  };
  /* this potentially takes a long time, better output something */
  cpl_msg_info(__func__, "Starting master sky fit");
  cpl_error_code r = muse_cpl_optimize_lvmq(&fit_data, pars, size-1,
                                            muse_sky_master_eval, &ctrl);
  if (r != CPL_ERROR_NONE) { 
    cpl_msg_error(__func__, "Master sky fit failed with error code %i: %s",
                  r, cpl_error_get_message());
  } else {
    cpl_msg_info(__func__, "Master sky fit finished successfully.");
  }

  muse_sky_master_params *p = muse_sky_master_apply_parametrization(pars, fit_data.ngroups);

  cpl_array_delete(pars);

  muse_sky_lines_apply_strength(aLines, p->line_strength);
  cpl_propertylist *order = cpl_propertylist_new();
//  cpl_propertylist_append_bool(order, "lambda", FALSE);
  cpl_propertylist_append_bool(order, "flux", TRUE);
  cpl_table_sort(aLines, order);
  cpl_propertylist_delete(order);

  cpl_msg_info(__func__, "refraction index=1%s%g, offset=%f Angstrom",
               p->lsf->refraction < 1?"-":"+",
               fabs(p->lsf->refraction-1), p->lsf->offset);

  cpl_array_delete(stat);
  muse_sky_master_params_delete(p);

  cpl_array_unwrap(lambda);
  cpl_array_unwrap(data);
  cpl_array_unwrap(stat2);

  return CPL_ERROR_NONE;
}
/**
   @brief Subtract sky lines from a pixtable.
   @param aPixtable   Pixel table. The <tt>data</tt> column will be changed.
   @param aLines      Line list with individual line fluxes
   @param aLsfParams  LSF parameters for the instrument
   @retval CPL_ERROR_NONE if everything was OK.
   @cpl_ensure{aPixtable != NULL, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT}
   @cpl_ensure{aPixtable->table != NULL, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT}
   @cpl_ensure{aPixtable->table is a pixel table, CPL_ERROR_DATA_NOT_FOUND, CPL_ERROR_DATA_NOT_FOUND}
   @cpl_ensure{aLines != NULL, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT}
   @cpl_ensure{aLsfParams != NULL, CPL_ERROR_NULL_INPUT, CPL_ERROR_NULL_INPUT}
   @remark This function adds a FITS header (@ref MUSE_HDR_PT_SKYSUB) with the
           boolean value 'T' to the pixel table, for information.
 */
cpl_error_code
muse_sky_subtract_lines_old(muse_pixtable *aPixtable, cpl_table *aLines,
                               muse_lsf_params **aLsfParams)
{
  cpl_ensure_code(aPixtable != NULL, CPL_ERROR_NULL_INPUT);
  cpl_ensure_code(aPixtable->table != NULL, CPL_ERROR_NULL_INPUT);
  cpl_ensure_code(aLines != NULL, CPL_ERROR_NULL_INPUT);

  muse_pixtable **slice_pixtable = muse_pixtable_extracted_get_slices(aPixtable);
  cpl_size n_slices = muse_pixtable_extracted_get_size(slice_pixtable);
  cpl_size i_slice;
  cpl_msg_info(__func__, "Starting sky subtraction of %"CPL_SIZE_FORMAT" slices",
               n_slices);
  cpl_boolean debug = getenv("MUSE_DEBUG_SKY")
                    && atoi(getenv("MUSE_DEBUG_SKY")) > 0;
  #pragma omp parallel for default(none)                 /* as req. by Ralf */ \
              shared(aLsfParams, aLines, debug, n_slices, slice_pixtable)
  for (i_slice = 0; i_slice < n_slices; i_slice++) {
    uint32_t origin
      = (uint32_t)cpl_table_get_int(slice_pixtable[i_slice]->table,
                                    MUSE_PIXTABLE_ORIGIN, 0, NULL);
    int ifu = muse_pixtable_origin_get_ifu(origin);
    int slice = muse_pixtable_origin_get_slice(origin);
    muse_lsf_params *slice_params = muse_lsf_params_get(aLsfParams, ifu, slice);
    if ((slice_params == NULL) && (aLines != NULL)){
      cpl_msg_warning(__func__, "No LSF params for slice #%i.%i."
                      " Ignoring lines in sky subtraction for this slice.",
                      ifu, slice);
    }

    cpl_size nrows = muse_pixtable_get_nrow(slice_pixtable[i_slice]);
    if (debug) {
      cpl_msg_debug(__func__, "Sky subtraction of %li pixels for slice #%i.%i",
                    (long)nrows, ifu, slice);
    }
    cpl_errorstate prestate = cpl_errorstate_get();
    muse_pixtable *slice_pt = slice_pixtable[i_slice];
    cpl_propertylist *order = cpl_propertylist_new();
    cpl_propertylist_append_bool(order, MUSE_PIXTABLE_LAMBDA, CPL_FALSE);
    cpl_table_sort(slice_pt->table, order);
    cpl_propertylist_delete(order);

    cpl_table_cast_column(slice_pt->table, MUSE_PIXTABLE_LAMBDA,
                          "lambda_double", CPL_TYPE_DOUBLE);
    cpl_array *lambda = muse_cpltable_extract_column(slice_pt->table, "lambda_double");
    cpl_table_unwrap(slice_pt->table, "lambda_double");
    cpl_array *spectrum = muse_lsf_params_spectrum(lambda, aLines, slice_params);

    cpl_array *data = muse_cpltable_extract_column(slice_pt->table,
                                                   MUSE_PIXTABLE_DATA);
    cpl_array_subtract(data, spectrum);

    cpl_size ii;
    for (ii = 0; ii < cpl_array_get_size(data); ii++) {
      if (!cpl_array_is_valid(spectrum, ii)) {
        cpl_table_set_invalid(slice_pt->table, MUSE_PIXTABLE_DATA, ii);
      }
    }
    cpl_array_unwrap(data);
    cpl_array_delete(spectrum);
    cpl_array_delete(lambda);

    if (!cpl_errorstate_is_equal(prestate)) {
      cpl_errorstate_dump(prestate, CPL_FALSE, NULL);
      cpl_errorstate_set(prestate);
    }
  }
  muse_pixtable_extracted_delete(slice_pixtable);

  if (aPixtable->header) {
    /* add the status header */
    cpl_propertylist_update_bool(aPixtable->header, MUSE_HDR_PT_SKYSUB,
                                 CPL_TRUE);
    cpl_propertylist_set_comment(aPixtable->header, MUSE_HDR_PT_SKYSUB,
                                 MUSE_HDR_PT_SKYSUB_COMMENT);
  }
  return CPL_ERROR_NONE;
}

/**@}*/
