/*                                                                              *
 *   This file is part of the ESO UVES Pipeline                                 *
 *   Copyright (C) 2004,2005 European Southern Observatory                      *
 *                                                                              *
 *   This library 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       *
 *                                                                              */

/*
 * $Author: amodigli $
 * $Date: 2010-09-24 09:32:07 $
 * $Revision: 1.11 $
 * $Name: not supported by cvs2svn $
 */

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

/*---------------------------------------------------------------------------*/
/**
 * @defgroup uves_physmod_cstacen Utility for Physical Model
 * 
 * This module contains functionalities to do a Gaussian fit along X,Y to 
 * a line (equivalent of MIDAS cstacen.c)
 */
/*---------------------------------------------------------------------------*/

/* code derived by MIDAS cstacen.c */
/*-----------------------------------------------------------------------------
                                Includes
 ----------------------------------------------------------------------------*/
/* definition of the used functions in this module */

#include "uves_physmod_cstacen.h"
#include <cpl.h>
#include <math.h>
/*-----------------------------------------------------------------------------
                                Defines
 ----------------------------------------------------------------------------*/
/* Define _POSIX_SOURCE to indicate that this is a POSIX program */
/* replaced osmmget by cpl_calloc */
/* replaced osmmfree by cpl_free */
#define  _POSIX_SOURCE 1

/* define some macros and constants */

#ifndef  PI
#define  PI             3.14159265358979325e0
#endif
/*
#ifndef true
#define true            1
#define false           0
#endif
*/

/* Constants used by the moment centering */

#define MINVAL      1.0e-37
#define MMXITER     8
#define SMALL           1.0e-20


/* Constants used by the gaussian centering */

#define MAXPAR        4
#define IGNORE        2
#define NOCONV        -1
#define OUTSIDE        -2
#define GMXITER     50
#define GCHIMAX        5.0e+16
#define GCHIFND        0.005


#define MYMIN(a,b)   ((a) > (b) ? (b) : (a))
#define MYMAX(a,b)   ((b) > (a) ? (b) : (a))

/**@{*/

/*-----------------------------------------------------------------------------
                            Functions prototypes
 ----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
                            Static variables
 ----------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------
                            Functions code
 ----------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/**
  @brief  Routines used to do Gaussian fit to a line

  @param  p_img   : input pointer to image
  @param  dimx     : input image's x dimension
  @param  dimy     : input image's y dimension
  @param  meth    : input method of centering: MOM (moment centering)
                                   GAU (gaussian centering)
  @param  image   : input image mapping array
  @param  xout    : ouput center position x (C indexing)
  @param  yout    : ouput center position x (C indexing)
  @param  xerr    : ouput error estimate of xout 
  @param  yerr    : ouput error estimate of yout
  @param  xsig    : output width of source on x 
  @param  ysig    : output width of source on x 
  @param  xyval   : output central value
  @param  stat     : return status

  @return   0 if everything is ok,
            1 if crowded, or weak source
            2 if no pos. source found
            3 if iteration failed

   This module contains the following statistic functions:
   - for the moment centering:   Ckapsig
   - for the gaussian centering: - Crhox  
                                 - Crhoy  
                                 - Cserch 
                                 - LSQFIT 
                                          - <- GAUSDE <- GAUSFU <- ERFCC
                                          - <- MATINV
                                          - <- GAUSFU <- ERFCC
                                          - <- FCHIS

  Performs a Gaussian fit of the intensity distribution obtained by collapsing
  along X or Y an image within a box.

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

int 
uves_physmod_stacen(float* p_img, int dimx, int dimy, char meth, int* image, 
                  float* xout, float* yout, float* xerr, float* yerr,
                  float* xsig, float* ysig, float* xyval, int* stat )

{
int   npix[2], imap[4];
float xypos[2], xyerr[2], xysig[2];

npix[0]  = dimx;
npix[1]  = dimy;

imap[0] = image[0] - 1;
imap[1] = image[1] - 1;
imap[2] = image[2] - 1;
imap[3] = image[3] - 1;

/*
 uves_msg("Input=: npix[0]=%d npix[1]=%d",npix[0],npix[1]);
 uves_msg("Input=: imap[0]=%d imap[1]=%d imap[2]=%d imap[3]=%d",imap[0],imap[1],imap[2],imap[3]);
*/

*stat = uves_physmod_cstacen(meth, p_img, npix, imap, xypos, xyerr, xysig, xyval );

*xout = xypos[0] + 1;
*yout = xypos[1] + 1;


*xerr = xyerr[0];
*yerr = xyerr[1];
*xsig = xysig[0];
*ysig = xysig[1];
/*
 uves_msg("xout=%f,yout=%f,xerr=%f,yerr=%f,xsig=%f,ysig=%f",
         *xout,*yout,*xerr,*yerr,*xsig,*ysig);
     */

 return 0;
} 
/*---------------------------------------------------------------------------*/
/**
  @brief  finds absolute value of nearest integer
  @param  a  value to convert
*/

static int CGN_NINT(float a){   
  int res=0;

 res = (a) > 0 ? floor(a+0.5) : -floor(a-0.5);
 return res;
}

/*---------------------------------------------------------------------------*/
/**
  @brief  selects a constant mean through a kap*sig clipping


  @param  val  :    input values
  @param  nval  :    number of input values
  @param  iter  :    number of iterations (min = 1)
  @param  akap  :    input AKAP * RMS


  output:
  @param  cons  :    output derived mean value
  @param  rms   :    output RMS of mean value
  @param  npts  : output number of points used to derive the mean value

  @return   status      :  0  O.K.
                        : -1  NVAL .LT. 2  

 Selects a constant mean through a kap*sig clipping:
  
 - 1.) calculate mean and rms
 - 2.) delete pixels beyond AKAP*RMS from mean
 - 3.) GO TO 1.)


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


static int Ckapsig( float *val, int nval, int iter, float akap, 
                          float *cons, float *rms, int *npts )

{

register int ii=0;
register int it=0;
register int nr=0;

int   nr_old=0;

float clip=0;
float dels=0;
float delv=0;
float mean=0;
float msq=0;
float sum=0;
float *vsq=NULL;

if ( nval < 2 ) return (-1);


/* initialize mean value */

