/*
 * This file is part of the GIRAFFE Pipeline
 * Copyright (C) 2002-2019 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 St, Fifth Floor, Boston, MA  02110-1301  USA
 */

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

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

#include <cxmemory.h>
#include <cxmessages.h>

#include "gimacros.h"
#include "gimath.h"
#include "gimath_lm.h"
#include "gimessages.h"

/**
 * @defgroup gimath_lm Levenberg-Marquardt Mathematical Utility Functions
 *
 * The module implements Levenberg-Marquardt Mathematical Utility Functions
 *
 * @par Synopis:
 * @code
 *   #include <gimath_lm.h>
 * @endcode
 *
 * @see mymrqmin()
 *
 */

#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}


/**
 * @var lmrq_model lmrq_models[]
 *
 * @brief
 *   List of available model functions
 */

lmrq_model lmrq_models[] = {
    {LMRQ_GAUSSUM,   mrqgaussum,   4, 1, "gaussum",   LINE_MODEL},
    {LMRQ_XOPTMOD,   mrqxoptmod,   7, 3, "xoptmod",   XOPT_MODEL},
    {LMRQ_XOPTMODGS, mrqxoptmodGS, 7, 3, "xoptmodGS", XOPT_MODEL},
    {LMRQ_XOPTMOD2,  mrqxoptmod2, 10, 3, "xoptmod2",  XOPT_MODEL},
    {LMRQ_PSFCOS,    mrqpsfcos,    5, 1, "psfcos",    LINE_MODEL},
    {LMRQ_PSFEXP,    mrqpsfexp,    5, 1, "psfexp",    LINE_MODEL},
    {LMRQ_YOPTMOD,   mrqyoptmod,   7, 3, "yoptmod",   YOPT_MODEL},
    {LMRQ_YOPTMOD2,  mrqyoptmod2, 10, 3, "yoptmod2",  YOPT_MODEL},
    {LMRQ_LOCYWARP,  mrqlocywarp,  5, 4, "locywarp",  LOCY_MODEL},
    {LMRQ_PSFEXP2,   mrqpsfexp2,   5, 1, "psfexp2",   LINE_MODEL},
    {LMRQ_TEST,      mrqtest,      2, 1, "test",      LINE_MODEL}
};

cxint nr_lmrq_models = CX_N_ELEMENTS(lmrq_models);


/**
 * @brief
 *   covariance_sort
 *
 * @param covar - Matrix containing covariance values [ma,ma]
 * @param ma    - Number of fit parameters
 * @param ia    - Fit flags [ma]
 * @param mfit  - Start index of sort [1..ma]
 *
 * @par Description
 * This function expands and contracts the covariance matrix
 * to account for the fact that there may be components of the
 * state which we may hold constant and not optimize.
 *
 */

static void
covariance_sort(cpl_matrix *covar, cxint ma, cxint ia[], cxint mfit)
{

    register cxint i, j, k;
    register cxdouble swap;

    cxdouble *pd_covar = NULL;
    cxint     nr_covar;

    pd_covar = cpl_matrix_get_data(covar);
    nr_covar = cpl_matrix_get_nrow(covar);

    for (i = mfit; i < ma; i++)
        for (j = 0; j <= i; j++)
            pd_covar[i * nr_covar + j] = pd_covar[j * nr_covar + i] = 0.0;

    k = mfit - 1;
    for (j = (ma-1); j >= 0; j--) {
        if (ia[j]) {
            for (i = 0; i < ma; i++)
                SWAP(pd_covar[i * nr_covar + k],pd_covar[i * nr_covar + j])

            for (i = 0;i < ma; i++)
                SWAP(pd_covar[k * nr_covar + i],pd_covar[j * nr_covar + i])

            k--;
        }
    }

} /* end covariance_sort() */

#undef SWAP

/**
 * @brief
 *   mrqdydaweight
 *
 * @param x
 * @param x0
 * @param dx
 *
 * @par Description
 * Helper function to calculate a weighted exponential used in several lmrq
 * routines.
 *
 * @code
 *
 *                       DW_DEGREE
 *        exp( - | x-x0 |          )
 *  w = -----------------------------
 *           / DW_DEGREE \
 *           | --------- |
 *           \ DW_LOG001 /
 *        dx
 *
 * @endcode
 *
 */

static double
mrqdydaweight(cxdouble x, cxdouble x0, cxdouble dx)
{
    register cxdouble w;

    w = exp(-pow(fabs(x-x0),DW_DEGREE)/pow(dx,DW_DEGREE/DW_LOG001));

    if (isnan(w))
        w = 1;

    return w;
}

/**@{*/

/**
 * @brief
 *   Levenberg-Marquardt non linear fit control loop
 *
 * @param x          -  X abcissa [ndata]
 * @param y          -  Y values  [ndata]
 * @param sig        -  Y sigmas [ndata]
 * @param ndata      -  Number of values
 * @param a          -  Initial guesses for model parameters [ma]
 * @param r          -  Maximum delta for model parameters [ma]
 * @param ia         -  Flags for model parameters to be fitted [ma]
 * @param ma         -  Number of model parameters to fit
 * @param fit_params -  Non linear fit parameters
 * @param funcs      -  Non linear model to fit
 *
 * @return niter     -  Number of iterations performed or negative error code
 * @retval chisq     -  Chi square of fit
 * @retval alpha     -  Fitted parameters [ma,ma]
 *
 * @see lmrq_params
 * @see fitted_func
 *
 */

cxint
mrqnlfit(
    cpl_matrix  *x,
    cpl_matrix  *y,
    cpl_matrix  *sig,
    cxint        ndata,
    cpl_matrix  *a,
    cxdouble     r[],
    cxint        ia[],
    cxint        ma,
    cpl_matrix  *alpha,
    cxdouble    *chisq,
    lmrq_params  fit_params,
    fitted_func  funcs
) {

    cxint       itst,
                n,
                res;

    cxdouble    alamda,
                ochisq;

    cpl_matrix *beta = NULL;

    /*************************************************************************
                                      PROCESSING
    *************************************************************************/

    beta = cpl_matrix_new(ma,ma);

    alamda = -1.0;

    res = mymrqmin(x, y, sig, ndata, a, r, ia, ma, alpha, beta, chisq,
                   funcs, &alamda);

    if (res != 0) {
        cpl_matrix_delete(beta); beta = NULL;
        return res;
    }

    itst=0;

    for (n = 1; n <= fit_params.imax; n++) {

        ochisq = *chisq;

        res = mymrqmin(x, y, sig, ndata, a, r, ia, ma, alpha, beta, chisq,
                       funcs, &alamda);

        if (res!=0) {
            cpl_matrix_delete(beta); beta = NULL;
            return res;
        }

        if (*chisq > ochisq)
            itst=0;
        else if (fabs(ochisq-*chisq) < fit_params.dchsq)
            itst++;

        if (itst > fit_params.tmax)
            break;
    }

    /* get covariance matrix */
    alamda=0.0;

    res = mymrqmin(x, y, sig, ndata, a, r, ia, ma, alpha, beta, chisq,
                   funcs, &alamda);

    if (res != 0) {
        cpl_matrix_delete(beta); beta = NULL;
        return res;
    }

    cpl_matrix_delete(beta); beta = NULL;

    return n;

} /* end mrqnlfit() */

/**
 * @brief
 *   Levenberg-Marquardt non linear fit routine
 *
 * @param x          -  X abcissa [ndata]
 * @param y          -  Y values [ndata]
 * @param sig        -  Y sigmas [ndata]
 * @param ndata      -  Number of values
 * @param a          -  Initial guesses for model parameters [ma]
 * @param r          -  Maximum delta for model parameters [ma]
 * @param ia         -  Flags fot model parameters to be fitted [ma]
 * @param ma         -  Number of parameters to fit
 * @param covar      -  Covariance matrix [ma,ma]
 * @param alpha      -  Working space [ma,ma]
 * @param chisq      -  Chi Square of fit
 * @param funcs      -  Non linear model to fit
 * @param alamda     -  Control parameter of fit
 *
 * @par Description:
 * Levenberg-Marquardt non linear fit method, based upon attempting to
 * reduce the value @em CHISQ of a fit between a set of data points
 * @a x[1..ndata], @a y[1..ndata] with individual standard deviations
 * @a sig[1..ndata], and a nonlinear function @a funcs dependent on
 * @a ma coefficients @a a[1..ma].
 * @par Fit Control Parameters:
 * The input array @a a[1..ma] contains initial guesses for the parameters
 * to be fitted.
 * The input array @a ia[1..ma] indicates by nonzero entries those components
 * of @a a[1..ma] that should be fitted for, and by zero entries those
 * components that should be held fixed at their input values.
 * @par
 * The program returns current best-fit values for the parameters @a a[1..ma],
 * and @em CHISQ=chisq. The arrays @a covar[1..ma][1..ma] and
 * @a alpha[1..ma][1..ma] are used as working space during most iterations.
 * @par
 * Supply a routine @a funcs(x,a,yfit,dyda,ma) that evaluates the fitting
 * function yfit, and its derivatives @em dyda[1..ma] with respect to the
 * fitting parameters @a a at @a x. On the first call provide an initial
 * guess for the parameters @a a, and set @a alamda<0  for initialization
 * (which then sets @a alamda=.001). If a step succeeds @a chisq becomes
 * smaller and @a alamda decreases by a factor of 10. If a step fails
 * @a alamda grows by a factor of 10.
 * @par
 * You @em must call this routine repeatedly until convergence is achieved.
 * Then, make one final call with @a alamda=0, so that @a covar[1..ma][1..ma]
 * returns the covariance matrix, and @a alpha[1..ma][1..ma] the
 * curvature matrix.
 * @par
 * Parameters held fixed will return zero covariances.
 *
 * @return  0 if succesful, <0 if an error occured
 *
 * @see mymrqcof()
 *
 */

