/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
/*******************************************************************************
* E.S.O. - VLT project
*
*
*
* who       when      what
* --------  --------  ----------------------------------------------
* schreib  16/01/02  created
*/

/************************************************************************
*   NAME
*        sinfo_focus.c -
*------------------------------------------------------------------------
*/

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

/*
 * System Headers
 */

/*
 * Local Headers
 */

#include "sinfo_focus.h"
#include "sinfo_recipes.h"
#include <float.h>

/*----------------------------------------------------------------------------
 *                                 Defines
 *--------------------------------------------------------------------------*/

#define XDIMG          2         /* dimension of the x values */
#define TOLG           0.001     /* fitting tolerance */
#define LABG           0.1       /* labda parameter */
#define ITSG           200       /* maximum number of iterations */
#define LABFACG        10.0      /* labda step factor */
#define LABMAXG        1.0e+10   /* maximum value for labda */
#define LABMING        1.0e-10   /* minimum value for labda */
#define NPAR           7         /* number of fit parameters */
#define PI_NUMB        (3.1415926535897932384626433832795) /* pi */


/*----------------------------------------------------------------------------
 *                                    Local variables
 *--------------------------------------------------------------------------*/

static double chi1 ;                    /* old reduced chi-squared */
static double chi2 ;                    /* new reduced chi-squared */
static double labda ;                   /* mixing parameter */
static double vec[NPAR] ;               /* correction sinfo_vector */
static double matrix1[NPAR][NPAR] ;     /* original sinfo_matrix */
static double matrix2[NPAR][NPAR] ;     /* inverse of matrix1 */
static int    nfree ;                   /* number of free parameters */
static int    parptr[NPAR] ;            /* parameter pointer */

/*----------------------------------------------------------------------------
 *                    Functions private to this module
 *--------------------------------------------------------------------------*/

static int new_inv_mat (void) ;

static void new_get_mat ( double * xdat,
                      int    * xdim,
                      double * ydat,
                      double * wdat,
                      int    * ndat,
                      double * fpar,
                      double * epar/*,
                      int    * npar */) ;

static int new_get_vec ( double * xdat,
                     int    * xdim,
                     double * ydat,
                     double * wdat,
                     int    * ndat,
                     double * fpar,
                     double * epar,
                     int    * npar ) ;

static int new_gauss2Ellipse ( double  * parlist ) ;
/**@{*/
/**
 * @defgroup sinfo_focus Focus determination functions
 *
 * routines to determine the focus position of the detector
 */


/*----------------------------------------------------------------------------
 *                            Function codes
 *--------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------*/
/**
  @brief Compute the value of a 2d Gaussian function at a given point.
  @param xdat    position array
  @param parlist parameters array:

  # parlist[0]:     center of Gaussian in x direction
  # parlist[1]:     center of Gaussian in y direction
  # parlist[2]:     Amplitude of the 2d Gaussian
  # parlist[3]:     Background level
  # parlist[4]:     FWHMx, fwhm of the axis of the ellipse near the x-axis
  # parlist[5]:     FWHMy, fwhm of the axis of the ellipse near the y-axis
  # parlist[6]:     theta, position angle of the fwhmx line (-PI/4 to PI/4)
 
  @return           double value of the 2d Gaussian at position xdat.

  Compute the value of a 2d Gaussian function at a given point.
  The ellptical 2D Gaussian is:
  F(x,y) = par(2) * EXP( -4.0*log(2.0)*[(xr/par(4))^2+(yr/par(5))^2]) + par(3),
  where: xr = x0 * cos(par(6)) + y0 * sin(par(6))
         yr = -x0 * sin(par(6)) + y0 * cos(par(6))
  and:   x0 = x - par(0)
         y0 = y - par(1)
 */
/*--------------------------------------------------------------------------*/

double sinfo_new_gaussian_ellipse(double * xdat, double * parlist)
{
    double  result ;
    double x ;
    double y ;
    double fwhmx ;
    double fwhmy ;
    double costheta ;
    double sintheta ;
    double argX ;           /* arguments in the exponent */
    double argY ;

    /* some abbreviations */
    x =  xdat[0] -  parlist[0] ;
    y =  xdat[1] -  parlist[1] ;

    fwhmx = fabs(parlist[4]) ;
    fwhmy = fabs(parlist[5]) ;
    
    costheta = cos ( parlist[6] ) ;
    sintheta = sin ( parlist[6] ) ;

    argX = x * costheta + y * sintheta ;
    argY = -x * sintheta + y * costheta ;
    
    /* function */
    result =  parlist[2] * exp(-4.*log(2.0)*((argX/fwhmx)*(argX/fwhmx)+
                                             (argY/fwhmy)*(argY/fwhmy))) +
              parlist[3] ; 

    return result ;
}

/**
   @brief calculates the partial derivatives for a 2d Gaussian i
          function with parameters parlist at position xdat
   @param xdat    positiona array
   @param parlist parameter list:
   #                     parlist(0): center of Gaussian in x direction
   #                     parlist(1): center of Gaussian in y direction
   #                     parlist(2): Amplitude of 2d Gaussian
   #                     parlist(3): Background level
   #                     parlist(4): FWHMx
   #                     parlist(5): FWHMy
   #                     parlist(6): theta
   @param dervs  derivative value of a 2d Gaussian function at position xdat:
   #                     dervs[0]: partial derivative by center x
   #                     dervs[1]: partial derivative by center y 
   #                     dervs[2]: partial derivative by the amplitude
   #                     dervs[3]: partial derivative by the background
   #                     dervs[4]: partial derivative by FWHMx 
   #                     dervs[5]: partial derivative by FWHMy
   #                     dervs[6]: partial derivative by theta
  @return nothing, void

*/