mean = 0.0;
for (ii=0; ii<nval; ii++) mean += val[ii];
mean /= (float) (nval);
msq = mean * mean;


/* initialize RMS */

vsq = (float *) cpl_calloc( nval, sizeof( float ));

dels = 0.0;
for (ii=0; ii<nval; ii++)
    {
    vsq[ii] = val[ii] * val[ii];
    delv = MYMAX( 0.0, vsq[ii] + msq - (2.0 * mean * val[ii]));
    dels += delv;
    }

*rms = (float) sqrt( MYMAX( MINVAL, dels / (nval-1)));
clip = akap * (*rms);


/* iterate */

nr_old = 0;
for (it=0; it<iter; it++)
   {
   nr = 0;
   sum  = 0.0;
   dels = 0.0;

   for ( ii = 0; ii < nval; ii++ )
      {
      if ( fabs( val[ii] - mean ) < (double) clip )     
         {
         nr++;
         delv = MYMAX( 0.0, vsq[ii] + msq - 2.0 * mean * val[ii]);
         dels += delv;
         sum  += val[ii];
         }
      }

   if ( nr <= 2 || nr == nr_old ) goto end_of_it;


   /* define new rms and mean value */

   nr_old = nr;
   *rms = (float) sqrt( MYMAX( MINVAL, dels / (nr-1)));
   clip = akap * *rms;
   mean = sum / nr_old;
   msq  = mean * mean;
   }

end_of_it:
*cons = mean;                /* exit */
*npts = nr;

cpl_free(vsq);
return (0);
}


/*---------------------------------------------------------------------------*/
/**
  @brief  calculate the inverse of a matrix

  input:
  @param  (*matrix) :    input matrix to be inverted
  @param  nfree             :    input number of free parameters

  @return   error status 0 = ok
                         1 = determinant is zero

 The algorithm used is the Gauss-Jordan algorithm described 
 in Stoer, Numerische matematik, 1 Teil.
             
 */
/*---------------------------------------------------------------------------*/

static int MATINV( double (*matrix)[MAXPAR], int nfree )
{

register int ii=0;
register int jj=0;
register int kk=0;

int    evin=0;

int    per[MAXPAR];
double even=0.;
double mjk=0.;

double hv[MAXPAR];


for ( ii = 0; ii < nfree; ii++ ) per[ii] = ii;     /* set permutation array */

for ( jj = 0; jj < nfree; jj++ )                   /* in j-th column, ... */
   {
   double rowmax = fabs( matrix[jj][jj] );             /* determine row with ... */
   int row = jj;                                    /* largest element. */
   for ( ii = jj + 1; ii < nfree; ii++ )
      {
      if ( fabs( matrix[ii][jj] ) > rowmax )
         {
         rowmax = fabs( matrix[ii][jj] );
         row = ii;
         }
      }

   if (fabs(matrix[row][jj]) < SMALL)             /* determinant is zero! */
      return (1);

   if ( row > jj )                           /* if largest element not ...*/
      {
      for ( kk = 0; kk < nfree; kk++ )     /* on diagonal, then ... */
         {
         even = matrix[jj][kk];            /* permutate rows. */
         matrix[jj][kk] = matrix[row][kk];
         matrix[row][kk] = even;
         }
      evin = per[jj];                      /* keep track of permutation */
      per[jj] = per[row];
      per[row] = evin;
      }

   even = 1.0 / matrix[jj][jj];              /* modify column */
   for (ii=0; ii<nfree; ii++) 
      matrix[ii][jj] *= even;
   matrix[jj][jj] = even;
   for (kk=0; kk<jj; kk++)
      { 
      mjk = matrix[jj][kk];
      for ( ii = 0; ii < jj; ii++ ) 
         matrix[ii][kk] -= matrix[ii][jj] * mjk;
      for ( ii = jj + 1; ii < nfree; ii++ )
         matrix[ii][kk] -= matrix[ii][jj] * mjk;
      matrix[jj][kk] = -even * mjk;
      }

   for ( kk = jj + 1; kk < nfree; kk++ )
      {
      mjk = matrix[jj][kk];
      for ( ii = 0; ii < jj; ii++ )
         matrix[ii][kk] -= matrix[ii][jj] * mjk;
      for ( ii = jj + 1; ii < nfree; ii++ )
         matrix[ii][kk] -= matrix[ii][jj] * mjk;
      matrix[jj][kk] = -even * mjk;
      }
   }

for ( ii = 0; ii < nfree; ii++ )                /* finally, repermute the ...*/
   { 
   for ( kk = 0; kk < nfree; kk++ )          /* columns. */
      {
      hv[per[kk]] = matrix[ii][kk];
      }
   for ( kk = 0; kk < nfree; kk++)
      {
      matrix[ii][kk] = hv[kk];
      }
   }

return 0;
}

/*---------------------------------------------------------------------------*/
/**
  @brief  returns complementary error function EFC( xx )

  @param  xx : input parameter of error function

  @return   complementary error function EFC( xx )
                    
  Factional error everywhere less than 1.2e-7
             
 */
/*---------------------------------------------------------------------------*/

static double ERFCC( double xx )