cxint
mymrqmin(
    cpl_matrix  *x,
    cpl_matrix  *y,
    cpl_matrix  *sig,
    cxint        ndata,
    cpl_matrix  *a,
    cxdouble     r[],
    cxint        ia[],
    cxint        ma,
    cpl_matrix  *covar,
    cpl_matrix  *alpha,
    cxdouble    *chisq,
    fitted_func  funcs,
    cxdouble    *alamda
) {

    register cxint    gj, j, k, l, m;

    static cxdouble   *pd_a, *pd_covar, *pd_alpha;
    static cxint       nr_covar, nr_alpha, nr_moneda, mfit;

    static cpl_matrix *matry, *mbeta, *mda, *moneda;
    static cxdouble   *atry,  *beta,  *da,  *oneda, ochisq;

    /*************************************************************************
                                      PROCESSING
    *************************************************************************/

    pd_a     = cpl_matrix_get_data(a);
    pd_covar = cpl_matrix_get_data(covar);
    pd_alpha = cpl_matrix_get_data(alpha);
    nr_covar = cpl_matrix_get_nrow(covar);
    nr_alpha = cpl_matrix_get_nrow(alpha);

    if (*alamda<0.0) {

        matry = cpl_matrix_new(ma,1);
        atry  = cpl_matrix_get_data(matry);

        mbeta = cpl_matrix_new(ma,1);
        beta  = cpl_matrix_get_data(mbeta);

        mda = cpl_matrix_new(ma,1);
        da  = cpl_matrix_get_data(mda);

        for (mfit = 0, j = 0; j < ma; j++)
            if (ia[j])
                mfit++;

        moneda    = cpl_matrix_new(1,mfit);
        oneda     = cpl_matrix_get_data(moneda);

        *alamda = 0.001;

        gj = mymrqcof(x, y, sig, ndata, a, r, ia, ma, alpha, mbeta,
                      chisq, funcs);

        if (gj != 0) {
            cpl_matrix_delete(moneda); moneda = NULL; oneda = NULL;
            cpl_matrix_delete(mda);    mda    = NULL; da    = NULL;
            cpl_matrix_delete(mbeta);  mbeta  = NULL; beta  = NULL;
            cpl_matrix_delete(matry);  matry  = NULL; atry  = NULL;
            return gj;
        }

        ochisq = (*chisq);

        for (j = 0; j < ma; j++)
            atry[j] = pd_a[j];

    }

    nr_moneda = cpl_matrix_get_nrow(moneda);

    for (j = -1, l = 0; l < ma; l++) {
        if (ia[l]) {
            for (j++, k = -1, m = 0; m < ma; m++) {
                if (ia[m]) {
                    k++;
                    pd_covar[j * nr_covar + k] = pd_alpha[j * nr_alpha + k];
                }
            }

            pd_covar[j * nr_covar + j] =
                pd_alpha[j * nr_alpha + j] * (1.0 + (*alamda));

            oneda[j * nr_moneda + 0] = beta[j];
        }
    }

    gj = giraffe_gauss_jordan(covar, mfit, moneda, 1);

    if (gj != 0) {
        cpl_matrix_delete(moneda); moneda = NULL; oneda = NULL;
        cpl_matrix_delete(mda);    mda    = NULL; da    = NULL;
        cpl_matrix_delete(mbeta);  mbeta  = NULL; beta  = NULL;
        cpl_matrix_delete(matry);  matry  = NULL; atry  = NULL;
        return gj;
    }

    for (j = 0; j < mfit; j++)
        da[j] = oneda[j * nr_moneda + 0];

    if (*alamda == 0.0) {
        covariance_sort(covar, ma, ia, mfit);
        cpl_matrix_delete(moneda); moneda = NULL; oneda = NULL;
        cpl_matrix_delete(mda);    mda    = NULL; da    = NULL;
        cpl_matrix_delete(mbeta);  mbeta  = NULL; beta  = NULL;
        cpl_matrix_delete(matry);  matry  = NULL; atry  = NULL;
        return 0;
    }

    for (j = -1, l = 0; l < ma; l++)
        if (ia[l])
            atry[l] = pd_a[l] + da[++j];

    gj = mymrqcof(x, y, sig, ndata, matry, r, ia, ma, covar, mda,
                  chisq, funcs);

    if (gj != 0) {
        cpl_matrix_delete(moneda); moneda = NULL; oneda = NULL;
        cpl_matrix_delete(mda);    mda    = NULL; da    = NULL;
        cpl_matrix_delete(mbeta);  mbeta  = NULL; beta  = NULL;
        cpl_matrix_delete(matry);  matry  = NULL; atry  = NULL;
        return gj;
    }

    if (*chisq < ochisq) {

        *alamda *= 0.1;
        ochisq = *chisq;

        for (j = -1, l = 0; l < ma; l++) {
            if (ia[l]) {
                for (j++, k = -1, m = 0; m < ma; m++) {
                    if (ia[m]) {
                        k++;
                        pd_alpha[j * nr_alpha + k] =
                            pd_covar[j * nr_covar + k];
                    }
                }

                beta[j] = da[j];
                pd_a[l] = atry[l];
            }
        }

    } else {
        *alamda *= 10.0;
        *chisq = ochisq;
    }

    return 0;

} /* end mymrqmin() */

/**
 * @brief
 *   LMRQ Chi Square Calculation
 *
 * @param x          -  X abcissa [ndata]
 * @param y          -  Y values [ndata]
 * @param sig        -  Y sigmas [ndata]
 * @param ndata      -  Number of values
 * @param a          -  Initial guesses for model parameters [ma]
 * @param r          -  Maximum deltat for modelparameters [ma]
 * @param ia         -  Flags for model parameters to be fitted [ma]
 * @param ma         -  Number of parameters to fit
 * @param alpha      -  Working space [ma,ma]
 * @param beta       -  Working space [ma,ma]
 * @param chisq      -  Chi Square value of fit
 * @param funcs      -  Non linear model to fit
 *
 * @par Description:
 * Used by @c mymrqmin() to evaluate the linearized fitting matrix @a alpha
 * and vector @a beta and calculate chi squared @a chisq.
 *
 * @return =0 if succesful, <0 if error an occured
 *
 * @see mymrqmin()
 *
 */

cxint
mymrqcof(
    cpl_matrix  *x,
    cpl_matrix  *y,
    cpl_matrix  *sig,
    cxint        ndata,
    cpl_matrix  *a,
    cxdouble     r[],
    cxint        ia[],
    cxint        ma,
    cpl_matrix  *alpha,
    cpl_matrix  *beta,
    cxdouble    *chisq,
    fitted_func  funcs
) {

    register cxint i, j, k, l, m, mfit = 0;

    cxdouble  ymod, wt, sig2i, dy, *dyda;

    cxdouble *pd_x     = NULL,
             *pd_y     = NULL,
             *pd_sig   = NULL,
             *pd_a     = NULL,
             *pd_alpha = NULL,
             *pd_beta  = NULL;

    cxint     nr_alpha, nc_x;

    /************************************************************************
                                     PROCESSING
    ************************************************************************/

    pd_x     = cpl_matrix_get_data(x);
    nc_x     = cpl_matrix_get_ncol(x);
    pd_y     = cpl_matrix_get_data(y);
    pd_sig   = cpl_matrix_get_data(sig);
    pd_a     = cpl_matrix_get_data(a);
    pd_alpha = cpl_matrix_get_data(alpha);
    nr_alpha = cpl_matrix_get_nrow(alpha);
    pd_beta  = cpl_matrix_get_data(beta);

    for (j = 0; j < ma; j++) {
        if (ia[j])
            mfit++;
    }

    for (j = 0; j < mfit; j++) {
        for (k = 0; k <= j; k++)
            pd_alpha[j * nr_alpha + k] = 0.0;

        pd_beta[j] = 0.0;
    }

    *chisq = 0.0;

    dyda = (cxdouble *) cx_calloc(ma, sizeof(cxdouble));

    for (i = 0; i < ndata; i++) {

        (*funcs)(&(pd_x[i*nc_x]), pd_a, r, &ymod, dyda, ma);

        if (pd_sig[i]==0.0) {
            continue;
        }

        sig2i = 1.0 / (pd_sig[i] * pd_sig[i]);

        dy = pd_y[i] - ymod;

        for (j = -1, l = 0; l < ma; l++) {

           if (ia[l]) {
                wt = dyda[l] * sig2i;
                for (j++, k = -1, m = 0; m <= l; m++) {
                    if (ia[m]) {
                        ++k;
                        pd_alpha[j * nr_alpha + k] += (wt * dyda[m]);
                    }
                }

                pd_beta[j] += (dy * wt);

            }
        }

        *chisq += (dy * dy * sig2i);

    }

    for (j = 1; j < mfit; j++)
        for (k = 0; k < j; k++)
            pd_alpha[k * nr_alpha + j] = pd_alpha[j * nr_alpha + k];


    cx_free(dyda);

    return 0;

} /* end mymrqcof() */

/**
 * @brief
 *   Returns R Squared of the current model
 *
 * @param resSS - TBD
 * @param y     - Y values [n]
 * @param n     - Number of values
 *
 * @return R^2 of of current model
 *
 * @par Description:
 * Used by @c mymrqmin() to evaluate the linearized fitting matrix @em alpha
 * and vector @em beta and calculate @em R squared.
 *
 */

cxdouble
r_squared(cxdouble resSS, cpl_matrix *y, cxint n)
{
    register cxint i;
    register cxdouble Sy, Syy, SS;
    cxdouble res, *pd_y = NULL;

    pd_y = cpl_matrix_get_data(y);

    if (n < 1)
        return 0.0;

    for (i=0, Sy=0.0, Syy=0.0; i<n; i++) {
        Sy  += pd_y[i];
        Syy += pd_y[i]*pd_y[i];
    }

    SS = Syy - Sy*Sy/n;
    res = resSS/SS;

    if (isnan(res))
        return 0.0;

    if (res > 0.0)
        res = sqrt(res);

    return res;

} /* end r_squared() */

