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

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

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

#include "muse_sky.h"
#include "muse_instrument.h"
#include "muse_lsf.h"
#include "muse_optimize.h"

/** @addtogroup muse_skysub
    @{
  */

cpl_array *
muse_sky_slice_lsf_firstguess(const muse_sky_fit_params *aFitParams);
cpl_array *
muse_sky_slice_lsf_set_param(muse_lsf_params *aLsf,
                             const muse_sky_fit_params *aFitParams);
muse_lsf_params *
muse_sky_slice_apply_lsf_parametrization(const muse_lsf_params *template,
                                         const cpl_array *aPar,
                                         const muse_sky_fit_params *aFitParams);

typedef struct {
  cpl_array *lambda;
  cpl_array *values;
  cpl_array *stat;
  cpl_array *linesLambda;
  cpl_array *linesFlux;
  const muse_sky_fit_params *fit_params;
  muse_lsf_params *firstGuess;
} muse_lsf_fit_struct;

/*----------------------------------------------------------------------------*/
/**
  @private
   @brief Evaluate a given parameter set
   @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_lsf_slice_eval(void *aData, cpl_array *aPar, cpl_array *aRetval) {

  muse_lsf_fit_struct *data = aData;
  cpl_size size = cpl_array_get_size(aRetval);

  muse_lsf_params *lsfParam
    = muse_sky_slice_apply_lsf_parametrization(data->firstGuess,
                                                    aPar, data->fit_params);
  cpl_array *linesFlux = data->linesFlux;
  if (data->linesFlux == NULL) {
    linesFlux = cpl_array_extract(aPar, cpl_array_get_size(aPar)
                                  - cpl_array_get_size(data->linesLambda),
                                  cpl_array_get_size(data->linesLambda));
  }

  cpl_array *simulated
    = muse_lsf_spectrum_get_lines(data->lambda, data->linesLambda,
                                  linesFlux, lsfParam);

  if (data->linesFlux == NULL) {
    cpl_array_delete(linesFlux);
  }

  muse_lsf_params_delete_one(lsfParam);
  cpl_array_subtract(simulated, data->values);
  cpl_array_divide(simulated, 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(simulated),
         size * sizeof(double));

  // replace invalid numbers by 0.0
  cpl_size i;
  double *d = cpl_array_get_data_double(aRetval);
  for (i = 0; i < size; i++) {
    if (isnan(d[i])) {
      d[i] = 0.0;
    }
  }

  cpl_array_delete(simulated);
  return CPL_ERROR_NONE;
}

/*----------------------------------------------------------------------------*/
/**
   @brief Fit all entries of one slice.
   @param aLambda Wavelength array [Angstrom].
   @param aData Measured spectrum.
   @param aStat The variance spectrum.
   @param aLines @ref muse_sky_lines_lines_def "List of emission lines".
   @param aFirstGuess First guess for LSF params, or NULL.
   @param aMaxIter Maximum number of iterations.
   @param aFitParams Specification which parameters to fit
   @return The fitted LSF parameters

   As a quality measure, the LSF fitted lines are subtracted from the
   measured spectrum.
 */