{

double z = fabs( xx );
double t = 1.0 / (1.0 + (0.5 * z));

/*
ans = t * exp( -z * z - 1.26551223 + t * ( 1.00002368 + t *
                      ( 0.37409196 + t * ( 0.09678418 + t * 
                      (-0.18628806 + t * ( 0.27886807 + t *
                      (-1.13520398 + t * ( 1.48851587 + t *
                      (-0.82215223 + t * 0.17087277 )))))))));


the original code above didn't work on Red Hat Linux 5.2
with CENTER/GAUSS where the main program is Fortran code
neither on Alpha nor on Intel PC (using f2c) ... 

however it works on SUSE Linux 6.xx on Intel (using g77)

therefore this work around which seemed to solve the problem, KB 000522  */

double zz =  -z * z - 1.26551223 + t * ( 1.00002368 + t *
                      ( 0.37409196 + t * ( 0.09678418 + t *
                      (-0.18628806 + t * ( 0.27886807 + t *
                      (-1.13520398 + t * ( 1.48851587 + t *
                      (-0.82215223 + t * 0.17087277 ))))))));

double zozo=0.;
if (zz < -500.0)
   zozo = 0.0;
else
   zozo = exp(zz);

double ans = t * zozo;


return  (xx >= 0.0 ? ans : 2.0 - ans);
}

/*---------------------------------------------------------------------------*/
/**
  @brief  

  @param  xx   : input data point of independent variable
  @param  gpar :    function parameters

  @return  
             
 */
/*---------------------------------------------------------------------------*/

static double GAUSFU( double xx, double *gpar )
{

double rc=0.;
double dd=0.;

static int    init = TRUE;
static double sqrt_2;
static double rc1;


if ( init )
   {
   sqrt_2  = sqrt( 2.0 );
   rc1 = sqrt(PI)/sqrt_2;
   init = FALSE;
   }

rc = 1.0 / (sqrt_2 * gpar[2]);
dd = xx - gpar[1];
dd = ERFCC(rc * (dd - 0.5)) - ERFCC(rc * (dd + 0.5));
return ( gpar[3] + rc1 * gpar[0] * gpar[2] * dd );
}

/*---------------------------------------------------------------------------*/
/**
  @brief  evaluates derivatives of function for least squares search 
          with shape of a gaussian distribution

  @param  xdat        : input data point of independent variable
  @param  gpar        :    input parameters of the gaussian distribution
  @param  deriv       :    output derivatives of function

  @return  
             
 */
/*---------------------------------------------------------------------------*/

static void GAUSDE( double xdat, double *gpar, double *deriv )
{
double temp=0.;
double tempp=0.;
double x1=0.;
double x2=0.;
double zz=0.;



static double sqrt_2;

register int jj=0;
static int    init = TRUE;


if ( init )
   {
   sqrt_2  = sqrt( 2.0 );
   init = FALSE;
   }

temp = sqrt_2 * gpar[2];
tempp = xdat - gpar[1];
x1 = (tempp - 0.5) / temp;
x2 = (tempp + 0.5) / temp;
zz = tempp / gpar[2] ;

if ( ((zz * zz) - 50.0) < 0.0 )
   { 
   deriv[0] = (GAUSFU( xdat, gpar ) - gpar[3]) / gpar[0];
   double zx = (-x1) * x1;
   double dv1=0.;
   if ( zx < -200.0)        /*  zx  always < 0 */
      dv1 = 0.0;        /*  e**(-200)  is = 0.0 ... */
   else
      dv1 = exp(zx);
   zx = (-x2) * x2;
   double dv2=0.;
   if ( zx < -200.0)
      dv2 = dv1;
   else
      dv2 = dv1 - exp(zx);
   deriv[1] = gpar[0] * dv2;

   /*    for (x1 * x1) > 400  we got floating point exceptions on DEC Alpha 
   deriv[1] = gpar[0] * (exp( -x1 * x1 ) - exp( -x2 * x2 ));
   */

   deriv[2] = deriv[1] * zz;
   }
else
   for (jj=0; jj<3; jj++) deriv[jj] = 0.0;
     
deriv[3] = 1.0;
}

/*---------------------------------------------------------------------------*/
/**
  @brief  evaluate reduced chi square for fit to data 
       
  @param data :    input data
  @param ndim  :    dimension of input data
  @param nfree :    number of degrees of freedom
  @param mode  :    determines method of weighting the least-squares fit: 
  - 0 = no weighting
  - 1 = with weighting
  @param dfit :    output array with the fit for data


  @return  sum( (y-yfit)**2 / sigma**2 ) / nfree
             
 */
/*---------------------------------------------------------------------------*/

static float FCHIS(double *data,int ndim,int nfree,int mode,double *dfit) {

register int ii=0;





if ( nfree > 0 )
   {
   double chisq = 0.0;

   for (ii=0; ii<ndim; ii++)
      {
	   double weight=0.;
      if ( mode < 0 )
         {
         if ( *data < 0 )
            weight = -1. / *data;
         else if ( *data == 0 )
            weight = 1.0;
         else
            weight = 1. / *data;
         }
      else
         weight = 1.0;

      double diff = (*data) - (*dfit);
      data++;  dfit++;
      chisq += weight * diff * diff;
      }
   return (chisq / nfree);
   }

else
   return 0.0;
}

/*---------------------------------------------------------------------------*/
/**
  @brief  least squares fit to a non-linear function  
       


  @param  xdat  : input array of data points ind. var. 
  @param  data  : input array of data points dep. var.
  @param  ndim  : input dimension of xdat and data
  
  @param  gpar  : in/out  function parameters:
                               - gpar[0] = data max
                               - gpar[1] = position of data max
                               - gpar[2] = width of the source
                               - gpar[3] = center of window

  @param lamda : in/out proportion of gradient search included
  @param dfit  : output array with the fit for data
  @param chisqr: output reduced chi square for fit
  @param sigma : the probable uncertainty (output)


  @return  0 (= ok), 1 (= no convertion)

  Note: set LAMDA to 0.001 at the beginning of the search 
             
 */

static int LSQFIT( double *xdat, double *data, int ndim,
                         double *gpar, float *lamda, double *dfit, 
                         double *chisqr, double *sigma )