/**
 * @brief
 *   Compute the sum of @em na/4 Gaussians and its derivatives
 *
 * @param x     - Initial guess : [x]
 * @param a     - Function parameters to be fitted : [amplitude,center,background,width]
 * @param r     - Not Used
 * @param na    - Number of parameters
 *
 * @code
 * y =
 *
 *                     1   / x0 - center \2
 * Amplitude *  exp( - - * | ----------- |   ) + background
 *                     2   \    width    /
 *
 *
 * @endcode
 *
 * where:
 *  - amplitude  :
 *  - center     :
 *  - background :
 *  - width      :
 *
 * @retval y    - Function value : y
 * @retval dyda - Function derivatives :
 *                [amplitude,center,background,width]
 *
 */

void
mrqgaussum(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
           cxdouble dyda[], cxint na)
{

    register cxint i, j;
    register cxdouble fac,ex,amplitude,center,backg,width,xred;

    (void) r;  /* Not used. */
    *y = 0.0;

    for (j = 0, i = 0; i < na; i += 4, j += 4) {
        amplitude = a[i];
        center    = a[i + 1];
        backg     = a[i + 2];
        width     = a[i + 3];
        xred = (x[0] - center) / width;
        ex   = exp(-xred * xred / 2.);
        fac  = amplitude * xred * ex;
        *y  += (amplitude * ex + backg);

        /* Check if derivatives expected */
        if (dyda == NULL) continue;

        /* derivatives for each parameters */
        dyda[j]     = ex;                      /* d(y)/d(amplitude) */
        dyda[j + 1] = fac / width;             /* d(y)/d(center)    */
        dyda[j + 2] = 1.;                      /* d(y)/d(backg)     */
        dyda[j + 3] = (fac * xred) / width;    /* d(y)/d(width)     */
    }

} /* end mrqgaussum() */

/**
 * @brief
 *   Compute X on ccd and its derivatives using simple optical model
 *
 * @param x     - Initial guess : [lambda,xfibre,yfibre]
 * @param a     - function parameters (to be fitted...) :
 *                [nx,ypixsize,fcoll,cfact,gtheta,gorder,gspace]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description :
 * @code
 * xccd =
 *
 *              /                           /          2     \1/2\
 *              |                           |    yfibre     2|   |
 * cfact*fcoll* |X*cos(gtheta)+sin(gtheta)* |1 - ------- - X |   |
 *              \                           \      D         /   /
 * --------------------------------------------------------------- + .5*nx
 *  /                             /          2      \1/2\
 *  |                             |    yfibre      2|   |
 *  | -X*sin(gtheta)+cos(gtheta)* |1 - ------- -  X |   |*xpixsize
 *  \                             \      D          /   /
 *
 *
 *           2         2        2
 * D = xfibre  + yfibre  + fcoll
 *
 *       lambda*gorder  xfibre*cos(gtheta)  fcoll*sin(gtheta)
 * X = - ------------- + ----------------- + ----------------
 *           gspace               1/2                1/2
 *                              D                 D
 * @endcode
 *
 * where:
 *  - lambda  : wavelength {mm}
 *  - xfibre  : X fibre {mm}
 *  - yfibre  : Y fibre {mm}
 *  - nx      : CCD size in X {pixels}
 *  - xpixsize: CCD pixel size in X {mm}
 *  - fcoll   : collimator focal length {mm}
 *  - cfact   : camera magnification factor
 *  - gtheta  : grating angle {radian}
 *  - gorder  : grating diffraction order
 *  - gspace  : grating groove spacing {mm}
 *
 * @retval y    - Function value : xccd {mm}
 * @retval dyda - Function derivatives :
 *                [nx,ypixsize,fcoll,cfact,gtheta,gorder,gspace]
 */

void
mrqxoptmod(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
           cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqxoptmod";

    register cxdouble xccd, d, X;
    register cxdouble lambda,xfibre,yfibre,pixsize,nx;
    /* Optical model parameters */
    register cxdouble fcoll,cfact;
    /* Grating parameters */
    register cxdouble gtheta,gorder,gspace;
    register cxdouble yfibre2,tmp,tmp2,d2,X2,gspace2,sqtmp,costheta,sintheta;

    /* check for number of parameters */
    if (na != 7) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[0] = dyda[1] = dyda[2] = dyda[3] =
                      dyda[4] = dyda[5] = dyda[6] = 0.0;
    }

    lambda  = x[LMI_WLEN];    /* wavelength [mm]              */
    xfibre  = x[LMI_XFIB];    /* Y fibre [mm]                 */
    yfibre  = x[LMI_YFIB];    /* Y fibre [mm]                 */

    nx      = a[LMP_NX];      /* CCD  size in X [pixels]      */
    pixsize = a[LMP_PXSIZ];   /* CCD pixel size [mm]          */
    fcoll   = a[LMP_FCOLL];   /* collimator focal length [mm] */
    cfact   = a[LMP_CFACT];   /* camera magnification factor  */
    gtheta  = a[LMP_THETA];   /* grating angle [radian]       */
    gorder  = a[LMP_ORDER];   /* grating diffraction order    */
    gspace  = a[LMP_SPACE];   /* grating groove spacing [mm]  */

    yfibre2  = yfibre * yfibre;
    gspace2  = gspace * gspace;
    costheta = cos(gtheta);
    sintheta = sin(gtheta);
    d2 = xfibre * xfibre + yfibre2 + (fcoll * fcoll);
    d  = sqrt(d2);
    X  = (-lambda*gorder/gspace) + (xfibre*costheta/d) + (fcoll*sintheta/d);
    X2 = X * X;
    sqtmp = sqrt(1.0 - yfibre2/d2 - X2);
    tmp   = -sintheta*X + costheta*sqtmp;
    tmp2  = tmp * tmp;
    xccd  = (cfact * fcoll * (X*costheta + sintheta*sqtmp))/tmp;

    /* takes care of model direction */
    if (nx < 0.0)
        *y = (xccd / pixsize - 0.5*nx);
    else
        *y = (-xccd / pixsize + 0.5*nx);

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    /* derivatives for each parameters */
    dyda[LMP_NX]    = 0.5;                   /* d(y)/d(nx)      */
    dyda[LMP_PXSIZ] = 0.0;                   /* d(y)/d(pixsize) */

    dyda[LMP_FCOLL] = cfact*(costheta*X+sintheta*sqtmp)/tmp +
              cfact*fcoll*(costheta*(-X*fcoll/d2+sintheta/d -
               gorder*lambda*fcoll/(d2*gspace)) +
               0.5*sintheta*(-2.0*X*(-X*fcoll/d2+sintheta/d -
               gorder*lambda*fcoll/(d2*gspace))+2.0*yfibre2*fcoll/(d2*d2))/sqtmp)/tmp -
              cfact*fcoll*(costheta*X+sintheta*sqtmp)*(-sintheta*(-X*fcoll/d2 +
               sintheta/d-gorder*lambda*fcoll/(d2*gspace)) +
               0.5*costheta*(-2.0*X*(-X*fcoll/d2+sintheta/d -
               gorder*lambda*fcoll/(d2*gspace))+2.0*yfibre2*fcoll/(d2*d2))/sqtmp)/tmp2;
    dyda[LMP_FCOLL] /= pixsize;                 /* d(y)/d(fcoll) */

    dyda[LMP_CFACT] = (xccd/cfact)/pixsize;     /* d(y)/d(cfact) */

    dyda[LMP_THETA] = cfact*fcoll*((-xfibre*sintheta/d+fcoll*costheta/d)*costheta -
               sintheta*X-sintheta*X*(-xfibre*sintheta/d+fcoll*costheta/d)/sqtmp +
               costheta*sqtmp)/tmp -
              cfact*fcoll*(costheta*X+sintheta*sqtmp)*(-(-xfibre*sintheta/d +
               fcoll*costheta/d)*sintheta-costheta*X -
               costheta*X*(-xfibre*sintheta/d+fcoll*costheta/d)/sqtmp -
               sintheta*sqtmp)/tmp2;
    dyda[LMP_THETA] /= pixsize;             /* d(y)/d(gtheta) */

    dyda[LMP_ORDER] = 0.0;                   /* d(y)/d(gorder) */
    dyda[LMP_SPACE] = cfact*fcoll*(lambda*gorder*costheta/gspace2-sintheta*X*lambda*gorder/(sqtmp*gspace2))/tmp -
              cfact*fcoll*(X*costheta+sintheta*sqtmp) *
              (-lambda*gorder*sintheta/gspace2-costheta*X*lambda*gorder/(sqtmp*gspace2))/tmp2;
    dyda[LMP_SPACE] /= pixsize;             /* d(y)/d(gspace) */

    if (nx > 0.0) {
        dyda[LMP_NX]    = -dyda[LMP_NX];
        dyda[LMP_PXSIZ] = -dyda[LMP_PXSIZ];
        dyda[LMP_FCOLL] = -dyda[LMP_FCOLL];
        dyda[LMP_CFACT] = -dyda[LMP_CFACT];
        dyda[LMP_THETA] = -dyda[LMP_THETA];
        dyda[LMP_ORDER] = -dyda[LMP_ORDER];
        dyda[LMP_SPACE] = -dyda[LMP_SPACE];
    }

    if (r != NULL) {
        register cxint k;

        k = LMP_FCOLL << 1;
        if (r[k+1] > 0) {
            dyda[LMP_FCOLL] *= mrqdydaweight(a[LMP_FCOLL],r[k],r[k+1]);
        }
        k = LMP_CFACT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CFACT] *= mrqdydaweight(a[LMP_CFACT],r[k],r[k+1]);
        }
        k = LMP_THETA << 1;
        if (r[k+1] > 0) {
            dyda[LMP_THETA] *=  mrqdydaweight(a[LMP_THETA],r[k],r[k+1]);
        }
        k = LMP_SPACE << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SPACE] *=  mrqdydaweight(a[LMP_SPACE],r[k],r[k+1]);
        }
    }

} /* end mrqxoptmod() */