void 
sinfo_new_gaussian_ellipse_deriv(double * xdat, 
                                 double * parlist, 
                                 double * dervs )
{
    double x ;
    double y ;
    double fwhmx ;
    double fwhmy ;
    double argX ;
    double argY ;
    double expon ;
    double e8log2 ;
    double fwx2 ;
    double fwy2 ;
    double costheta ;
    double sintheta ;

    /* some abbreviations */
    x = xdat[0] - parlist[0] ;
    y = xdat[1] - parlist[1] ;

    fwhmx = fabs(parlist[4]) ;
    fwhmy = fabs(parlist[5]) ;
    fwx2 = fwhmx * fwhmx ;
    fwy2 = fwhmy * fwhmy ;

    costheta = cos ( parlist[6] ) ;
    sintheta = sin ( parlist[6] ) ;

    argX = x * costheta + y * sintheta ;
    argY = -x * sintheta + y * costheta ;

    expon = exp ( -4.0 * log(2.0) * ((argX/fwhmx)*(argX/fwhmx) + 
                                     (argY/fwhmy)*(argY/fwhmy)) ) ;
    e8log2 = expon * 8.0 * log(2.0) ;

    /* determine the derivatives */
    /* partial derivative x-position */
    dervs[0] = -parlist[2]*e8log2 * (-argX*costheta/fwx2 + argY*sintheta/fwy2);
    /* partial derivative y-position */
    dervs[1] = -parlist[2]*e8log2 * (-argX*sintheta/fwx2 - argY*costheta/fwy2);
    /* partial derivative amplitude */
    dervs[2] = expon ;
    /* partial derivative background */
    dervs[3] = 1. ;
    /* partial derivative fwhmx */
    dervs[4] = parlist[2]*e8log2 * argX*argX/(fwx2*fwhmx) ;
    /* partial derivative fwhmy */
    dervs[5] = parlist[2]*e8log2 * argY*argY/(fwy2*fwhmy) ;
    /* partial derivative theta */
    dervs[6] = -parlist[2]*e8log2 * argY * argX * (1.0/fwx2 - 1.0/fwy2) ;
    
}

/**
@brief Calculates the inverse of matrix2.
   
   
   @return integer (0 if it worked, -6 if determinant is zero) 
   
   The algorithm used is the Gauss-Jordan algorithm described in Stoer,
   Numerische Mathematik, 1. Teil.

 */

static int new_inv_mat (void)
{
    double even ;
    double hv[NPAR] ;
    double mjk ;

    int evin ;
    int i, j, k;
    int per[NPAR] ;
   
    /* set permutation array */
    for ( i = 0 ; i < nfree ; i++ )
    {
        per[i] = i ;
    }
    
    for ( j = 0 ; j < nfree ; j++ ) /* in j-th column */
    {
        /* determine largest element of a row */                                
        double rowmax = fabs ( matrix2[j][j] ) ;
        int row = j ;

        for ( i = j + 1 ; i < nfree ; i++ )
        {
            if ( fabs ( matrix2[i][j] ) > rowmax )
            {
                rowmax = fabs( matrix2[i][j] ) ;
                row = i ;
            }
        }

        /* determinant is zero! */
        if ( matrix2[row][j] == 0.0 )
        {
            return -6 ;
        }
        
        /* if the largest element is not on the diagonal, 
           then permutate rows */
        if ( row > j )
        {
            for ( k = 0 ; k < nfree ; k++ )
            {
                even = matrix2[j][k] ;
                matrix2[j][k] = matrix2[row][k] ;
                matrix2[row][k] = even ;
            }
            /* keep track of permutation */
            evin = per[j] ;
            per[j] = per[row] ;
            per[row] = evin ;
        }
        
        /* modify column */
        even = 1.0 / matrix2[j][j] ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            matrix2[i][j] *= even ;
        }
        matrix2[j][j] = even ;
        
        for ( k = 0 ; k < j ; k++ )
        {
            mjk = matrix2[j][k] ;
            for ( i = 0 ; i < j ; i++ )
            {
                matrix2[i][k] -= matrix2[i][j] * mjk ;
            }
            for ( i = j + 1 ; i < nfree ; i++ )
            {
                matrix2[i][k] -= matrix2[i][j] * mjk ;
            }
            matrix2[j][k] = -even * mjk ;
        }
    
        for ( k = j + 1 ; k < nfree ; k++ )
        {
            mjk = matrix2[j][k] ;
            for ( i = 0 ; i < j ; i++ )
            {
                matrix2[i][k]  -= matrix2[i][j] * mjk ;
            }
            for ( i = j + 1 ; i < nfree ; i++ )
            {
                matrix2[i][k]  -= matrix2[i][j] * mjk ;
            }
            matrix2[j][k] = -even * mjk ;
        }
    }
    
    /* finally, repermute the columns */
    for ( i = 0 ; i < nfree ; i++ )
    {
        for ( k = 0 ; k < nfree ; k++ )
        {
            hv[per[k]] = matrix2[i][k] ;
        }
        for ( k = 0 ; k < nfree ; k++ )
        {
            matrix2[i][k] = hv[k] ;
        }
    }
        
    /* all is well */
    return 0 ;
}
    
/**
@brief build the matrix
   
   @param xdat: position
   @param xdim: factor of the indices of the position array
   @param ydat: real data
   @param wdat: weights
   @param ndat: number of data points
   @param fpar: function parameters
   @param epar: partial derivatives of the function
   @param npar: number of function parameters
   @return void
    
 */

