/* GAUSS FIT

*/


#include <mpfit.h>
#include <fitnol.h>
#include <math.h>
#include <stdlib.h>

extern int non_lfit();



void fgauss_c(x,a,y,dyda)
float x,a[5],*y,dyda[5];
{
  double fac,ex,arg;

  *y=0.0;
  arg=(x-a[2])/a[3];
  ex=exp(-arg*arg);
  fac=a[1]*ex*2.0*arg;
  *y = a[1]*ex + a[4];
  dyda[1]=ex;
  dyda[2]=fac/a[3];
  dyda[3]=fac*arg/a[3];
  dyda[4]=1.0;
}

static void fgauss_cd(double x, double * a, double * y, double * dyda)
{
    double fac,ex,arg;

    *y=0.0;
    arg=(x-a[1])/a[2];
    ex=exp(-arg*arg);
    fac=a[0]*ex*2.0*arg;
    *y = a[0]*ex + a[3];
    dyda[0]=ex;
    dyda[1]=fac/a[2];
    dyda[2]=fac*arg/a[2];
    dyda[3]=1.0;
}


typedef struct {
    double * x;
    double * e;
    double * y;
} priv_data_e;


/* fit with error */
int fitgauss_e(int ndata, int npar, double *p, double *deviates,
               double **derivs, priv_data_e *d)
{
    int i;
    double * x = d->x;
    double * y = d->y;
    double * dyda = malloc(npar * sizeof(*dyda));
    for (i=0; i < ndata; i++) {
        double v;
        fgauss_cd(x[i], p, &v, dyda);
        deviates[i] = y[i] - v;
        if (derivs) {
            int j;
            for (j=0; j<npar; j++) {
                if (derivs[j]) {
                    derivs[j][i] = -dyda[j] / d->e[i];
                }
            }
        }
        deviates[i] /= d->e[i];
    }

    free(dyda);
    return 0;
}

int fit_gauss_float(float *x, float * y, float *sig, int n, float *a, int nfp)
{
    int s, i;
    priv_data_e pdata;
    double * ad = malloc(nfp * sizeof(*ad));
    mp_par * pars = calloc(nfp, sizeof(*pars));
    pdata.x = malloc(n * sizeof(double));
    pdata.e = malloc(n * sizeof(double));
    pdata.y = malloc(n * sizeof(double));
    for (i = 0; i < n; i++) {
        pdata.x[i] = x[i + 1];
        pdata.e[i] = sig[i + 1];
        pdata.y[i] = y[i + 1];
    }
    for (i = 0; i < nfp; i++) {
        ad[i] = a[i + 1];
    }
    for (i = 0; i < nfp; i++) {
        //pars[i].deriv_debug = 1;
        pars[i].side = 3;
    }

    s = mpfit((mp_func)&fitgauss_e, n, nfp,
              ad, pars, NULL,
              &pdata, NULL);
    free(pdata.x);
    free(pdata.e);
    free(pdata.y);
    for (i = 0; i < nfp; i++) {
        a[i + 1] = ad[i];
    }
    free(ad);
    free(pars);
    return s > 0 ? 1 : -1;
}


void gauss_fit(y,sig,ndata,param,npar,istat)
float *y, *sig, *param;
int   ndata, npar, *istat;
{
    float * x = malloc((ndata + 1) * sizeof(*x));
    int i;
    for (i = 1; i <= ndata; i++) {
        x[i] = i;
    }
    *istat = fit_gauss_float(x, y, sig, ndata, param, npar);
    free(x);
}


/* necessary for FORTRAN call in MIDAS */
/* CALL GAUSS_FIT(...) */
/* esoext.exe will interpret this call as GAUSST !! */
int gausst_(y,sig,ndata,param,npar,istat)
float *y, *sig, *param;
int   *ndata, *npar, *istat;
{
  gauss_fit(y-1,sig-1,*ndata,param-1,*npar,istat);
  return 0;
}


int gausst(y,sig,ndata,param,npar,istat)
float *y, *sig, *param;
int   *ndata, *npar, *istat;
{
  gauss_fit(y-1,sig-1,*ndata,param-1,*npar,istat);
  return 0;
}