/**
 * @brief
 *   Compute X on ccd and its derivatives using simple optical model
 *   with slit move
 *
 * @param x     - Initial guess : [lambda,xfibre,yfibre]
 * @param a     - function parameters (to be fitted...) :
 *                [nx,xpixsize,fcoll,cfact,gtheta,gorder,gspace,
 *                 slitdx,slitdy,slitphi]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description :
 * @code
 * xccd =
 *              /                             /          2      \1/2\
 *              |                             |        YF      2|   |
 *  cfact*fcoll*| cos(gtheta)*X + sin(gtheta)*|1 - ------- -  X |   |
 *              \                             \       D         /   /
 *  ----------------------------------------------------------------- + .5 nx
 *     /                              /       2      \1/2\
 *     |                              |     YF      2|   |
 *     | -sin(gtheta)*X + cos(gtheta)*|1 - ---- -  X |   |*xpixsize
 *     \                              \      D       /   /
 *
 * XF = xfibre*(1. + slitphi*yfibre) + slitdx
 *
 *                          2 1/2
 * YF = yfibre*(1. - slitphi )    + slitdy
 *
 *       2     2        2
 * D = XF  + YF  + fcoll
 *
 *       lambda*gorder   XF*cos(gtheta)   fcoll*sin(gtheta)
 * X = - ------------- + -------------- + -----------------
 *          gspace             1/2                1/2
 *                            D                  D
 * @endcode
 *
 * where:
 *  - lambda  : wavelength {mm}
 *  - xfibre  : X fibre {mm}
 *  - yfibre  : Y fibre {mm}
 *  - nx      : CCD size in X {pixels}
 *  - xpixsize: CCD pixel size in X {mm}
 *  - fcoll   : collimator focal length {mm}
 *  - cfact   : camera magnification factor
 *  - gtheta  : grating angle {radian}
 *  - gorder  : grating diffraction order
 *  - gspace  : grating groove spacing {mm}
 *  - slitdx  : slit position x offset {mm}
 *  - slitdy  : slit position y offset {mm}
 *  - slitphi : slit position angle {radian}
 *
 * @retval y    - Function value : xccd {mm}
 * @retval dyda - Function derivatives
 *                [nx,xpixsize,fcoll,cfact,gtheta,gorder,gspace,
 *                 slitdx,slitdy,slitphi]
 *
 */

void
mrqxoptmod2(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
            cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqxoptmod2";

    register cxdouble lambda,xfibre,yfibre,pixsize,nx;
    /* Optical model parameters */
    register cxdouble fcoll,cfact;
    /* Grating parameters */
    register cxdouble gtheta,gorder,gspace;
    /* Slit position parameters */
    cxdouble  slitdx,slitdy,slitphi;

    register cxdouble t1,t10,t104,t107,t11,t113,t119,t12,t120,t121,t124,t136,
                    t137,t138,t14,t143,t148,t16,t161,t162,t166,t168,t17,t173,
                    t18,t19,t191,t195,t196,t2,t20,t201,t21,t210,t23,t24,t26,
                    t27,t28,t3,t30,t32,t33,t34,t35,t36,t37,t38,t39,t4,t40,t44,
                    t49,t52,t58,t60,t61,t62,t64,t68,t75,t76,t78,t80,t9,t91,t93;

    /* check for number of parameters */
    if (na != 10) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[0] = dyda[1] = dyda[2] = dyda[3] =
                      dyda[4] = dyda[5] = dyda[6] =
                      dyda[7] = dyda[8] = dyda[9] = 0.0;
    }

    lambda  = x[LMI_WLEN];    /* wavelength [mm]              */
    xfibre  = x[LMI_XFIB];    /* Y fibre [mm]                 */
    yfibre  = x[LMI_YFIB];    /* Y fibre [mm]                 */

    nx      = a[LMP_NX];      /* CCD  size in X [pixels]      */
    pixsize = a[LMP_PXSIZ];   /* CCD pixel size [mm]          */
    fcoll   = a[LMP_FCOLL];   /* collimator focal length [mm] */
    cfact   = a[LMP_CFACT];   /* camera magnification factor  */
    gtheta  = a[LMP_THETA];   /* grating angle [radian]       */
    gorder  = a[LMP_ORDER];   /* grating diffraction order    */
    gspace  = a[LMP_SPACE];   /* grating groove spacing [mm]  */
    slitdx  = a[LMP_SOFFX];    /* slit position x offset [mm]  */
    slitdy  = a[LMP_SOFFY];    /* slit position y offset [mm]  */
    slitphi = a[LMP_SPHI];     /* slit position angle [radian] */

    t1 = cfact*fcoll;
    t2 = cos(gtheta);
    t3 = lambda*gorder;
    t4 = 1.0/gspace;
    t9 = xfibre*(1.0+slitphi*yfibre)+slitdx;
    t10 = t9*t2;
    t11 = t9*t9;
    t12 = slitphi*slitphi;
    t14 = sqrt(1.0-t12);
    t16 = yfibre*t14+slitdy;
    t17 = t16*t16;
    t18 = fcoll*fcoll;
    t19 = t11+t17+t18;
    t20 = sqrt(t19);
    t21 = 1.0/t20;
    t23 = sin(gtheta);
    t24 = fcoll*t23;
    t26 = -t3*t4+t10*t21+t24*t21;
    t27 = t2*t26;
    t28 = 1.0/t19;
    t30 = t26*t26;
    t32 = sqrt(1.0-t17*t28-t30);
    t33 = t23*t32;
    t34 = t27+t33;
    t35 = t23*t26;
    t36 = t2*t32;
    t37 = -t35+t36;
    t38 = 1.0/t37;
    t39 = t34*t38;
    t40 = 1.0/pixsize;
    t44 = pixsize*pixsize;
    t49 = t38*t40;
    t52 = 1.0/t20/t19;
    t58 = -t10*t52*fcoll+t23*t21-t18*t23*t52;
    t60 = 1.0/t32;
    t61 = t23*t60;
    t62 = t19*t19;
    t64 = t17/t62;
    t68 = 2.0*t64*fcoll-2.0*t26*t58;
    t75 = t1*t34;
    t76 = t37*t37;
    t78 = 1.0/t76*t40;
    t80 = t2*t60;
    t91 = -t9*t23*t21+fcoll*t2*t21;
    t93 = t26*t91;
    t104 = t2*lambda;
    t107 = t26*lambda*t4;
    t113 = t23*lambda;
    t119 = gspace*gspace;
    t120 = 1.0/t119;
    t121 = gorder*t120;
    t124 = t3*t120;
    t136 = t2*t21;
    t137 = 2.0*t9;
    t138 = t52*t137;
    t143 = t136-t10*t138/2.0-t24*t138/2.0;
    t148 = t64*t137-2.0*t26*t143;
    t161 = 2.0*t16;
    t162 = t52*t161;
    t166 = -t10*t162/2.0-t24*t162/2.0;
    t168 = t16*t28;
    t173 = -2.0*t168+t64*t161-2.0*t26*t166;
    t191 = 1.0/t14;
    t195 = 2.0*t9*xfibre*yfibre-2.0*t16*yfibre*t191*slitphi;
    t196 = t52*t195;
    t201 = xfibre*yfibre*t136-t10*t196/2.0-t24*t196/2.0;
    t210 = 2.0*t168*yfibre*t191*slitphi+t64*t195-2.0*t26*t201;

    /* takes care of model direction */
    if (nx < 0.0)
        *y = t1*t39*t40-0.5*nx;
    else
        *y = -t1*t39*t40+0.5*nx;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    /* derivatives for each parameters */
    dyda[LMP_NX]    = 0.5;                   /* d(y)/d(nx)      */
    dyda[LMP_PXSIZ] = -t1*t39/t44;           /* d(y)/d(pixsize) */
    dyda[LMP_FCOLL] =                        /* d(y)/d(fcoll) */
                    cfact*t34*t49+t1*(t2*t58+t61*t68/2.0)*t38*t40 -
                    t75*t78*(-t23*t58+t80*t68/2.0);
    dyda[LMP_CFACT] =                        /* d(y)/d(cfact) */
                    fcoll*t34*t49;
    dyda[LMP_THETA] =                        /* d(y)/d(gtheta) */
                    t1*(-t35+t2*t91+t36-t61*t93)*t38*t40 -
                    t75*t78*(-t27-t23*t91-t33-t80*t93);
    dyda[LMP_ORDER] =                        /* d(y)/d(gorder) */
                    t1*(-t104*t4+t61*t107)*t38*t40-t75*t78*(t113*t4+t80*t107);
    dyda[LMP_SPACE] =                        /* d(y)/d(gspace) */
                    t1*(t104*t121-t61*t26*t124)*t38*t40 -
                    t75*t78*(-t113*t121-t80*t26*t124);
    dyda[LMP_SOFFX] =                        /* d(y)/d(slitdx) */
                    t1*(t2*t143+t61*t148/2.0)*t38*t40 -
                    t75*t78*(-t23*t143+t80*t148/2.0);
    dyda[LMP_SOFFY] =                        /* d(y)/d(slitdy) */
                    t1*(t2*t166+t61*t173/2.0)*t38*t40 -
                    t75*t78*(-t23*t166+t80*t173/2.0);
    dyda[LMP_SPHI]  =                        /* d(y)/d(slitphi) */
                    t1*(t2*t201+t61*t210/2.0)*t38*t40 -
                    t75*t78*(-t23*t201+t80*t210/2.0);

    if (nx > 0.0) {
        dyda[LMP_NX]    = -dyda[LMP_NX];
        dyda[LMP_PXSIZ] = -dyda[LMP_PXSIZ];
        dyda[LMP_FCOLL] = -dyda[LMP_FCOLL];
        dyda[LMP_CFACT] = -dyda[LMP_CFACT];
        dyda[LMP_THETA] = -dyda[LMP_THETA];
        dyda[LMP_ORDER] = -dyda[LMP_ORDER];
        dyda[LMP_SPACE] = -dyda[LMP_SPACE];
        dyda[LMP_SOFFX] = -dyda[LMP_SOFFX];
        dyda[LMP_SOFFY] = -dyda[LMP_SOFFY];
        dyda[LMP_SPHI]  = -dyda[LMP_SPHI];
    }

    if (r != NULL) {
        register cxint k;

        k = LMP_PXSIZ << 1;
        if (r[k+1] > 0) {
            dyda[LMP_PXSIZ] *= mrqdydaweight(a[LMP_PXSIZ],r[k],r[k+1]);
        }
        k = LMP_FCOLL << 1;
        if (r[k+1] > 0) {
            dyda[LMP_FCOLL] *= mrqdydaweight(a[LMP_FCOLL],r[k],r[k+1]);
        }
        k = LMP_CFACT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CFACT] *= mrqdydaweight(a[LMP_CFACT],r[k],r[k+1]);
        }
        k = LMP_THETA << 1;
        if (r[k+1] > 0) {
            dyda[LMP_THETA] *=  mrqdydaweight(a[LMP_THETA],r[k],r[k+1]);
        }
        k = LMP_ORDER << 1;
        if (r[k+1] > 0) {
            dyda[LMP_ORDER] *=  mrqdydaweight(a[LMP_ORDER],r[k],r[k+1]);
        }
        k = LMP_SPACE << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SPACE] *=  mrqdydaweight(a[LMP_SPACE],r[k],r[k+1]);
        }
        k = LMP_SOFFX << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SOFFX] *=  mrqdydaweight(a[LMP_SOFFX],r[k],r[k+1]);
        }
        k = LMP_SOFFY << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SOFFY] *=  mrqdydaweight(a[LMP_SOFFY],r[k],r[k+1]);
        }
        k = LMP_SPHI << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SPHI] *=  mrqdydaweight(a[LMP_SPHI],r[k],r[k+1]);
        }
    }

} /* end mrqxoptmod2() */