static void new_get_mat ( double * xdat,
                      int    * xdim,
                      double * ydat,
                      double * wdat,
                      int    * ndat,
                      double * fpar,
                      double * epar/*,
                      int    * npar */)
{
    double wd ;

    double yd ;
    int i, j, n ;

    for ( j = 0 ; j < nfree ; j++ )
    {
        vec[j] = 0.0 ; /* zero sinfo_vector */
        for ( i = 0 ; i<= j ; i++ )   /* zero sinfo_matrix only on 
                                         and below diagonal */
        {
            matrix1[j][i] = 0.0 ;
        }
    }
    chi2 = 0.0 ;  /* reset reduced chi-squared */
    
    /* loop through data points */
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        double wn = wdat[n] ;
        if ( wn > 0.0 )  /* legal weight ? */
        {
            yd=ydat[n] - sinfo_new_gaussian_ellipse(&xdat[(*xdim) * n],fpar) ;
            sinfo_new_gaussian_ellipse_deriv( &xdat[(*xdim) * n], fpar, epar ) ;
            chi2 += yd * yd * wn ; /* add to chi-squared */
            for ( j = 0 ; j < nfree ; j++ )
            {
                wd = epar[parptr[j]] * wn ;  /* weighted derivative */
                vec[j] += yd * wd ;       /* fill sinfo_vector */
                for ( i = 0 ; i <= j ; i++ ) /* fill sinfo_matrix */
                {
                    matrix1[j][i] += epar[parptr[i]] * wd ;
                }
            }
        }
    }                   
}  
                
            
/**
@brief Computes the correction vector
  
  @param xdat: position
  @param xdim: factor of the indices of the position array
  @param ydat: real data
  @param wdat: weights
  @param ndat: number of data points
  @param fpar: function parameters
  @param epar: partial derivatives of the function
  @param npar: number of function parameters

  @return integer (
          # 0 if it had worked, 
          # -5 or -7 if diagonal element is wrong, or 
          # -6, if determinant is zero 
          )
 

calculates the correction vector. The matrix has been
built by get_mat(), we only have to re-scale it for the current value of lambda.
The matrix is rescaled so that the diagonal gets the value 1 + lambda.
Next we calculate the inverse of the matrix and then the correction vector.


 */
            
static int new_get_vec ( double * xdat,
                     int    * xdim,
                     double * ydat,
                     double * wdat,
                     int    * ndat,
                     double * fpar,
                     double * epar,
                     int    * npar )
{

    double dy ;
    double mii ;
    double mji ;
    double mjj ;

    int i, j, n, r ;

    /* loop to modify and scale the sinfo_matrix */
    for ( j = 0 ; j < nfree ; j++ )
    {
        mjj = matrix1[j][j] ;
        if ( mjj <= 0.0 )             /* diagonal element wrong */
        {
            return -5 ;
        }
        mjj = sqrt( mjj ) ;
        for ( i = 0 ; i < j ; i++ )
        {
            mji = matrix1[j][i] / mjj / sqrt( matrix1[i][i] ) ;
            matrix2[i][j] = matrix2[j][i] = mji ;
        }
        matrix2[j][j] = 1.0 + labda ;  /* scaled value on diagonal */
    }    
    
    if ( (r = new_inv_mat()) ) /* sinfo_invert sinfo_matrix inlace */
    {
        return r ;
    }
    
    for ( i = 0 ; i < (*npar) ; i ++ )
    {
        epar[i] = fpar[i] ;
    }
    
    /* loop to calculate correction sinfo_vector */
    for ( j = 0 ; j < nfree ; j++ )
    {
        double dj = 0.0 ;
        mjj = matrix1[j][j] ;
        if ( mjj <= 0.0)               /* not allowed */
        {
            return -7 ;
        }
        mjj = sqrt ( mjj ) ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            mii = matrix1[i][i] ;
            if ( mii <= 0.0 )
            {
                return -7 ;
            }
            mii = sqrt( mii ) ;
            dj += vec[i] * matrix2[j][i] / mjj / mii ;
        }
        epar[parptr[j]] += dj ;       /* new parameters */
    }    
    chi1 = 0.0 ;                      /* reset reduced chi-squared */
 
    /* loop through the data points */
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        double wn = wdat[n] ;               /* get weight */
        if ( wn > 0.0 )              /* legal weight */
        {
            dy=ydat[n] - sinfo_new_gaussian_ellipse(&xdat[(*xdim) * n],epar);
            chi1 += wdat[n] * dy * dy ;
        }
    }
    return 0 ;
}   
    
        