{

register int icnt=0;
register int ii=0;
register int jj=0;
register int kk=0;

int    nfree=0;

double chisq1=0;
double b[MAXPAR], beta[MAXPAR], deriv[MAXPAR], 
               array[MAXPAR][MAXPAR], alpha[MAXPAR][MAXPAR];
                                                                               

nfree = ndim - MAXPAR;
*sigma = 0.0;
if ( nfree < 1 || fabs( (double) *gpar ) < SMALL ) return (1);


/* evaluate ALPHA and BETA matrices */

for (ii=0; ii<MAXPAR; ii++)
   {
   beta[ii] = 0.0;
   for (jj=0; jj<=ii; jj++) alpha[ii][jj] = 0.0;
   }

for (ii=0; ii<ndim; ii++)
    {
    GAUSDE( xdat[ii], gpar, deriv );          /* here we divide by gpar[1] */

    for (jj=0; jj<MAXPAR; jj++)
       {
       beta[jj] += (data[ii] - GAUSFU( xdat[ii], gpar )) * deriv[jj];
       for (kk=0; kk<=jj; kk++) 
          alpha[jj][kk] += deriv[jj] * deriv[kk];
       }
    }

for (ii=0; ii<MAXPAR; ii++)
   {
   for (jj=0; jj<=ii; jj++) 
      alpha[jj][ii] = alpha[ii][jj];
   }


/* invert matrix */

if ( *lamda < SMALL)
   {
   if (MATINV(alpha,MAXPAR) == 1) return (2);    /* determinant -> 0.0 */

   *sigma = MYMAX( 0.0, alpha[1][1] );
   }

else                                /* evaluate chi square at starting point */
   {
   for (ii=0; ii<ndim; ii++)
      dfit[ii] = GAUSFU( xdat[ii], gpar );

   chisq1 = FCHIS( data, ndim, nfree, 0, dfit );

   icnt = 0;            /* invert matrix */
loop:
   for ( jj = 0; jj < MAXPAR; jj++ )
      {
      for ( kk = 0; kk < MAXPAR; kk++ )
         {
         if (fabs( alpha[jj][jj] ) < 1.e-15 || fabs( alpha[kk][kk] ) < 1.e-15) 
            return 2;
         array[jj][kk] = alpha[jj][kk] / 
                                      sqrt( alpha[jj][jj] * alpha[kk][kk] ) ;
         }
      array[jj][jj] = 1.0 + *lamda;
      }

   (void) MATINV( array, MAXPAR );

   for ( jj = 0; jj < MAXPAR; jj++ )
      {
      b[jj] = gpar[jj] ;
      for ( kk = 0; kk < MAXPAR ; kk++ )
         {
         b[jj] += beta[kk] * array[jj][kk] / 
                                        sqrt( alpha[jj][jj] * alpha[kk][kk] );
         }
      }

/* if chi square increased, increase LAMDA and try again */

   for (ii=0; ii<ndim; ii++)
      dfit[ii] = GAUSFU( xdat[ii], b );
   
   *chisqr = FCHIS( data, ndim, nfree, 0, dfit );

   if ( chisq1 - *chisqr < 0.0 )
      {
      if (++icnt < 60)
         {
         *lamda *= 10;
         goto loop;
         }
      else
         return (2);
      }

   for (jj=0; jj<MAXPAR; jj++) gpar[jj] = b[jj];
   *lamda /= 10.0;
   }

return 0;
}


/*---------------------------------------------------------------------------*/
/**
  @brief  compute X-marginal vector KRX.  
       

  @param p_img   : input pointer to 2-D image
  @param npix    : input dimension of image
  @param image   : (input) contains in pixel units
                                in C notation (0 ...)
  @param lnew    : input Y lower/upper limits to the marginal
                                in C notation (0 ...)

  @param krx     : output X-marginal vector

  @return  nothing
 
  For the algorithm description see Stetson, P.B., 1979  Astron. J., 84  1149.
             
 */

static void Crhox( float *p_img, int *npix, int *image, 
                         int *lnew, double *krx )

{
register int nxdim=0;
register int ix=0;
register int iy=0;

int nrx=0;
int nry=0;
 





/*  original FORTRAN code:

      IXA = IMAP(1)        IMAP(1-4) => image[0-3]
      IXE = IMAP(2)
      IYA = IMAP(3)
      JYA = IYA + JY - 1    JY => lnew[0]
      JYE = IYA + LY - 1    LY => lnew[1]
      M = 1

         DO 200 J=IXA,IXE
            ISUM = 0.D0
            DO 100 K=JYA,JYE
               ISUM = ISUM + AIMG(J,K)
100         CONTINUE
            KRX(M) = ISUM
            M = M + 1
200      CONTINUE
*/


nrx = image[1] - image[0] + 1;
nry = lnew[1] - lnew[0] + 1;
nxdim = *npix;
p_img += nxdim * (image[2] + lnew[0]);

for (ix=0; ix<nrx; ix++)
   {
   double sum = 0.0;
   for (iy=0; iy<nry*nxdim; iy+=nxdim) sum += p_img[iy];
   p_img++;
   *krx++ = sum;
   }
}



/*---------------------------------------------------------------------------*/
/**
  @brief  compute Y-marginal vector KRY.  
       

  @param p_img   : input pointer to 2-D image
  @param npix    : input dimension of image
  @param image   : (input) contains in pixel units
                                in C notation (0 ...)
  @param lnew    : input Y lower/upper limits to the marginal
                                in C notation (0 ...)

  @param kry     : output X-marginal vector

  @return  nothing
 
  For the algorithm description see Stetson, P.B., 1979  Astron. J., 84  1149.
             
 */

static void Crhoy( float *p_img, int *npix, int *image, 
                         int *lnew, double *kry )