/**
 * @brief
 *   Compute Y on ccd and its derivatives using simple optical model
 *
 * @param x         - Initial guess : [lambda,xfibre,yfibre]
 * @param a         - function parameters (to be fitted...) :
 *                    [ny,ypixsize,fcoll,cfact,gtheta,gorder,gspace]
 * @param r         - Not used
 * @param na        - Number of parameters
 *
 * @par Description :
 * @code
 * yccd =
 *                         /      /
 *                         |  1/2 |
 *  - cfact*fcoll*yfibre / |D     |
 *                         |      |
 *                         \      \
 *
 *     /  lambda*gorder   xfibre*cos(gtheta)   fcoll*sin(gtheta)\
 *    -|- ------------- + ------------------ + -----------------| sin(gtheta) +
 *     |     gspace               1/2                  1/2      |
 *     \                        D                    D         /
 *
 *                /
 *                |
 *    cos(gtheta) |
 *                |
 *                \
 *
 *              2                                                               \
 *        yfibre    /  lambda*gorder   xfibre*cos(gtheta)   fcoll*sin(gtheta)\2 |
 *    1 - ------- - |- ------------- + ------------------ + -----------------|  |^
 *          D       |     gspace               1/2                  1/2      |  |
 *                  \                        D                    D          /  /
 *
 *    1/2\        \
 *       |        |
 *       | pixsize| + .5*ny
 *       |        |
 *       /        /
 *
 *           2         2        2
 * D = xfibre  + yfibre  + fcoll
 *
 * @endcode
 *
 * where:
 *  - lambda  : wavelength {mm}
 *  - xfibre  : X fibre {mm}
 *  - yfibre  : Y fibre {mm}
 *  - ny      : CCD size in Y {pixels}
 *  - ypixsize: CCD pixel size in Y {mm}
 *  - fcoll   : collimator focal length {mm}
 *  - cfact   : camera magnification factor
 *  - gtheta  : grating angle {radian}
 *  - gorder  : grating diffraction order
 *  - gspace  : grating groove spacing {mm}
 *
 * @retval y        - Function valeu : yccd {mm}
 * @retval dyda[na] - Function derivatives :
 *                    [ny,ypixsize,fcoll,cfact,gtheta,gorder,gspace]
 */

void
mrqyoptmod(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
           cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqyoptmod";

    register cxdouble lambda,xfibre,yfibre,pixsize,ny;
    /* Optical model parameters */
    register cxdouble fcoll,cfact;
    /* Grating parameters */
    register cxdouble gtheta,gorder,gspace;

    cxdouble t10,t12,t13,t15,t18,t2,t22,t24,t26,t27,t28,t29,t3,t30,t33,
           t4,t41,t45,t47,t5,t53,t56,t57,t6,t7,t76,t8,t9,t93,t94;

    (void) r;  /* Not used. */

    /* check for number of parameters */
    if (na != 7) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_NX] = dyda[LMP_PXSIZ] = dyda[LMP_FCOLL] = dyda[LMP_CFACT] =
                      dyda[LMP_THETA] = dyda[LMP_ORDER] = dyda[LMP_SPACE] = 0.0;
    }

    lambda  = x[LMI_WLEN];
    xfibre  = x[LMI_XFIB];
    yfibre  = x[LMI_YFIB];

    ny      = a[LMP_NY];
    pixsize = a[LMP_PXSIZ];
    fcoll   = a[LMP_FCOLL];
    cfact   = a[LMP_CFACT];
    gtheta  = a[LMP_THETA];
    gorder  = a[LMP_ORDER];
    gspace  = a[LMP_SPACE];

    t2 = cfact*fcoll*yfibre;
    t3 = xfibre*xfibre;
    t4 = yfibre*yfibre;
    t5 = fcoll*fcoll;
    t6 = t3+t4+t5;
    t7 = sqrt(t6);
    t8 = 1.0/t7;
    t9 = lambda*gorder;
    t10 = 1.0/gspace;
    t12 = cos(gtheta);
    t13 = xfibre*t12;
    t15 = sin(gtheta);
    t18 = -t9*t10+t13*t8+fcoll*t15*t8;
    t22 = t18*t18;
    t24 = sqrt(1.0-t4/t6-t22);
    t26 = -t18*t15+t12*t24;
    t27 = 1.0/t26;
    t28 = t8*t27;
    t29 = 1.0/pixsize;
    t30 = t28*t29;
    t33 = pixsize*pixsize;
    t41 = 1.0/t7/t6;
    t45 = t26*t26;
    t47 = t8/t45;
    t53 = -t13*t41*fcoll+t15*t8-t5*t15*t41;
    t56 = t12/t24;
    t57 = t6*t6;
    t76 = -xfibre*t15*t8+fcoll*t12*t8;
    t93 = gspace*gspace;
    t94 = 1.0/t93;

    *y = -t2*t30+0.5*ny;

    /* Check if derivatives expected */
    if (dyda == NULL) return;

    /* derivatives for each parameters */
    dyda[LMP_NY]    = 0.5;                   /* d(y)/d(ny)      */

    dyda[LMP_PXSIZ] = t2*t28/t33;
    dyda[LMP_FCOLL] = -cfact*yfibre*t30+cfact*t5*yfibre*t41*t27*t29+t2*t47*t29*
                   (-t53*t15+t56*(2.0*t4/t57*fcoll-2.0*t18*t53)/2.0);
    dyda[LMP_CFACT] = -fcoll*yfibre*t30;
    dyda[LMP_THETA] = t2*t47*t29*(-t76*t15-t18*t12-t15*t24-t56*t18*t76);
    dyda[LMP_ORDER] = t2*t47*t29*(lambda*t10*t15+t56*t18*lambda*t10);
    dyda[LMP_SPACE] = t2*t47*t29*(-t9*t94*t15-t56*t18*t9*t94);

} /* end mrqyoptmod() */

/**
 * @brief
 *   Compute Y on ccd and its derivatives using simple optical model
 *   with slit move
 *
 * @param x         - Initial guess : [lambda,xfibre,yfibre]
 * @param a         - Function parameters (to be fitted...) :
 *                    [ny,xpixsize,fcoll,cfact,gtheta,gorder,gspace,
 *                     slitdx,slitdy,slitphi]
 * @param r         - Not used
 * @param na        - Number of parameters
 *
 * @par Description:
 * @code
 * yccd =
 *                    /      /
 *                    |  1/2 |
 *   - cfact*fcoll*YF/|D     |
 *                    |      |
 *                    \      \
 *
 *     /  lambda*gorder   XF*cos(gtheta)   fcoll*sin(gtheta)\
 *    -|- ------------- + -------------- + -----------------| sin(gtheta) +
 *     |     gspace             1/2                1/2      |
 *     \                      D                  D          /
 *
 *    cos(gtheta)*
 *
 *    /      2                                                         \1/2\
 *    |    YF    /  lambda*gorder   XF*cos(gtheta)   fcoll *(gtheta) \2 |   |
 *    |1 - --- - |- ------------- + -------------- + -----------------| |   |
 *    |    D     |     gspace             1/2                1/2      | |   |
 *    \          \                      D                  D          / /   /
 *
 *           \
 *           |
 *    pixsize| + .5*ny
 *           |
 *           /
 *
 * XF = xfibre*(1. + slitphi*yfibre) + slitdx
 *
 *                          2 1/2
 * YF = yfibre*(1. - slitphi )    + slitdy
 *
 *       2     2        2
 * D = XF  + YF  + fcoll
 *
 * @endcode
 *
 * where:
 *  - lambda  : wavelength {mm}
 *  - xfibre  : X fibre {mm}
 *  - yfibre  : Y fibre {mm}
 *  - ny      : CCD size in Y {pixels}
 *  - ypixsize: CCD pixel size in Y {mm}
 *  - fcoll   : collimator focal length {mm}
 *  - cfact   : camera magnification factor
 *  - gtheta  : grating angle {radian}
 *  - gorder  : grating diffraction order
 *  - gspace  : grating groove spacing {mm}
 *  - slitdx  : slit position x offset {mm}
 *  - slitdy  : slit position y offset {mm}
 *  - slitphi : slit position angle {radian}
 *
 * @retval y        - Function value : yccd {mm}
 * @retval dyda[na] - Function derivatives :
 *                    [ny,xpixsize,fcoll,cfact,gtheta,gorder,gspace,
 *                     slitdx,slitdy,slitphi]
 *
 */