/**
@brief Least-squares fit of a function to a set of data points.
   
   @param xdat: position, coordinates of data points.
                xdat is 2 dimensional: XDAT ( XDIMG, NDAT )
   @param xdim: dimension of fit
   @param ydat: data points
   @param wdat: weights for data points
   @param ndat: number of data points
   @param fpar: on input contains initial estimates of the parameters for 
                non-linear fits, on output the fitted parameters.
   @param epar: contains estimates of the errors in fitted parameters
   @param mpar: logical mask telling which parameters are free (non-zero)
                              and which parameters are fixed (0)
   @param npar: number of function parameters ( free + fixed )
   @param tol:  relative tolerance. sinfo_lsqfit stops when successive 
                iterations
                fail to produce a decrement in reduced chi-squared less
                than tol. If tol is less than the minimum tolerance 
                possible, tol will be set to this value. This means
                that maximum accuracy can be obtained by setting tol = 0.0.
   @param its:  maximum number of iterations
   @param lab:  mixing parameter, lab determines the initial weight
                of steepest descent method relative to the Taylor method
                lab should be a small value (i.e. 0.01). lab can only
                be zero when the partial derivatives are independent
                of the parameters. In fact in this case lab should be
                exactly equal to zero.

   @return returns number of iterations needed to achieve convergence according 
           to tol. When this number is negative, the fitting was not continued
           because a fatal error occurred:
           #             -1 too many free parameters, maximum is 32
           #             -2 no free parameters
           #             -3 not enough degrees of freedom
           #             -4 maximum number of iterations too small to obtain
                           a solution which satisfies tol.
           #             -5 diagonal of matrix contains elements which are zero
           #             -6 determinant of the coefficient matrix is zero
           #             -7 square root of a negative number 
   
 This is a routine for making a least-squares fit of a function to a set of 
 data points. The method used is described in: 
 Marquardt, J.Soc.Ind.Appl.Math. 11. 431 (1963).
 This method is a mixture of the steepest descent method and the Taylor method.

 */

int sinfo_new_lsqfitd ( double * xdat,
              int    * xdim,
              double * ydat,
              double * wdat,
              int    * ndat,
              double * fpar,
              double * epar,
              int    * mpar,
              int    * npar,
              double * tol ,
              int    * its ,
              double * lab  )
{
    int i, n, r ;
    int itc ;                      /* fate of fit */
    int found ;                    /* fit converged: 1, not yet converged: 0 */
    int  nuse ;                    /* number of useable data points */
    double tolerance ;             /* accuracy */

    itc   = 0 ;                    /* fate of fit */
    found = 0 ;                    /* reset */
    nfree = 0 ;                    /* number of free parameters */
    nuse  = 0 ;                    /* number of legal data points */

    if ( *tol < (DBL_EPSILON * 10.0 ) )
    {
        tolerance = DBL_EPSILON * 10.0 ;  /* default tolerance */
    }
    else
    {
        tolerance = *tol ;                /* tolerance */
    }
    
    labda = fabs( *lab ) * LABFACG ; /* start value for mixing parameter */
    for ( i = 0 ; i < (*npar) ; i++ )
    {
        if ( mpar[i] )
        {
            if ( nfree > NPAR )         /* too many free parameters */
            {
                return -1 ;
            }
            parptr[nfree++] = i ;         /* a free parameter */
        }
    }
    
    if (nfree == 0)                       /* no free parameters */     
    {
        return -2 ;
    }
    
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        if ( wdat[n] > 0.0 )              /* legal weight */
        {
            nuse ++ ;
        }
    }
    
    if ( nfree >= nuse )
    {
        return -3 ;                       /* no degrees of freedom */
    }
    if ( labda == 0.0 )                   /* linear fit */
    {
        /* initialize fpar array */
        for ( i = 0 ; i < nfree ; fpar[parptr[i++]] = 0.0 ) ;  
        new_get_mat ( xdat, xdim, ydat, wdat, ndat, fpar, epar/*, npar */) ;
        r =  new_get_vec ( xdat, xdim, ydat, wdat, ndat, fpar, epar, npar ) ;
        if ( r )                         /* error */
        {
            return r ;
        }
        for ( i = 0 ; i < (*npar) ; i++ )
        {
            fpar[i] = epar[i] ;           /* save new parameters */
            epar[i] = 0.0 ;               /* and set errors to zero */
        }
        chi1 = sqrt( chi1 / (double) (nuse - nfree) ) ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            if ( (matrix1[i][i] <= 0.0 ) || (matrix2[i][i] <= 0.0) )
            {
                return -7 ;
            }
            epar[parptr[i]] = chi1 * sqrt( matrix2[i][i] ) / 
                                     sqrt( matrix1[i][i] ) ;
        }
    }
    else                                  /* non-linear fit */
    {
        /*----------------------------------------------------------------
         * the non-linear fit uses the steepest descent method in combination
         * with the Taylor method. The mixing of these methods is controlled
         * by labda. In the outer loop ( called the iteration loop ) we build
         * the matrix and calculate the correction sinfo_vector. In the 
           inner loop
         * (called the interpolation loop) we check whether we have obtained a
         * better solution than the previous one. If so, we leave the inner loop
         * else we increase lambda ( give more weight to the steepest descent 
         * method) calculate the correction vector and check again. After the 
         * inner loop
         * we do a final check on the goodness of the fit and if this satisfies
         * the tolerance we calculate the errors of the fitted parameters.
         */
        while ( !found )                  /* iteration loop */
        {      
            if ( itc++ == (*its) )        /* increase iteration counter */
            {
                return -4 ;               
            }
            new_get_mat( xdat, xdim, ydat, wdat, ndat, fpar, epar/*, npar*/ ) ;
            
            /*-------------------------------------------------------------
             * here we decrease labda since we may assume that each iteration
             * brings us closer to the answer.
             */
            if ( labda > LABMING )
            {
                labda = labda / LABFACG ;         /* decrease labda */
            }
            r = new_get_vec ( xdat, xdim, ydat, wdat, ndat, fpar, epar, npar ) ;

            if ( r )                      /* error */
            {
                return r ;
            }

            while ( chi1 >= chi2 )        /* interpolation loop */
            {
                /*-----------------------------------------------------------
                 * The next statement is based on experience, not on the 
                 * mathematics of the problem. It is assumed that we have 
                 * reached convergence when the pure steepest descent method 
                 * does not produce a better solution.
                 */
                if ( labda > LABMAXG )    /* assume solution found */
                {
                    break ;
                }
                labda = labda * LABFACG ; /* increase mixing parameter */
                r = new_get_vec(xdat,xdim,ydat,wdat,ndat,fpar,epar,npar) ;

                if ( r )                  /* error */
                {
                    return r ;
                }
            }

            if ( labda <= LABMAXG )        /* save old parameters */
            {
                for ( i = 0 ; i < *npar ; i++ )
                {
                    fpar[i] = epar[i] ;
                }
            }
            if ( (fabs( chi2 - chi1 ) <= (tolerance * chi1)) || 
                      (labda > LABMAXG) )
            {
                /*-----------------------------------------------------------
                 * we have a satisfying solution, so now we need to calculate 
                 * the correct errors of the fitted parameters. This we do by 
                 * using the pure Taylor
                 * method because we are very close to the real solution.
                 */
                labda = LABMING ;              /* for Taylor solution */
                new_get_mat(xdat,xdim,ydat,wdat,ndat,fpar,epar/*, npar */) ;
                r=new_get_vec(xdat,xdim,ydat,wdat,ndat,fpar,epar,npar ) ;

                if ( r )                    /* error */
                {
                    return r ;
                }
                for ( i = 0 ; i < (*npar) ; i++ )
                {
                    epar[i] = 0.0 ;          /* set error to zero */
                }
                chi2 = sqrt ( chi2 / (double) (nuse - nfree) ) ;

                for ( i = 0 ; i < nfree ; i++ )
                {
                    if ( (matrix1[i][i] <= 0.0) || (matrix2[i][i] <= 0.0) )
                    {
                        return -7 ;
                    }
                    epar[parptr[i]] = chi2 * sqrt( matrix2[i][i] ) / 
                                             sqrt( matrix1[i][i] ) ;
                }
                found = 1 ;                  /* we found a solution */
            }
        }
    }
    return itc ;                             /* return number of iterations */
}