{
register int nxdim=0;
register int ix=0;
register int iy=0;

int nrx=0;
int nry=0;





/*  original FORTRAN code:

      IXA = IMAP(1)        IMAP(1-4) => image[0-3]
      IYA = IMAP(3)
      IYE = IMAP(4)
      JXA = IXA + JX - 1    JX => lnew[0]
      JXE = IXA + LX - 1    LX => lnew[1]

         M = 1
         DO 200 J=IYA,IYE
            ISUM = 0.D0
            DO 100 K=JXA,JXE
               ISUM = ISUM + AIMG(K,J)
100         CONTINUE
            KRY(M) = ISUM
            M = M + 1
200      CONTINUE

*/

nrx = lnew[1] - lnew[0] + 1;
nry = image[3] - image[2] + 1;
nxdim = *npix;
p_img += (nxdim * image[2]) + (image[0] + lnew[0]);

for (iy=0; iy<nry; iy++)
   {
   double sum = 0.0;
   for (ix=0; ix<nrx; ix++) sum += p_img[ix];
   p_img += nxdim;
   *kry++ = sum;
   }
}

/*---------------------------------------------------------------------------*/
/**
  @brief  search a star from a marginal distribution

  @param  marg :    input marginal data
  @param  ndim :    input dimension of MARG
  @param  ign  :    input number of points ignored at each end of KRX
                ( greater or equal 2)

  @param  lmin   : output low-side local minimum (in pixels)
  @param  lmax   : output high-side local minimum (in pixels)
  @param  s_cent : output output estimate of the object centre
  @param  s_width: output output estimate of the object width
             
  @return     
   - 0  if no other star is detected in the field
   - -1  if a brighter star is found on the low side
   - +1  if a brighter star is found on the high side

 
  For the algorithm description see Stetson, P.B., 1979  Astron. J., 84  1149.
             
 */

static int Cserch( double *marg, int ndim, int ign, 
                         int *lmin, int *lmax, float *s_cent, float *s_width )

{

register int ii=0;

int ql=0;
int ibgn=0;
int icrowd=0;
int iend=0;
int imax=0;
int imin=0;
int indx=0;

double dxk=0.;
double diff=0.;


double sum=0.;
double  *work=NULL;


ibgn = ign;
iend = ndim - ign -1;            /* ojo */

/* create workspace to store the derivative of the marginal data */

work = (double *) cpl_calloc( ndim , sizeof( double ));


/* find maximum and minimum derivative of MARG */

imin = imax = 0;
double drmn = 0., drmx = 0.0;
for (ii = ibgn; ii < iend; ii++ )
    {
    diff = marg[ii+1] - marg[ii-1];
    work[ii] = marg[ii+2] - marg[ii-2] + (2 * diff);
    if ( work[ii] >= drmx )     
       {
       drmx = work[ii];
       imax = ii;
       }
    if (
       work[ii] <= drmn )     
       { drmn = work[ii];
       imin = ii;
       }
    }


/* crowded ? */

icrowd = 0;
if (imin <= imax ) /* bright source to the left, compute right a new minima */
   {
   if ( ndim - imax > imin )     
      {
      icrowd = -1;
      drmn = drmx;
      for ( ii = imax+1; ii < iend; ii++ )
         {
         if ( work[ii] < drmn )
            {
            drmn = work[ii];
            imin = ii;
            }
         }
      }
   else           /* bright source to the right, compute left a new maxima */
      {
      icrowd = 1;
      drmx = drmn;
      for ( ii = ibgn; ii < imin; ii++ )
         {
         if ( work[ii] >= drmx )
            { 
            drmx = work[ii];
            imax = ii;
            }
         }
      }
   }


/* compute estimates of image centre and width */

*s_cent  = ((float)(imax + imin)) * 0.5;
*s_width = imin - imax;

sum = 0.0;
for ( ii = imax; ii <= imin; ii++ ) sum += work[ii];

diff = drmx - drmn;
if ( fabs(diff) > SMALL)
   {
   dxk = sum * *s_width / ( (*s_width+1.0)*diff );
   *s_cent += dxk;
   }
*s_width /= 2;
indx = CGN_NINT(*s_cent);
if (indx < 0)
   {
   *s_cent = 0.0;
   indx = 0;
   }
else if (indx >= ndim)
   {
   *s_cent = (float)(ndim-1);
   indx = ndim-1;
   }


/* find low- (left-) side local minimum */

ql = indx - 2;
*lmin = 0;
if (ql < 2) goto next_step;

low_loop:
ql --;
if (ql <= 0 ) goto next_step;
if (ql == 1) goto lo5;
if (ql == 2) goto lo4;
if (ql == 3) goto lo3;

if (marg[ql] > marg[ql-4]) goto low_loop;
lo3:
if (marg[ql] > marg[ql-3]) goto low_loop;
lo4:
if (marg[ql] > marg[ql-2]) goto low_loop;
lo5:
if (marg[ql] > marg[ql-1]) goto low_loop;

*lmin = ql + 1;


/* find high- (right-) side local minimum */

next_step:
ql = indx + 2;
*lmax = ndim - 1;
ii = ndim - ql;
if (ii < 3) goto end_of_it;

hi_loop:
ql ++ ;
ii = ndim - ql;
if (ii == 1 ) goto end_of_it;
if (ii == 2) goto hi5;
if (ii == 3) goto hi4;
if (ii == 4) goto hi3;

if (marg[ql] > marg[ql+4]) goto hi_loop;
hi3:
if (marg[ql] > marg[ql+3]) goto hi_loop;
hi4:
if (marg[ql] > marg[ql+2]) goto hi_loop;
hi5:
if (marg[ql] > marg[ql+1]) goto hi_loop;
*lmax = ql - 1;

end_of_it:
(void) cpl_free( (char *) work );
return (icrowd);

}

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

  @brief  Routines used to do Gaussian fit to a line

  @param  meth  :   input method of centering: MOM (moment centering)
                                      GAU (gaussian centering)
  @param  p_img : input pointer to image
  @param  npix  : input number of pixels in image
  @param  image : input image size in pixel coordinates (F77 indexing)

  @param  xypos :   output center position (x,y)  (C indexing)
  @param  xyerr :   output error estimate of xypos (x,y)
  @param  xysig :   output width of source (x,y)
  @param  xyval :   output central value

  @return   
  - 0 if everything is ok,
  - 1 if crowded, or weak source
  - 2 if no pos. source found
  - 3 if iteration failed

   This module contains the following statistic functions:
             - for the moment centering:   Ckapsig
             - for the gaussian centering: 
                - Crhox  
                - Crhoy  
                - Cserch 
                       - LSQFIT 
                         - <- GAUSDE <- GAUSFU <- ERFCC
                         - <- MATINV
                         - <- GAUSFU <- ERFCC
                         - <- FCHIS

  Performs a Gaussian fit of the intensity distribution obtained by collapsing
  along X or Y an image within a box.

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