void
mrqyoptmod2(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
            cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqyoptmod2";

    register cxdouble lambda,xfibre,yfibre,pixsize,ny;
    /* Optical model parameters */
    register cxdouble fcoll,cfact;
    /* Grating parameters */
    register cxdouble gtheta,gorder,gspace;
    /* Slit position parameters */
    cxdouble  slitdx,slitdy,slitphi;

    double t1,t102,t103,t11,t112,t117,t118,t12,t123,t13,t136,t14,t141,t145,
          t147,t15,t159,t16,t160,t17,t172,t179,t18,t184,t19,t2,t21,t22,t24,
          t25,t27,t29,t31,t33,t35,t36,t37,t38,t39,t4,t42,t50,t51,t54,t56,t6,
          t62,t65,t66,t68,t7,t85;

    (void) r;  /* Not used. */

    /* check for number of parameters */
    if (na != 10) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_NY] = dyda[LMP_PXSIZ] = dyda[LMP_FCOLL] = dyda[LMP_CFACT] =
                      dyda[LMP_THETA] = dyda[LMP_ORDER] = dyda[LMP_SPACE] =
                      dyda[LMP_SOFFX] = dyda[LMP_SOFFY] = dyda[LMP_SPHI] = 0.0;
    }

    lambda  = x[LMI_WLEN];
    xfibre  = x[LMI_XFIB];
    yfibre  = x[LMI_YFIB];

    ny      = a[LMP_NY];
    pixsize = a[LMP_PXSIZ];
    fcoll   = a[LMP_FCOLL];
    cfact   = a[LMP_CFACT];
    gtheta  = a[LMP_THETA];
    gorder  = a[LMP_ORDER];
    gspace  = a[LMP_SPACE];
    slitdx  = a[LMP_SOFFX];
    slitdy  = a[LMP_SOFFY];
    slitphi = a[LMP_SPHI];

    t1 = cfact*fcoll;
    t2 = slitphi*slitphi;
    t4 = sqrt(1.0-t2);
    t6 = yfibre*t4+slitdy;
    t7 = t1*t6;
    t11 = xfibre*(1.0+slitphi*yfibre)+slitdx;
    t12 = t11*t11;
    t13 = t6*t6;
    t14 = fcoll*fcoll;
    t15 = t12+t13+t14;
    t16 = sqrt(t15);
    t17 = 1/t16;
    t18 = lambda*gorder;
    t19 = 1/gspace;
    t21 = cos(gtheta);
    t22 = t11*t21;
    t24 = sin(gtheta);
    t25 = fcoll*t24;
    t27 = -t18*t19+t22*t17+t25*t17;
    t29 = 1/t15;
    t31 = t27*t27;
    t33 = sqrt(1.0-t13*t29-t31);
    t35 = -t27*t24+t21*t33;
    t36 = 1/t35;
    t37 = t17*t36;
    t38 = 1/pixsize;
    t39 = t37*t38;
    t42 = pixsize*pixsize;
    t50 = 1/t16/t15;
    t51 = t50*t36;
    t54 = t35*t35;
    t56 = t17/t54;
    t62 = -t22*t50*fcoll+t24*t17-t14*t24*t50;
    t65 = t21/t33;
    t66 = t15*t15;
    t68 = t13/t66;
    t85 = -t11*t24*t17+fcoll*t21*t17;
    t102 = gspace*gspace;
    t103 = 1/t102;
    t112 = 2.0*t11;
    t117 = t21*t17;
    t118 = t50*t112;
    t123 = t117-t22*t118/2.0-t25*t118/2.0;
    t136 = 2.0*t6;
    t141 = t50*t136;
    t145 = -t22*t141/2.0-t25*t141/2.0;
    t147 = t6*t29;
    t159 = 1/t4;
    t160 = yfibre*t159;
    t172 = 2.0*t11*xfibre*yfibre-2.0*t6*yfibre*t159*slitphi;
    t179 = t50*t172;
    t184 = xfibre*yfibre*t117-t22*t179/2.0-t25*t179/2.0;

    *y = -t7*t39+0.5*ny;

    /* Check if derivatives expected */
    if (dyda == NULL) return;

    /* derivatives for each parameters */
    dyda[LMP_NY]    = 0.5;                   /* d(y)/d(ny)      */
    dyda[LMP_PXSIZ] = t7*t37/t42;            /* d(y)/d(pixsize) */
    dyda[LMP_FCOLL] =                        /* d(y)/d(fcoll) */
                    -cfact*t6*t39+cfact*t14*t6*t51*t38+
                     t7*t56*t38*(-t62*t24+t65*(2.0*t68*fcoll-2.0*t27*t62)/2.0);
    dyda[LMP_CFACT] =                        /* d(y)/d(cfact) */
                    -fcoll*t6*t39;
    dyda[LMP_THETA] =                        /* d(y)/d(gtheta) */
                    t7*t56*t38*(-t85*t24-t27*t21-t24*t33-t65*t27*t85);
    dyda[LMP_ORDER] =                        /* d(y)/d(gorder) */
                    t7*t56*t38*(lambda*t19*t24+t65*t27*lambda*t19);
    dyda[LMP_SPACE] =                        /* d(y)/d(gspace) */
                    t7*t56*t38*(-t18*t103*t24-t65*t27*t18*t103);
    dyda[LMP_SOFFX] =                        /* d(y)/d(slitdx) */
                    t7*t51*t38*t112/2.0+t7*t56*t38*(-t123*t24+t65*
                     (t68*t112-2.0*t27*t123)/2.0);
    dyda[LMP_SOFFY] =                        /* d(y)/d(slitdy) */
                    -t1*t39+t7*t51*t38*t136/2.0+t7*t56*t38*(-t145*t24+t65*
                     (-2.0*t147+t68*t136-2.0*t27*t145)/2.0);
    dyda[LMP_SPHI]  =                        /* d(y)/d(slitphi) */
                    t1*t160*slitphi*t17*t36*t38+t7*t51*t38*t172/2.0+
                    t7*t56*t38*(-t184*t24+t65*(2.0*t147*t160*slitphi+t68*t172-
                                 2.0*t27*t184)/2.0);

} /* end mrqyoptmod2() */

/**
 * @brief
 *   Compute Y PSF profile using cosine based model
 *
 * @param x     - Initial guess : [x]
 * @param a     - Function parameters (to be fitted...) :
 *                [amplitude,center,background,width1,width2]
 * @param r     - Not used
 * @param na    - Number of parameters
 *
 *
 * @par Description:
 * @code
 *                           /           /| x - center |\lwidth  \3
 * mrqpsfcos = 1/8 amplitude |1 + cos(Pi |--------------|      ) |  + background
 *                           \           \    xwidth    /        /
 *
 * @endcode
 *
 * @retval y    - Function value : mrqpsfcos
 * @retval dyda - Function derivatives :
 *                [amplitude,center,background,width1,width2]
 *
 */

void
mrqpsfcos(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
          cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqpsfcos";

    cxdouble t1,t10,t13,t14,t15,t16,t2,t26,t3,t4,t5,t6,t7,t8,t9;

    cxdouble amplitude  = a[LMP_AMPL];
    cxdouble center     = a[LMP_CENT];
    cxdouble background = a[LMP_BACK];
    cxdouble width1     = a[LMP_WID1];
    cxdouble width2     = a[LMP_WID2];

    (void) r;  /* Not used. */

    /* check for number of parameters */
    if (na != 5) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_AMPL] = dyda[LMP_CENT] = dyda[LMP_BACK] = dyda[LMP_WID1] =
                         dyda[LMP_WID2] = 0.0;
    }

    t1 = x[0]-center;
    t2 = fabs(t1);
    t3 = 1.0/width2;
    t4 = t2*t3;
    t5 = pow(t4,width1);
    t6 = CX_PI*t5;
    t7 = cos(t6);
    t8 = 1.0+t7;
    t9 = t8*t8;
    t10 = t9*t8;
    t13 = amplitude*t9;
    t14 = sin(t6);
    t15 = t13*t14;
    t16 = log(t4);
    t26 = (t1>0.0)? 1.0:-1.0;

    if (t2 > width2) {
        *y =  background;

        /* Check if derivatives expected */
        if (dyda == NULL) return;

        dyda[LMP_AMPL] = dyda[LMP_CENT] = dyda[LMP_BACK] = dyda[LMP_WID1] = 0.0;
        dyda[LMP_WID2] = 1.0;
    } else {
        *y = amplitude*t10/8.0+background;     /* Function value     */

        /* Check if derivatives expected */
        if (dyda == NULL)
            return;

        dyda[LMP_AMPL] = t10/8.0;                     /* d(y)/d(amplitude)  */
                                               /* d(y)/d(center)     */
        dyda[LMP_CENT] = 3.0/8.0*t13*t14*CX_PI*t5*width1*t26/t2;
        dyda[LMP_BACK] = 1.0;                         /* d(y)/d(background) */
        dyda[LMP_WID1] = -3.0/8.0*t15*t6*t16;         /* d(y)/d(width1)     */
        dyda[LMP_WID2] = 3.0/8.0*t15*t6*width1*t3;    /* d(y)/d(width2)     */
    }

    return;

} /* end mrqpsfcos() */