/**
  @brief fits the image of a point source by using a 2-D Gaussian fit.
   
  @param image reconstructed image of a point source 
  @param fit_par    array of 7 free fit parameters of a 2D-Gauss
  @param derv_par   derivatives of the fit_par array
  @param mpar       mask to set free parameters, 1: free, 0: fixed. 
  @param lleftx     lower left starting point of the box in which the fit is 
                    carried throught to find the maximum of point source image
  @param llefty     lower left starting point of the box in which the fit is 
                    carried throught to find the maximum of point source image
  @param halfbox_x  half box length in x-direction in pixels inside which 
                    the fit is carried through
  @param halfbox_y  half box length in y-direction in pixels inside 
                    which the fit is carried through

  @param check      check switch (start at 0 and is increased in case of 
                    failure
  @return number of needed iterations
          #              -1 if an error occurred.
   
  Remark on the fit results for the fit parameters 
  (fwhmx and fwhmy and theta): 
  theta will always be between -PI/4 and +PI/4, exchange of the fwhmx and fwhmy
  values corresponds to a shift of theta by PI/2. Consequently, an expected 
  theta > |PI/4| will result in an exchange of the fwhm values and a shift of 
  theta by PI/2 to a value < |PI/4| what yields exactly the same image.


 */

int 
sinfo_new_fit_2d_gaussian ( cpl_image   * image, 
                    double     * fit_par, 
                    double     * derv_par,   
                    int        * mpar,
                    int          lleftx,
                    int          llefty,
                    int          halfbox_x,
                    int          halfbox_y, 
                    int* check )
{
    int i, j, n ;
    int col, row ;
    int boxi, boxj ;
    int iters ;
    int ndata ;
    int xdim ;
    int npar ;
    int its ;
    double lab ;
    double tol ;
    double maxval ;
    double background ;
    double amplitude ;
    float * backarray=NULL ;
    double M, Mx, My ;
    double Mxx, Mxy, Myy ; 
    double X0, Y0 ;
    double xydat[4 *halfbox_x*halfbox_y][XDIMG] ;
    double zdat[4*halfbox_x*halfbox_y] ;
    double wdat[4*halfbox_x*halfbox_y] ;
    double xco, yco ;
    double value ;
    double denom ;

    int llx, lly ;
    int foundrow ;
    int foundcol ;

    int ilx=0;
    int ily=0;
    float* pidata=NULL;

    memset(&wdat[0], 0, (4*halfbox_x*halfbox_y)* sizeof(double));

    if ( NULL == image )
    {
        sinfo_msg_error("no image given") ;
        return -1 ;
    }
    ilx=cpl_image_get_size_x(image);
    ily=cpl_image_get_size_y(image);

    if ( NULL == fit_par )
    {
        sinfo_msg_error("no fit parameters given") ;
        return -1 ;
    }
    if ( NULL == derv_par )
    {
        sinfo_msg_error("no derivatives of fit parameters given") ;
        return -1 ;
    }
    if ( lleftx < 0 || lleftx + 2*halfbox_x >= ilx ||
         llefty < 0 || llefty + 2*halfbox_y >= ily )
    {
        sinfo_msg_error("wrong lower left point of fitting box given!") ;
        return -1 ;
    }
    if ( halfbox_x <= 1 || halfbox_y <= 1 )
    {
        sinfo_msg_error("wrong box dimensions given") ;
        return -1 ;
    }
    /* allocate memory */
    if ( NULL == (backarray = (float*) cpl_calloc(4*halfbox_x+4*halfbox_y, 
                  sizeof(float) ) ) ) 
    {
        sinfo_msg_error("could not allocate memory") ;
        return -1 ;
    }


    /* -------------------------------------------------------------------
     * find the initial estimates for the free parameters
     */

    /* first search for the position of the maximum intensity */
    foundrow = 0 ;
    foundcol = 0 ;
    maxval   = -SINFO_DBL_MAX ;
    pidata=cpl_image_get_data_float(image);
    for ( col = lleftx ; col < lleftx + 2*halfbox_x ; col++ )
    {
        for ( row = llefty ; row < llefty + 2*halfbox_y ; row++ )
        {
            if ( isnan(pidata[col+row*ilx]) )
            {
                continue ;
            }
            if ( maxval < pidata[col+row*ilx] ) 
            {
                maxval = pidata[col+row*ilx] ;
                foundrow = row ;
                foundcol = col ;
            }
        }
    }

    if ( foundrow == 0 || foundcol == 0 || maxval <= 0. ||
         foundrow == ilx-1 || foundcol == ily-1 )
    {
        sinfo_msg_warning("no maximum found") ;
        cpl_free(backarray) ;
        return -1 ;
    }

    /* determine the lower left sinfo_edge of the fitting box, center it 
       on the maximum value */
    llx = foundcol - halfbox_x ;
    lly = foundrow - halfbox_y ;
    if ((foundcol - halfbox_x) > 0) {
      llx = (foundcol - halfbox_x);
    } else {
      llx=1;
      check++;
    }

    if ((foundrow - halfbox_y) > 0) {
      lly = (foundrow - halfbox_y);
    } else {
      lly=1;
      check++;
    } 

    if ( ( llx + 2*halfbox_x) <  ilx-1 ) {
       //halfbox_x=halfbox_x;
    } else {
       halfbox_x=(int) (ilx-2-llx)/2;
      check++;
    }

    if ( ( lly + 2*halfbox_y) <  ily-1 ) {
       //halfbox_y= halfbox_y;
    } else {
      halfbox_y=(int) (ily-2-lly)/2;
      check++;
    }

    if ( llx <= 0 || lly < 0 || llx + 2*halfbox_x >= ilx-1 ||
         lly + 2*halfbox_y >= ily )
    {
        sinfo_msg_error("box does not fit into image") ;
        cpl_free(backarray) ;
        return -1 ;
    }
    
    /* determine the zeroth and first order moments of the image  
       within the fitting box */ 
    M = Mx = My = 0. ;
    n = 0 ;
    boxi = boxj = 0 ;
    for ( j = lly ; j < lly + 2*halfbox_y ; j++ )
    {
        boxj = j - lly ;
        for ( i = llx ; i < llx + 2*halfbox_x ; i++ )
        {
            boxi = i - llx ;
            if ( !isnan(pidata[i+j*ilx]) )
            {
                M  += pidata[i+j*ilx] ;  
                Mx += (double)boxi * pidata[i+j*ilx] ;
                My += (double)boxj * pidata[i+j*ilx] ;
                /*-----------------------------------------------------------
                 * estimate the amplitude and the background 
                 * go through the margins of the fitting box 
                 * and calculate the clean mean to
                 * determine the background 
                 */
                if ( i == llx || i == llx + 2*halfbox_x -1 ||
                     j == lly || j == lly + 2*halfbox_y -1 )
                {
                    backarray[n] = pidata[i+j*ilx] ;
                    n++ ;
                }
            }
        }
    }
    if ( M <= 0. )
    {
        sinfo_msg_warning("only negative or zero values") ;
        cpl_free(backarray) ;
        return -1 ;
    }
    if ( n < 3 )
    {
        sinfo_msg_error("not enough data points to calculate background") ;
        cpl_free(backarray) ;
        return -1 ;
    }
    /* determine the background as sinfo_median of the surrounding pixels */
    if (FLT_MAX==(background=sinfo_new_clean_mean(backarray,n,10.,10.))) 
    {
        sinfo_msg_error("it was not possible to compute the "
                        "clean mean of the background values") ;
        cpl_free(backarray) ;
        return -1 ;
    }
    cpl_free (backarray) ;
    /* now calculate the amplitude estimation */
    amplitude = maxval - background ;
    if ( amplitude < 1e-12 )
    {
        sinfo_msg_warning("amplitude is too small") ;
        return -1 ;
    }

    /* determine the center of gravity = centroid */
    X0 = Mx / M ;
    Y0 = My / M ;
    /* if one of the values is outside the fitting box return with error */
    if ( X0 <= 0. || Y0 <= 0. || X0 >= 2.*(double)halfbox_x || 
         Y0 >= 2.*(double)halfbox_y )
    {
        sinfo_msg_warning("center of gravity is outside the fitting box!") ;
        return -1 ;
    }
    
    /*------------------------------------------------------------------------ 
     * put the data in the 2-d array xydat[][] (pixel position) and zdat[] 
     * (data values) additionally, determine the second order momentum
     */
    n = 0 ;
    M = Mx = Mxx = My = Myy = Mxy = 0. ;
    boxi = boxj = 0 ;
    for ( j = lly ; j < lly + 2*halfbox_y ; j++ )
    {
        boxj = j - lly ;
        for ( i = llx ; i < llx + 2*halfbox_x ; i++ )
        {
            boxi = i - llx ;
            value = pidata[i+j*ilx] ;
            if ( !isnan(value) )
            {
                xydat[n][0] = (double) boxi ;
                xydat[n][1] = (double) boxj ;
                zdat[n]     = value ;
                wdat[n]     = 1. ;
                n++ ; 
               
                /* now calculate the moments without background in the 
                   centroid coordinate system */
                value -= background ;
                xco = (double) boxi - X0 ;
                yco = (double) boxj - Y0 ;
                M   += value ;
                Mx  += xco * value ;
                My  += yco * value ;
                Mxx += xco * xco * value ;
                Myy += yco * yco * value ;
                Mxy += xco * yco * value ;
            }
        }
    }
    if ( M <= 0. )
    {
        sinfo_msg_warning("only negative or zero values") ;
        return -1 ;
    }
  
    /* ----------------------------------------------------------------
     * estimate the fwhm_x and fwhm_y and theta 
     */ 
  
    /* first scale the moments */
    /* TODO: why use Mx is later this is never used? */
    //Mx  /= M ;
    //My  /= M ;
    Mxx /= M ;
    Myy /= M ;
    Mxy /= M ;
   
    denom = 2. * (Mxx*Myy - Mxy*Mxy) ;
    if ( denom == 0. )
    {
        sinfo_msg_error("denominator is zero!") ;
        return -1 ;
    }
    
    /* now associate the parameter list with the found estimates */
    fit_par[0] = X0 ;
    fit_par[1] = Y0 ;
    fit_par[2] = amplitude ;
    fit_par[3] = background ;
    fit_par[4] = Myy/denom ;
    fit_par[5] = Mxx/denom ;
    fit_par[6] = -Mxy/denom ;

    /* convert the moments to ellipse paramters */
    if ( 0 > new_gauss2Ellipse (fit_par) )
    {
        sinfo_msg_warning("gauss2Ellipse does not run!") ;
        return -1 ;
    }

    /* total number of data points */
    ndata = 4 * halfbox_x * halfbox_y ;
    xdim = XDIMG ; /* dimension of xydat array */
    npar = NPAR ; /* number of parameters in the fit */
    its = ITSG ;
    lab = LABG ;
    tol = TOLG ;
    for ( i = 0 ; i < NPAR ; i++ )
    {
        derv_par[i] = 0. ;
    }
    
    if ( 0 > ( iters = sinfo_new_lsqfitd ( &xydat[0][0],
                                 &xdim,
                                 zdat,
                                 wdat, 
                                 &ndata,
                                 fit_par,
                                 derv_par,
                                 mpar,
                                 &npar,
                                 &tol,
                                 &its,
                                 &lab )) ) 
    {
        sinfo_msg_warning(" least squares fit failed, error no: %d!", iters) ;
        return -1 ;
    }

    /* exclude impossible fit results */
    if ( fit_par[2] <= 0. || fit_par[4] < 0. || fit_par[5] < 0. )
    {
        sinfo_msg_error("sorry, some impossible negative fit results!") ;
        sinfo_msg_error("amplitude=%g, background=%g, Mxx/denom =%g",
                        fit_par[2],fit_par[4],fit_par[5]);
        return -1 ;
    }

    fit_par[0] += llx ;
    fit_par[1] += lly ;
    if ( fit_par[0] < llx || fit_par[0] >= llx + 2*halfbox_x ||
         fit_par[1] < lly || fit_par[1] >= lly + 2*halfbox_y )
    {
        sinfo_msg_error("sorry, 2D-Gaussian (x,y)=(%g,%g) position is outside "
                        "the fitting box (llx,lly,urx,ury)=(%d,%d,%d,%d)",
                        fit_par[0],fit_par[1],llx,lly,
                        llx + 2*halfbox_x,lly + 2*halfbox_y) ;
        return -1 ;
    }

    /* exchange fwhmx and fwhmy if |theta| is bigger than 
       pi/4 and subtract pi/2 from theta */
    if ( fabs ( fit_par[6] ) > PI_NUMB / 4. )
    {
        /* first convert angle to smaller than 2 pi */
        if ( fabs (fit_par[6]) >= 2. * PI_NUMB )
        { 
            int k = (int) (fit_par[6] / (2.*PI_NUMB)) ;
            if ( k > 0 ) 
            {
                fit_par[6] -= k*2.*PI_NUMB ;
            }
            else
            {
                fit_par[6] += k*2.*PI_NUMB ;
            }
        }
        /* first convert angle to smaller than pi/2 */
        if ( fabs (fit_par[6]) > PI_NUMB / 2. )
        {
            if ( fit_par[6] > 0. )
            {
                fit_par[6] -= PI_NUMB ;
            }
            else
            {
                fit_par[6] += PI_NUMB ;
            }
        }

        if ( fabs (fit_par[6]) > PI_NUMB / 4. )
        {
            double temp       = fit_par[4] ;
            fit_par[4] = fit_par[5] ;
            fit_par[5] = temp ;
            if ( fit_par[6] < 0. )
            { 
                fit_par[6] += PI_NUMB / 2. ;
            }
            else
            {
                fit_par[6] -= PI_NUMB / 2. ;
            }  
        }
    }
    
    return iters ;
}