/*----------------------------------------------------------------------------*/
muse_lsf_params *
muse_lsf_params_fit(const cpl_array *aLambda, cpl_array *aData,
                             const cpl_array *aStat,
                             const cpl_table *aLines,
                             muse_lsf_params *aFirstGuess,
                             int aMaxIter,
                             const muse_sky_fit_params *aFitParams) {

  muse_lsf_params *firstGuess = (aFirstGuess != NULL)?
    aFirstGuess: muse_lsf_params_new(1, 3, 1);

  int debug = getenv("MUSE_DEBUG_LSF_FIT")
            && atoi(getenv("MUSE_DEBUG_LSF_FIT")) > 0;
  muse_cpl_optimize_control_t ctrl = {
    -1, -1, -1, // default ftol, xtol, gtol
    aMaxIter, debug
  };

  //
  // Create "lambda", "data", and "stat" fields from the pixel table.
  // They do not contain all pixels, but a certain percentage of them.
  //
  cpl_size reduction = 2; // we use only 50% of all pixels for the fit (speed).
  cpl_size size = cpl_array_get_size(aLambda)/reduction;
  cpl_array *lambda = cpl_array_new(size, CPL_TYPE_DOUBLE);
  cpl_array *data = cpl_array_new(size, CPL_TYPE_DOUBLE);
  cpl_array *stat = cpl_array_new(size, CPL_TYPE_DOUBLE);

  cpl_size i;
  for (i = 0; i < size; i++) {
    cpl_array_set(lambda, i, cpl_array_get(aLambda, reduction * i, NULL));
    cpl_array_set(data, i, cpl_array_get(aData, reduction * i, NULL));
    cpl_array_set(stat, i, sqrt(cpl_array_get(aStat, reduction * i, NULL)));
  }

  cpl_array *linesLambda = muse_cpltable_extract_column((cpl_table *)aLines,
                                                        "lambda");

  //
  // First minimization step: Fit the lsf width and the fluxes of all lines
  //
  muse_sky_fit_params *slice_fit_params0 = muse_sky_fit_params_new
    (
     0, // aParams->slice_fit_offset,
     0, // aParams->slice_fit_refraction,
     0, // sensitivity is not used here
     0, // aParams->slice_fit_slit_width,
     0, // aParams->slice_fit_bin_width,
     aFitParams->lsf_width, // aParams->slice_fit_lsf_width + 1,
     0, // aParams->slice_fit_h3 + 1,
     0, // aParams->slice_fit_h4 + 1,
     0, // aParams->slice_fit_h5 + 1,
     0  // aParams->slice_fit_h6 + 1
     );

  muse_lsf_fit_struct fit_data = {
    lambda,
    data,
    stat,
    linesLambda,
    NULL, // linesFlux
    slice_fit_params0,
    firstGuess,
  };

  cpl_array *pars = (aFirstGuess == NULL)?
    muse_sky_slice_lsf_firstguess(slice_fit_params0):
    muse_sky_slice_lsf_set_param(aFirstGuess, slice_fit_params0);

  // Take the lines fluxes as they come out of the line list as first guess
  cpl_array *lf = muse_cpltable_extract_column((cpl_table *)aLines, "flux");
  cpl_array *linesFlux = cpl_array_cast(lf, CPL_TYPE_DOUBLE);
  cpl_array_unwrap(lf);
  cpl_array_insert(pars, linesFlux, cpl_array_get_size(pars));

  cpl_error_code r
    = muse_cpl_optimize_lvmq(&fit_data, pars, size,
                             muse_lsf_slice_eval, &ctrl);

  if (r != CPL_ERROR_NONE) { // on error: reset to first guess
    cpl_array_delete(pars);
    pars = (aFirstGuess == NULL)?
      muse_sky_slice_lsf_firstguess(aFitParams):
      muse_sky_slice_lsf_set_param(aFirstGuess, aFitParams);
    cpl_array_insert(pars, linesFlux, cpl_array_get_size(pars));
  }

  //
  // Second minimization step: Keep the line fluxes constant and fit
  // all shape parameters
  //
  fit_data.fit_params = aFitParams;
  cpl_array_delete(linesFlux);
  linesFlux = cpl_array_extract(pars, cpl_array_get_size(pars)
                                - cpl_array_get_size(linesLambda),
                                cpl_array_get_size(linesLambda));
  fit_data.linesFlux = linesFlux;
  fit_data.firstGuess
    = muse_sky_slice_apply_lsf_parametrization(firstGuess,
                                                    pars, slice_fit_params0);
  muse_sky_fit_params_delete(slice_fit_params0);
  cpl_array_delete(pars);
  pars = muse_sky_slice_lsf_set_param(fit_data.firstGuess, aFitParams);

  r = muse_cpl_optimize_lvmq(&fit_data, pars, size, muse_lsf_slice_eval, &ctrl);
  if (r != CPL_ERROR_NONE) { // on error: reset to first guess
    cpl_array_delete(pars);
    pars = (aFirstGuess == NULL)?
      muse_sky_slice_lsf_firstguess(aFitParams):
      muse_sky_slice_lsf_set_param(aFirstGuess, aFitParams);
    cpl_array_insert(pars, linesFlux, cpl_array_get_size(pars));
  }

  muse_lsf_params *lsfParam
    = muse_sky_slice_apply_lsf_parametrization(firstGuess,
                                                    pars, aFitParams);
  cpl_msg_debug(__func__, "Slit width: %f (%s), bin width: %f (%s)",
                lsfParam->slit_width, aFitParams->slit_width?"fit":"fixed",
                lsfParam->bin_width, aFitParams->bin_width?"fit":"fixed");

  cpl_array *simulated = muse_lsf_spectrum_get_lines(aLambda, linesLambda,
                                                     linesFlux, lsfParam);
  cpl_array_delete(linesFlux);
  cpl_array_subtract(aData, simulated);
  cpl_array_delete(simulated);

  cpl_array_delete(pars);
  cpl_array_unwrap(fit_data.linesLambda);
  cpl_array_delete(fit_data.lambda);
  cpl_array_delete(fit_data.values);
  cpl_array_delete(fit_data.stat);
  muse_lsf_params_delete_one(fit_data.firstGuess);
  if (aFirstGuess == NULL) {
    // remove temporary first guess template
    muse_lsf_params_delete_one(firstGuess);
  }

  return lsfParam;
}

/**@}*/