/**
 * @brief
 *   Compute Y PSF profile using exponential based model
 *
 * @param x     - Initial guess : [x]
 * @param a     - Function parameters (to be fitted...) :
 *                [amplitude,center,background,width1,width2]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description :
 * @code
 *                              /               wid2\
 *                              || x - center |     |
 *  mrqpsfexp = amplitude*exp(- |-------------------|) + background
 *                              \       wid1        /
 *
 * @endcode
 *
 * @retval y    - Function value : mrqpsfexp
 * @retval dyda - Function derivatives :
 *                [amplitude,center,background,width1,width2]
 *
 */

void
mrqpsfexp(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
          cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqpsfexp";

    cxdouble t1,t2,t3,t4,t6,t8,t10,t15,t18;

    cxdouble amplitude  = a[LMP_AMPL];
    cxdouble center     = a[LMP_CENT];
    cxdouble background = a[LMP_BACK];
    cxdouble width1     = a[LMP_WID1];
    cxdouble width2     = a[LMP_WID2];

    /* check for number of parameters */
    if (na != 5) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_AMPL] = dyda[LMP_CENT] = dyda[LMP_BACK] = dyda[LMP_WID1] =
                         dyda[LMP_WID2] = 0.0;
    }

    t1 = x[0]-center;

    if (t1 > 0.0) {
        t2 = t1;
        t10 = 1.0;
    } else {
        t2 = -t1;
        t10 = -1.0;
    }

    t3 = pow(t2,width2);
    t4 = 1.0/width1;
    t6 = exp(-t3*t4);
    t8 = amplitude*t3;
    t15 = width1*width1;
    t18 = log(t2);

    *y = amplitude*t6+background;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    dyda[LMP_AMPL] = t6;                      /* d(y)/d(amplitude)  */
    dyda[LMP_CENT] = t8*width2*t10/t2*t4*t6;  /* d(y)/d(center)     */

    if (isnan(dyda[LMP_CENT]))
        dyda[LMP_CENT] = 0.;

    dyda[LMP_BACK] = 1.0;                     /* d(y)/d(background) */
    dyda[LMP_WID1] = t8/t15*t6;               /* d(y)/d(width1)     */
    dyda[LMP_WID2] = -t8*t18*t4*t6;           /* d(y)/d(width2)     */

    if (isnan(dyda[LMP_WID2]))
        dyda[LMP_WID2] = 0.;

    if (r != NULL) {
        register cxint k;

        k = LMP_AMPL << 1;
        if (r[k+1] > 0) {
            dyda[LMP_AMPL] *= mrqdydaweight(a[LMP_AMPL],r[k],r[k+1]);
        }
        k = LMP_CENT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CENT] *= mrqdydaweight(a[LMP_CENT],r[k],r[k+1]);
        }
        k = LMP_WID1 << 1;
        if (r[k+1] > 0) {
            dyda[LMP_WID1] *= mrqdydaweight(a[LMP_WID1],r[k],r[k+1]);
        }
        k = LMP_WID2 << 1;
        if (r[k+1] > 0) {
            dyda[LMP_WID2] *= mrqdydaweight(a[LMP_WID2],r[k],r[k+1]);
        }
    }

    return;

} /* end mrqpsfexp() */

/**
 * @brief
 *   Compute Y PSF profile using exponential based model
 *
 * @param x     - Initial guess : [x]
 * @param a     - Function parameters (to be fitted...) :
 *                [amplitude,center,background,width1,width2]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description :
 * @code
 *                               /              \wid2
 *                               || x - center ||
 *  mrqpsfexp2 = amplitude*exp(- |--------------|      ) + background
 *                               \    wid1      /
 *
 * @endcode
 *
 * @retval y    - Function value : mrqpsfexp2
 * @retval dyda - Function derivatives :
 *                [amplitude,center,background,width1,width2]
 *
 */

void
mrqpsfexp2(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
           cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqpsfexp2";

    cxdouble t1,t2,t3,t4,t5,t6,t8,t10,t16;

    cxdouble amplitude  = a[LMP_AMPL];
    cxdouble center     = a[LMP_CENT];
    cxdouble background = a[LMP_BACK];
    cxdouble width1     = a[LMP_WID1];
    cxdouble width2     = a[LMP_WID2];

    /* check for number of parameters */
    if (na != 5) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_AMPL] = dyda[LMP_CENT] = dyda[LMP_BACK] = dyda[LMP_WID1] =
                         dyda[LMP_WID2] = 0.0;
    }

    t1 = x[0]-center;

    if (t1 > 0.0) {
        t2 = t1;
        t10 = 1.0;
    } else {
        t2 = -t1;
        t10 = -1.0;
    }

    t3 = 1.0/width1;
    t4 = t2*t3;
    t5 = pow(t4,width2);
    t6 = exp(-t5);
    t8 = amplitude*t5;
    t16 = log(t4);

    *y = amplitude*t6+background;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    dyda[LMP_AMPL] = t6;                         /* d(y)/d(amplitude)  */
    dyda[LMP_CENT] = t8*width2*t10/t2*t6;        /* d(y)/d(center)     */

    if (isnan(dyda[LMP_CENT]))
        dyda[LMP_CENT] = 0.0;

    dyda[LMP_BACK] = 1.0;                        /* d(y)/d(background) */
    dyda[LMP_WID1] = t8*width2*t3*t6;            /* d(y)/d(width1)     */
    dyda[LMP_WID2] = -t8*t16*t6;                 /* d(y)/d(width2)     */

    if (isnan(dyda[LMP_WID2]))
        dyda[LMP_WID2] = 0.0;

    if (r != NULL) {
        register cxint k;

        k = LMP_AMPL << 1;
        if (r[k+1] > 0) {
            dyda[LMP_AMPL] *= mrqdydaweight(a[LMP_AMPL],r[k],r[k+1]);
        }
        k = LMP_CENT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CENT] *= mrqdydaweight(a[LMP_CENT],r[k],r[k+1]);
        }
        k = LMP_WID1 << 1;
        if (r[k+1] > 0) {
            dyda[LMP_WID1] *= mrqdydaweight(a[LMP_WID1],r[k],r[k+1]);
        }
        k = LMP_WID2 << 1;
        if (r[k+1] > 0) {
            dyda[LMP_WID2] *= mrqdydaweight(a[LMP_WID2],r[k],r[k+1]);
        }
    }

    return;

} /* end mrqpsfexp2() */

/**
 * @brief
 *   Compute Y PSF profile using displacement/rotation model
 *
 * @param x     - Initial guess : [xccd, nx, startx, ncoeff]
 * @param a     - Function parameters (to be fitted...) :
 *                [Tx,Ty,1/Kx,Ky,tan(theta)]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description:
 * @code
 * @endcode
 *
 * @retval y    - Function value : y
 * @retval dyda - Function derivatives :
 *                [Tx,Ty,1/Kx,Ky,tan(theta)]
 *
 */

void
mrqlocywarp(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
            cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqlocywarp";

    cxdouble          xccd, nx, startx;
    register cxint    i,ncoef;
    cxdouble          Tx, Ty, cx, Ky, tt, xx;
    cpl_matrix       *mBase = NULL, *mX = NULL;
    register cxdouble fxx = 0.0, f1xx = 0.0, f2xx = 0.0, z1;
    cxdouble         *pd_x = NULL, *pd_mbase = NULL;

    /* check for number of parameters */
    if (na != 5) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_TX] = dyda[LMP_TY] = dyda[LMP_CX] = dyda[LMP_KY] =
                       dyda[LMP_TT] = 0.0;
    }

    xccd   = x[LMI_XCCD];        /* pixel abcissa */
    nx     = x[LMI_NX];          /* number of pixels in dispersion dir. */
    startx = x[LMI_STRX];        /* 1st pixel position */
    ncoef  = (cxint) x[LMI_NCOF];  /* number of chebyshev coef */

    Tx = a[LMP_TX];   /* Translation X */
    Ty = a[LMP_TY];   /* Translation Y */
    cx = a[LMP_CX];   /* Scaling X: cx = 1/Kx */
    Ky = a[LMP_KY];   /* Scaling Y */
    tt = a[LMP_TT];   /* Rotation theta: tt = tan(theta) */

    xx = cx*(xccd-Tx);

    mX = cpl_matrix_new(1,1);
    pd_x = cpl_matrix_get_data(mX);
    pd_x[0] = xx;

    mBase = giraffe_chebyshev_base1d(startx, nx, ncoef, mX);

    pd_mbase = cpl_matrix_get_data(mBase);

    for (i = 0; i < ncoef; i++)
        fxx += pd_mbase[i] * x[i+4];

    if (ncoef > 1) {
        for (i = 0; i < (ncoef - 1); i++)
            f1xx += pd_mbase[i] * (i+1)*x[i+5];
    }

    if (ncoef > 2) {
        for (i = 0; i < (ncoef - 2); i++)
            f2xx += pd_mbase[i] * (i+2)*x[i+6];
    }

    if (mX!=NULL) { cpl_matrix_delete(mX); mX = NULL; pd_x = NULL; }
    if (mBase!=NULL) { cpl_matrix_delete(mBase); mBase = NULL; pd_mbase = NULL; }

    z1 = 1.0 - tt*tt + tt*f1xx;
    *y = Ky*(fxx-tt*xx)/z1 + Ty;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    dyda[LMP_TX] =                          /* d(y)/d(Tx)  */
              (cx*Ky/z1)*((tt-f1xx) + tt*f2xx*(fxx-tt*xx)/z1);

    dyda[LMP_TY] = 1.0;                     /* d(y)/d(Ty)     */

    dyda[LMP_CX] =                          /* d(y)/d(cx)     */
              (Ky*(xccd-Tx)/z1)*((f1xx-tt) - tt*f2xx*(fxx-tt*xx)/z1);

    dyda[LMP_KY] = (fxx-tt*xx)/z1;              /* d(y)/d(Ky)     */
    dyda[LMP_TT] =                          /* d(y)/d(tt) */
              (Ky/(z1*z1))*(-xx*(1.+tt*tt)+2.*tt*fxx-fxx*f1xx);

    if (r != NULL) {
        register cxint k;

        k = LMP_TX << 1;
        if (r[k+1] > 0) {
            dyda[LMP_TX] *= mrqdydaweight(a[LMP_TX],r[k],r[k+1]);
        }
        k = LMP_CX << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CX] *= mrqdydaweight(a[LMP_CX],r[k],r[k+1]);
        }
        k = LMP_KY << 1;
        if (r[k+1] > 0) {
            dyda[LMP_KY] *= mrqdydaweight(a[LMP_KY],r[k],r[k+1]);
        }
        k = LMP_TT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_TT] *= mrqdydaweight(a[LMP_TT],r[k],r[k+1]);
        }
    }

    return;

} /* end mrqlocywarp() */