/**
@brief converts gauss parameters to ellipse parameters.

   @param parlist: parameters of 2D-Gaussian
   @return 0 (transformed Gaussian parameters to ellipse parameters)
  */

static int new_gauss2Ellipse ( double     * parlist )
{
    double a, b, c ;
    double ellipseconst ;
    double axisX, axisY, phi ;
    double p ;
    
    if ( parlist == NULL )
    {
        sinfo_msg_error(" no parameters given!\n") ;
        return -1 ;
    }

    a = parlist[4] ; /* fwhmx */
    b = parlist[5] ; /* fwhmy */
    c = parlist[6] ; /* theta */

    ellipseconst = 2. * log(2.) ;

    if ( a*b - c*c <= 0. )
    {
        sinfo_msg_warning("estimates of moments are unusable, "
                          "they do not make an ellipse!") ;
        return -1 ;
    }
    
    if ( a == b )
    { 
        phi = 0. ;
    }
    else
    {
        phi = 0.5 * atan( 2. * c / (a-b) ) ;
    }

    p = sqrt ( (a-b) * (a-b) + 4. * c*c ) ;

    if ( a > b )
    {
        axisX = 2. * sqrt ( ellipseconst / (a+b+p) ) ;
        axisY = 2. * sqrt ( ellipseconst / (a+b-p) ) ;
    }
    else
    {
        axisX = 2. * sqrt ( ellipseconst / (a+b-p) ) ;
        axisY = 2. * sqrt ( ellipseconst / (a+b+p) ) ;
    }

    parlist[4] = axisX ;
    parlist[5] = axisY ;
    parlist[6] = phi ;

    return 0 ;
}