/* ------------------------------------------*/
/* here starts the code of the main function */
/* ------------------------------------------*/


int uves_physmod_cstacen(char meth, float* p_img, int* npix, int* image, 
            float* xypos, float* xyerr, float* xysig, float* xyval )

{

register int it=0;
register int ix=0;
register int iy=0;

int bgnr=0;
int indx=0;
int indy=0;
int istat=0;
int nrx=0;
int nry=0;
int nval=0;
int ifram[4];

float bgval=0.;
float clip=0.;
float rms=0.;
float xmom=0.;
float ymom=0.;
float source=0.;
float sumi=0.;
float xold=0.;
float yold=0.;

float *p_bgn=NULL;
float *p_edge=NULL;


istat = 0;                        /* initialize */
p_bgn = p_img;
for (ix=0; ix<4; ix++)
   ifram[ix] = image[ix] + 1;            /* 1 ---> ndim  */
nrx = ifram[1] - ifram[0] + 1;
nry = ifram[3] - ifram[2] + 1;
      
xypos[0] = (ifram[0] + ifram[1]) * 0.5;             /* init to center pixel */
xypos[1] = (ifram[2] + ifram[3]) * 0.5;
xyerr[0] = xyerr[1] = 0.0;
xysig[0] = xysig[1] = 0.0;
*xyval = 0.0;

/* MOMENT centering */

if ( meth != 'G' && meth != 'g' )
   {
   int kk, istr, iend;

   xold = yold = -1.0;        /* find bgval and rms from edge pixels */

   p_img += (ifram[0] - 1) + (npix[0] * (ifram[2] - 1));

   /* collect edge pixels */

   if (nry > 1)
      {
      nval = (2 * nrx) + (2 * (nry-2));
      p_edge = (float *) cpl_calloc( nval , sizeof( float ));

      for (ix=0; ix<nrx;ix++)
         *p_edge++ = p_img[ix];

      p_img += *npix;
      for (iy=0; iy<(nry-2); iy++)
         {
         *p_edge++ = p_img[0];
         *p_edge++ = p_img[nrx - 1];
         p_img += npix[0];
         }
      for (ix=0; ix<nrx; ix++) *p_edge++ = p_img[ix];
      }
   else
      {
      nval = nrx;
      p_edge = (float *) cpl_calloc( nval , sizeof( float ));

      for (ix=0; ix<nrx;ix++)
         *p_edge++ = p_img[ix];
      }

   p_img = p_bgn;

   p_edge -= nval;
   (void) Ckapsig( p_edge, nval, 5, 2.0, &bgval, &rms, &bgnr );
   (void) cpl_free( (char *) p_edge );


   /* calculate moment for pixel values > 3 * RMS above BGVAL */

   clip = 3.0 * rms;

   for (it=0; it<MMXITER; it++)                      /* iteration loop */
      {
      sumi = xmom = ymom = 0.0;
      p_img += ifram[0] - 1 + (npix[0] * (ifram[2] - 1));
      for (iy=0; iy<nry; iy++)
         {
         for (ix=0; ix<nrx; ix++) 
            {
            if ( (source = p_img[ix] - bgval) > clip )
               {
               sumi += source;
               xmom += source * (ifram[0] + ix);
               ymom += source * (ifram[2] + iy);
               }
            }
         p_img += npix[0];
         }
      p_img = p_bgn;            /* reset to start of array */

      if ((nrx < 3) || (nry < 3))
         {
         xysig[0] = nrx;
         xysig[1] = nry;
         istat = 1;
         if ( sumi > 0.0 )
            {
            xypos[0] = xmom / sumi;
            xypos[1] = ymom / sumi;
            }
         else
            {
            istat = 2;
            xypos[0] = (ifram[0] + ifram[1]) * 0.5;
            xypos[1] = (ifram[2] + ifram[3]) * 0.5;
            }
         indx = CGN_NINT(xypos[0]-1);
         indy = CGN_NINT(xypos[1]-1);
         *xyval = p_img[indx + ((*npix) * indy)];
         goto end_of_iter;        /* EXIT iteration loop */
         }

      if (sumi > 0.0)                  /* only positive sources */
         {
         xypos[0] = xmom / sumi;
         xypos[1] = ymom / sumi;
         xysig[0] = nrx;
         xysig[1] = nry;

         if ( xold == xypos[0] && yold == xypos[1] )
            {
            int    nr = 0;
            double xdif, ydif, xrms, yrms;

            xrms = yrms = sumi = 0.0;
            p_img += ifram[0] - 1 + (npix[0] * (ifram[2] - 1));
            for (iy=0; iy<nry; iy++ )
               {
               for (ix=0; ix<nrx; ix++) 
                  {
                  if ( (source = p_img[ix] - bgval) > clip )
                     {
                     xdif = (ifram[0] + ix) - xypos[0];
                     ydif = (ifram[2] + iy) - xypos[1];
                     xrms += fabs( source * xdif *xdif );
                     yrms += fabs( source * ydif *ydif );
                     sumi += source;
                     nr++;
                     }
                  }
               p_img += npix[0];
               }
            p_img = p_bgn;

            indx = CGN_NINT(xypos[0]-1) + (npix[0] * CGN_NINT(xypos[1]-1));
            *xyval = p_img[indx];
            xysig[0] = (float) sqrt(xrms /(sumi+ *xyval - bgval));
            xysig[1] = (float) sqrt(yrms /(sumi+ *xyval - bgval));
            xyerr[0] = (float) (xysig[0] / sqrt( (double) (nr - 1)));
            xyerr[1] = (float) (xysig[1] / sqrt( (double) (nr - 1)));
            goto end_of_iter;            /* succesful return */
            }


         xold = xypos[0];
         yold = xypos[1]; 
         }
      else
         {
         istat = 2;
         xypos[0] = (ifram[0] + ifram[1]) * 0.5;
         xypos[1] = (ifram[2] + ifram[3]) * 0.5;
         indx = CGN_NINT(xypos[0]-1);
         indy = CGN_NINT(xypos[1]-1);
         *xyval = p_img[indx + ((*npix) * indy)];
         goto end_of_iter;              /* EXIT iteration loop */
         }


      /* crowded or weak source conditions */

      indx = CGN_NINT(xypos[0]-1) + (npix[0] * CGN_NINT(xypos[1]-1));
      if ( (*xyval = p_img[indx] - bgval) <= clip )
         {
         xysig[0] = xysig[1] = 0.0;
         istat = 1;
         goto end_of_iter;              /* EXIT iteration loop */
         }


      /* find extent of source i.e. delete spikes, etc.  */

      ix = CGN_NINT( xypos[0] );        /* ix, iy = 1,2,...   */
      iy = CGN_NINT( xypos[1] );
      kk = npix[0] * (iy - 1);
      istr = ifram[0];
      source = p_img[ix-1 + kk] - bgval;
      while ( source > clip && ix >= istr )
         {
         ifram[0] = ix;
         source = p_img[ix-1 + kk] - bgval;
         ix --;
         }

      ix = CGN_NINT( xypos[0] );
      iend = ifram[1];
      source = p_img[ix-1 + kk] - bgval;
      while ( source > clip && ix <= iend )
         {
         ifram[1] = ix;
         source = p_img[ix-1 + kk] -bgval;
         ix ++;
         } 

      ix = CGN_NINT( xypos[0] );
      istr = ifram[2];
      source = p_img[ix-1 + kk] - bgval;
      while ( source > clip && iy >= istr )
         {
         ifram[2] = iy;
         source = p_img[ix-1 + (*npix *(iy-1))] -bgval;
         iy --;
         }

      iy = CGN_NINT( xypos[1] );
      iend = ifram[3];
      source = p_img[ix-1 + kk] - bgval;
      while ( source > clip && iy <= iend )
         {
         ifram[3] = iy;
         source = p_img[ix-1 + (*npix *(iy-1))] -bgval;
         iy++;
         }
      nrx = ifram[1] - ifram[0] + 1;
      nry = ifram[3] - ifram[2] + 1;
      }

   istat = 3;                /* iteration failed */

end_of_iter:
   xypos[0] --;
   xypos[1] --;
   }


/* GAUSSIAN centering */

else
   {
   register int ii;
   int    found, ierr, xlim[2], ylim[2], lnew[2];
   float  lamda, xcent, ycent, xwidth, ywidth;
   double chisqr, oldchi, sigma, *krx, *kry, *gfit, *xpos, *yfit, 
          gpar[MAXPAR];


   /* construct two marginal distibutions (in pixel coordinates!) */

   lnew[0] = (nry / 4);        /* in C notation, from 0 ...  */
   lnew[1] = nry - (nry / 4) - 1;


/* Take care of 1-dim case */

   if (nry == 1)     
      {
      krx = (double *) cpl_calloc(nrx , sizeof(double));
      Crhox(p_img,npix,image,lnew,krx); 
      ierr = Cserch(krx,nrx,IGNORE,xlim,xlim+1,&xcent,&xwidth);

      /* store the data of the fit */

      nval = xlim[1] - xlim[0] + 1;
      xpos = (double *) cpl_calloc( nval , sizeof( double ));
      yfit = (double *) cpl_calloc( nval , sizeof( double ));
      gfit = (double *) cpl_calloc( nval , sizeof( double ));
      for (ii=0; ii<nval; ii++)
         {
         xpos[ii] = xlim[0] + ii;
         yfit[ii] = krx[xlim[0] + ii];
          }

      /* set parameters for LSQFIT (old FITINTE) */

      lamda = 0.001;
      chisqr = GCHIMAX;
      gpar[0] = krx[CGN_NINT(xcent)];
      gpar[1] = xcent;
      gpar[2] = xwidth;
      gpar[3] = (krx[xlim[0]] + krx[xlim[1]]) / 2;
      (void) cpl_free( (char *) krx );

      it = 0;
      found = FALSE;
      while ( ! found && it++ < GMXITER )
         {
         oldchi = chisqr;
         ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma);
         if ( ierr != 0 ) 
            {
            found = NOCONV;
            istat = 3;
            }
         else if ( (oldchi - chisqr)/ chisqr < GCHIFND ) 
            found = TRUE;
         }

      /* Is the source still in the image and has it a resonable shape? */

      lamda = 0.0;
      ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma);
      if ( ierr != 0 )
         {
         found = NOCONV;
         istat = 3;
         }
      else
         {
         sumi = (float)(gpar[1] + image[0]);
         indx = CGN_NINT(sumi);
         if ( indx < 0 || indx >= *npix )
            {
            found = OUTSIDE;
            istat = 2;
            }
         }
      (void) cpl_free( (char *) xpos );
      (void) cpl_free( (char *) yfit );
      (void) cpl_free( (char *) gfit );

      if ( found == TRUE )
         {
         xypos[0] = sumi;
         xypos[1] = 0;
         xysig[0] = (float) gpar[2];
         xyerr[0] = (float) sqrt( sigma * chisqr );
         indx = CGN_NINT( xypos[0]);
         *xyval = p_img[indx];
         }
      }