/**
 * @brief
 *   Compute X on ccd and its derivatives using simple optical model
 *
 * @param x     - Initial guess : [lambda, xfibre, yfibre]
 * @param a     - Function parameters (to be fitted...) :
 *                [nx, pixsize, fcoll, cfact, gtheta, gorder, gspace]
 * @param r     - Ponderation of y derivatives
 * @param na    - Number of parameters
 *
 * @par Description:
 * @code
 * @endcode
 *
 * @retval y    - Function value : y
 * @retval dyda - Function derivatives :
 *                [nx, pixsize, fcoll, cfact, gtheta, gorder, gspace]
 *
 */

void
mrqxoptmodGS(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
             cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqxoptmodGS";

    register cxdouble lambda,xfibre,yfibre,pixsize,nx;
    /* Optical model parameters */
    register cxdouble fcoll,cfact;
    /* Grating parameters */
    register cxdouble gtheta,gorder,gspace;

    register cxdouble t1,t10,t109,t11,t110,t114,t12,t14,t17,t18,t2,t21,t23,t24;
    register cxdouble t25,t26,t27,t28,t29,t3,t30,t31,t35,t40,t43,t49,t5,t51,t52;
    register cxdouble t53,t59,t6,t66,t67,t69,t7,t71,t8,t82,t84,t9,t95,t98;

    /* check for number of parameters */
    if (na != 7) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    if (dyda != NULL) {
        dyda[LMP_NX] = dyda[LMP_PXSIZ] = dyda[LMP_FCOLL] = dyda[LMP_CFACT] =
                      dyda[LMP_THETA] = dyda[LMP_ORDER] = dyda[LMP_SPACE] = 0.0;
    }

    lambda  = x[LMI_WLEN];    /* wavelength [mm]              */
    xfibre  = x[LMI_XFIB];    /* Y fibre [mm]                 */
    yfibre  = x[LMI_YFIB];    /* Y fibre [mm]                 */

    nx      = a[LMP_NX];      /* CCD  size in X [pixels]      */
    pixsize = a[LMP_PXSIZ];   /* CCD pixel size [mm]          */
    fcoll   = a[LMP_FCOLL];   /* collimator focal length [mm] */
    cfact   = a[LMP_CFACT];   /* camera magnification factor  */
    gtheta  = a[LMP_THETA];   /* grating angle [radian]       */
    gorder  = a[LMP_ORDER];   /* grating diffraction order    */
    gspace  = a[LMP_SPACE];   /* grating groove spacing [mm]  */

    t1 = cfact*fcoll;
    t2 = lambda*gorder;
    t3 = 1.0/gspace;
    t5 = cos(gtheta);
    t6 = xfibre*t5;
    t7 = xfibre*xfibre;
    t8 = yfibre*yfibre;
    t9 = fcoll*fcoll;
    t10 = t7+t8+t9;
    t11 = sqrt(t10);
    t12 = 1.0/t11;
    t14 = sin(gtheta);
    t17 = -t2*t3+t12*t6+fcoll*t14*t12;
    t18 = t17*t5;
    t21 = t17*t17;
    t23 = sqrt(1.0-t8/t10-t21);
    t24 = t14*t23;
    t25 = t18+t24;
    t26 = t17*t14;
    t27 = t5*t23;
    t28 = -t26+t27;
    t29 = 1.0/t28;
    t30 = t25*t29;
    t31 = 1.0/pixsize;
    t35 = pixsize*pixsize;
    t40 = t29*t31;
    t43 = 1.0/t11/t10;
    t49 = -t6*t43*fcoll+t14*t12-t9*t14*t43;
    t51 = 1.0/t23;
    t52 = t14*t51;
    t53 = t10*t10;
    t59 = 2.0*t8/t53*fcoll-2.0*t17*t49;
    t66 = t1*t25;
    t67 = t28*t28;
    t69 = 1.0/t67*t31;
    t71 = t5*t51;
    t82 = -xfibre*t14*t12+fcoll*t5*t12;
    t84 = t17*t82;
    t95 = lambda*t3;
    t98 = t17*lambda*t3;
    t109 = gspace*gspace;
    t110 = 1.0/t109;
    t114 = t2*t110;

    /* takes care of model direction */
    if (nx < 0.0)
        *y = t1*t30*t31-0.5*nx;
    else
        *y = -t1*t30*t31+0.5*nx;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    /* derivatives for each parameters */
    dyda[LMP_NX]    = 0.5;                   /* d(y)/d(nx)      */
    dyda[LMP_PXSIZ] = -t1*t30/t35;           /* d(y)/d(pixsize) */
    dyda[LMP_FCOLL] =                        /* d(y)/d(fcoll) */
                    cfact*t25*t40+t1*(t49*t5+t52*t59/2.0)*t29*t31-
                    t66*t69*(-t49*t14+t71*t59/2.0);
    dyda[LMP_CFACT] =                        /* d(y)/d(cfact) */
                    fcoll*t25*t40;
    dyda[LMP_THETA] =                        /* d(y)/d(gtheta) */
                    t1*(t82*t5-t26+t27-t52*t84)*t29*t31-
                    t66*t69*(-t82*t14-t18-t24-t71*t84);
    dyda[LMP_ORDER] =                        /* d(y)/d(gorder) */
                    t1*(-t95*t5+t52*t98)*t29*t31-t66*t69*(t95*t14+t71*t98);
    dyda[LMP_SPACE] =                        /* d(y)/d(gspace) */
                    t1*(t2*t110*t5-t52*t17*t114)*t29*t31-
                    t66*t69*(-t2*t110*t14-t71*t17*t114);

    if (nx > 0.0) {
        dyda[LMP_NX]    = -dyda[LMP_NX];
        dyda[LMP_PXSIZ] = -dyda[LMP_PXSIZ];
        dyda[LMP_FCOLL] = -dyda[LMP_FCOLL];
        dyda[LMP_CFACT] = -dyda[LMP_CFACT];
        dyda[LMP_THETA] = -dyda[LMP_THETA];
        dyda[LMP_ORDER] = -dyda[LMP_ORDER];
        dyda[LMP_SPACE] = -dyda[LMP_SPACE];
    }

    if (r != NULL) {
        register cxint k;

        k = LMP_PXSIZ << 1;
        if (r[k+1] > 0) {
            dyda[LMP_PXSIZ] *= mrqdydaweight(a[LMP_PXSIZ],r[k],r[k+1]);
        }
        k = LMP_FCOLL << 1;
        if (r[k+1] > 0) {
            dyda[LMP_FCOLL] *= mrqdydaweight(a[LMP_FCOLL],r[k],r[k+1]);
        }
        k = LMP_CFACT << 1;
        if (r[k+1] > 0) {
            dyda[LMP_CFACT] *= mrqdydaweight(a[LMP_CFACT],r[k],r[k+1]);
        }
        k = LMP_THETA << 1;
        if (r[k+1] > 0) {
            dyda[LMP_THETA] *=  mrqdydaweight(a[LMP_THETA],r[k],r[k+1]);
        }
        k = LMP_ORDER << 1;
        if (r[k+1] > 0) {
            dyda[LMP_ORDER] *=  mrqdydaweight(a[LMP_ORDER],r[k],r[k+1]);
        }
        k = LMP_SPACE << 1;
        if (r[k+1] > 0) {
            dyda[LMP_SPACE] *=  mrqdydaweight(a[LMP_SPACE],r[k],r[k+1]);
        }
    }

} /* end mrqxoptmodGS() */

/**
 * @brief
 *   LMRQ Test Routine
 *
 * @param x     - Initial guess : [x]
 * @param a     - Function parameters (to be fitted...) :
 *                [a,x]
 * @param r     - Not used
 * @param na    - Number of parameters
 *
 * @par Description:
 * @code
 *
 *  mrqtest =    a * x + b
 *
 * @endcode
 *
 * @retval y    - Function value : mrqtest
 * @retval dyda - Function derivatives :
 *                [a,x]
 *
 */

void
mrqtest(cxdouble x[], cxdouble a[], cxdouble r[], cxdouble *y,
           cxdouble dyda[], cxint na)
{

    const cxchar *fctid = "mrqtest";

    cxdouble a1 = a[0];
    cxdouble b1 = a[1];

    (void) r;  /* Not used. */

    /* check for number of parameters */
    if (na != 2) {
        cpl_error_set(fctid, CPL_ERROR_ILLEGAL_INPUT);
        return;
    }

    *y = 0.0;
    *y = a1 * x[0] + b1;

    /* Check if derivatives expected */
    if (dyda == NULL)
        return;

    dyda[0] = 0.0;
    dyda[1] = 0.0;

    return;

} /* end mrqtest() */

/*@}**/