/**
@brief Determines the conversion factor for an instrument
   
   @param cube       reduced data cube of a standard star
   @param mag        brightness of the standard star
   @param exptime    exposure time read from the fits header
   @param llx        lower left point of fitting box
   @param lly        lower left point of fitting box
   @param halfbox_x  half width of a box inside which
                     a 2D-Gauss fit is carried out
   @param halfbox_y  half width of a box inside which
                     a 2D-Gauss fit is carried out
   @param check      if search box is outside image definition.

   @return intensity conversion value: magnitude per counts/s.
           -FLT_MAX if error occurred.
   
   determines an intensity conversion factor for the instrument by fitting a
   2D-Gaussian to an collapsed image of a standard star with known brightness 
   (only for non-AO observations). Then the resulting Gaussian is integrated 
   and the counts are divided by the exposure time (Fits header information) 


 */

float sinfo_new_determine_conversion_factor ( cpl_imagelist * cube, 
                                  float     mag,
                                  float     exptime,
                                  int       llx,
                                  int       lly,
                                  int       halfbox_x,
                                  int       halfbox_y, 
                                  int* check )
{
    int row, col, i ;
    int first_row, first_col ;
    int last_row, last_col ;
    float factor ;
    int mpar[7] ;
    double fit_par[7] ;
    double derv_par[7] ;

    double sum ;
    double xdat[2] ;
    cpl_image * summedIm ;

    int ilx=0;
    int ily=0;
    //int inp=0;

    if ( NULL == cube )
    {
        sinfo_msg_error(" no cube given!\n") ;
        return -FLT_MAX ;
    }

    ilx=cpl_image_get_size_x(cpl_imagelist_get(cube,0));
    ily=cpl_image_get_size_y(cpl_imagelist_get(cube,0));
    //inp=cpl_imagelist_get_size(cube);

    if ( halfbox_x <= 0 || halfbox_y <= 0 || 
       2*halfbox_x > ilx || 2*halfbox_y > ily)
    {
        sinfo_msg_error(" wrong width of halfbox given!") ;
        return -FLT_MAX ;
    }
    if ( exptime <= 0. )
    {
        sinfo_msg_error(" impossible exposure time given !") ;
        return -FLT_MAX ;
    }

    /* collapse the cube to be able to do 2D-Gaussian fitting */
    if ( NULL == (summedIm = sinfo_new_sum_cube_to_image(cube)) )
    {
        sinfo_msg_error(" sinfo_averageCubeToImage failed!") ;
        return -FLT_MAX ;
    }

    /* call the 2D-Gaussian fit routine */
    for ( i = 0 ; i < 7 ; i++ )
    {
        mpar[i] = 1 ;
    }
    int fitInd ;
    if ( -1 == (fitInd = sinfo_new_fit_2d_gaussian(summedIm, fit_par, derv_par,
                                                   mpar, llx, lly, halfbox_x, 
                                                   halfbox_y, check)) )
    {
        sinfo_msg_warning("sinfo_fit2dGaussian failed!") ;
        cpl_image_delete( summedIm) ;
        return -FLT_MAX ;
    }
    cpl_image_delete(summedIm) ;

    /* now integrate the found 2D Gaussian by first 
       subtracting the background */
    if  ((fit_par[0] - halfbox_x) < 0) {
      first_col=0;
      check++;
    } else {
      first_col=(fit_par[0] - halfbox_x);
    }

    if ((fit_par[0] + halfbox_x) < ilx) {
      last_col  = (fit_par[0] + halfbox_x);
    } else {
      last_col = (ilx-1) ;
      check++;
    }

    if ((fit_par[1] - halfbox_y) < 0) {
      first_row=0;
      check++;
    } else {
      first_row=(fit_par[1] - halfbox_y) ;
    }

    if ((fit_par[1] + halfbox_y) < ily) {
      last_row=(fit_par[1] + halfbox_y);
    } else {
      last_row= (ily-1);
      check++;
    }


    if ( first_col < 0 || first_row < 0 || last_col >= ilx || last_row >= ily )
    {
        sinfo_msg_error("star badly centered in FOV or fitting box too big!") ;
        return -FLT_MAX ;
    }
    sum = 0. ;
    for ( row = first_row ; row < last_row ; row++ )  
    {
        for( col = first_col ; col < last_col ; col++ )
        {
            xdat[0] = (double) col ;
            xdat[1] = (double) row ;
            sum += (sinfo_new_gaussian_ellipse( xdat, fit_par ) - fit_par[3]) ;
        }
    }
    if ( sum <= 0. )
    {
        sinfo_msg_error("zero or negative sum of counts!") ;
        return -FLT_MAX ;
    }
    factor = mag / (float)sum * exptime ;
    return factor ;
}
                              
/*--------------------------------------------------------------------------*/
/**@}*/