/* Take care of 2-dim case */

   else
      {
      krx = (double *) cpl_calloc( nrx , sizeof( double ));
      kry = (double *) cpl_calloc( nry , sizeof( double ));

      /* Compute and search X-marginal & Y-marginal */

      Crhox( p_img, npix, image, lnew, krx ); 
      ierr = Cserch( krx, nrx, IGNORE, xlim, xlim+1, &xcent, &xwidth );
      lnew[0] = MYMAX( xlim[0], CGN_NINT(xcent - (2 * xwidth)));
      lnew[1] = MYMIN( xlim[1], CGN_NINT(xcent + (2 * xwidth)));

      Crhoy( p_img, npix, image, lnew, kry ); 
      ierr = Cserch( kry, nry, IGNORE, ylim, ylim+1, &ycent, &ywidth );
      lnew[0] = MYMAX( ylim[0], CGN_NINT(ycent - (2 * ywidth)));
      lnew[1] = MYMIN( ylim[1], CGN_NINT(ycent + (2 * ywidth)));

      Crhox( p_img, npix, image, lnew, krx ); 
      ierr = Cserch( krx, nrx, IGNORE, xlim, xlim+1, &xcent, &xwidth );
      lnew[0] = MYMAX( xlim[0], CGN_NINT(xcent - (2 * xwidth)));
      lnew[1] = MYMIN( xlim[1], CGN_NINT(xcent + (2 * xwidth)));

      Crhoy( p_img, npix, image, lnew, kry ); 
      ierr = Cserch( kry, nry, IGNORE, ylim, ylim+1, &ycent, &ywidth );

      /* fit a gaussian to the source along the X-axis */

      nval = xlim[1] - xlim[0] + 1;
      xpos = (double *) cpl_calloc( nval , sizeof( double ));
      yfit = (double *) cpl_calloc( nval , sizeof( double ));
      gfit = (double *) cpl_calloc( nval , sizeof( double ));
      for (ii=0; ii<nval; ii++)
         {
         xpos[ii] = xlim[0] + ii;
         yfit[ii] = krx[xlim[0] + ii];
        }

      /* set parameters for LSQFIT */

      lamda = 0.001;
      chisqr = GCHIMAX;
      gpar[0] = krx[CGN_NINT( xcent )];
      gpar[1] = xcent;
      gpar[2] = xwidth;
      gpar[3] = (krx[xlim[0]] + krx[xlim[1]]) / 2;
      (void) cpl_free( ( char *) krx );

      it = 0;
      found = FALSE;
      while ( ! found && it++ < GMXITER )
         { 
         oldchi = chisqr;
         ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma);
         if ( ierr != 0 || gpar[2] <= 0.0 ) 
            {
            found = NOCONV;
            istat = 3;
            }
         else if ( (oldchi - chisqr)/ chisqr < GCHIFND ) 
            found = TRUE;
         }

      /* Is the source still in the image and has it a resonable shape? */

      lamda = 0.0;
      ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma);
      if ( ierr != 0 )
         {
         found = NOCONV;
         istat = 3;
         }
      else
         {
         sumi = (float)(gpar[1] + image[0]);
         indx = CGN_NINT(sumi);
         if ( indx < 0 || indx >= *npix ) 
            {
            found = OUTSIDE; 
            istat = 2;
            }
         }

      (void) cpl_free( (char *) xpos );
      (void) cpl_free( (char *) yfit );
      (void) cpl_free( (char *) gfit );

      if ( found == TRUE )
         {
         xypos[0] = sumi;
         xysig[0] = (float) gpar[2];
         xyerr[0] = (float) sqrt( sigma * chisqr );

         /* x-dir o.k. - now fit a gaussian to the source along the Y-axis */

         nval = ylim[1] - ylim[0] + 1;
         xpos = (double *) cpl_calloc( nval , sizeof( double ));
         yfit = (double *) cpl_calloc( nval , sizeof( double ));
         gfit = (double *) cpl_calloc( nval , sizeof( double ));

         for (ii=0; ii<nval; ii++)
            {
            xpos[ii] = ylim[0] + ii;
            yfit[ii] = kry[ylim[0] + ii]; 
            }

         /* set parameters for LSQFIT */

         lamda = 0.001;
         chisqr = GCHIMAX;
         gpar[0] = kry[CGN_NINT( ycent )];     
         gpar[1] = ycent;
         gpar[2] = ywidth;
         gpar[3] = (kry[ylim[0]] + kry[ylim[1]]) / 2;

         it = 0;
         found = FALSE;
         while ( ! found && it++ < GMXITER )
            {
            oldchi = chisqr;
            ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma );
            if ( ierr != 0 || gpar[2] <= 0.0 ) 
               {
               found = NOCONV;
               istat = 3;
               }
            else if ( (oldchi - chisqr)/ chisqr < GCHIFND ) 
               found = TRUE;
            }

         /* Is the source still in the image and has it a resonable shape? */

         lamda = 0.0;
         ierr = LSQFIT(xpos,yfit,nval,gpar,&lamda,gfit,&chisqr,&sigma);
         if ( ierr != 0 ) 
            {
            found = NOCONV;
            istat = 3;
            }
         else
            {
            indx = CGN_NINT(xypos[0]);
            sumi = (float) (gpar[1] + image[2]);
            indy = CGN_NINT(sumi);
            if ( indy < 0 || indy >= npix[1] ) 
               {
               found = OUTSIDE; 
               istat = 2;
               }
            else
               indx += (*npix) * indy;
            }
         (void) cpl_free( (char *) xpos );
         (void) cpl_free( (char *) yfit );
         (void) cpl_free( (char *) gfit );

         if ( found == TRUE )
            {
            xypos[1] = sumi;
            xysig[1] = (float) gpar[2];
            xyerr[1] = (float) sqrt( sigma * chisqr );
            *xyval = p_img[indx];
            }
         }
      (void) cpl_free( ( char *) kry );
      }
   }

return istat;
}
/**@}*/

