#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_statistics.h>

#include "expfit.c"

/* number of data points to fit */
#define N 240
#define P 9

static double gaussian_Y[N];
static double gaussian_X[N];

int load(char *fname, double *x, double *y, int npoints) {
	FILE *fp;
	int i;

	/* Save data for simple plotting with gnuplot */
	fp = fopen(fname, "r");
	if (! fp) {
		perror("fopen(): ");
		return -1;
	}

	i = 0;
	while(!feof(fp)) {
		if (i >= npoints) {
			break;
		}
		fscanf(fp, "%lf\t%lf\n", &x[i], &y[i]);
		i++;
	}
	fclose(fp);

	printf("read %d / %d points\n", i, npoints);
	return 0;
}

int
main (void)
{
  const gsl_multifit_fdfsolver_type *T = gsl_multifit_fdfsolver_lmsder;
  gsl_multifit_fdfsolver *s;
  int status, info;
  size_t i;
  const size_t n = N;
  const size_t p = P;

  gsl_matrix *J = gsl_matrix_alloc(n, p);
  gsl_matrix *covar = gsl_matrix_alloc (p, p);
  struct data d;
  gsl_multifit_function_fdf f;
  double x_init[P] = { 870.0, -2.5, 0.1, 5530.0, 0.2, 0.1, 2554.0, 1.3, 0.1 };
  gsl_vector_view x = gsl_vector_view_array (x_init, p);
  gsl_vector *res_f;
  double chi, chi0;

  const double xtol = 1e-6;
  const double gtol = 1e-6;
  const double ftol = 0.0;

  load("./800W5-95kv_ROI.txt", gaussian_X, gaussian_Y, N);

  /* subtract mean from gaussian_X */
  {
    double mean = gsl_stats_mean(gaussian_X, 1, N);
    for (i = 0; i < N; ++i)
      gaussian_X[i] -= mean;
  }

  d.t = gaussian_X;
  d.y = gaussian_Y;
  d.n = N;

  f.f = &gaussian_f;
  f.df = NULL;

  f.n = n;
  f.p = p;
  f.params = &d;

  s = gsl_multifit_fdfsolver_alloc (T, n, p);

  /* initialize solver with starting point */
  gsl_multifit_fdfsolver_set (s, &f, &x.vector);

  /* compute initial residual norm */
  res_f = gsl_multifit_fdfsolver_residual(s);
  chi0 = gsl_blas_dnrm2(res_f);

  /* solve the system with a maximum of 2000 iterations */
  status = gsl_multifit_fdfsolver_driver(s, 2000, xtol, gtol, ftol, &info);

  gsl_multifit_fdfsolver_jac(s, J);
  gsl_multifit_covar (J, 0.0, covar);

  /* compute final residual norm */
  chi = gsl_blas_dnrm2(res_f);

#define FIT(i) gsl_vector_get(s->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))

  fprintf(stderr, "summary from method '%s'\n",
          gsl_multifit_fdfsolver_name(s));
  fprintf(stderr, "number of iterations: %zu\n",
          gsl_multifit_fdfsolver_niter(s));
  fprintf(stderr, "function evaluations: %zu\n", f.nevalf);
  fprintf(stderr, "Jacobian evaluations: %zu\n", f.nevaldf);
  fprintf(stderr, "reason for stopping: %s\n",
          (info == 1) ? "small step size" : "small gradient");
  fprintf(stderr, "initial |f(x)| = %g\n", chi0);
  fprintf(stderr, "final   |f(x)| = %g\n", chi);

  { 
    double dof = n - p;
    double c = GSL_MAX_DBL(1, chi / sqrt(dof)); 

    fprintf(stderr, "chisq/dof = %g\n",  pow(chi, 2.0) / dof);

    for (i = 0; i < p; i +=3) {
		fprintf (stderr, "Amp      = %.5f +/- %.5f\n", FIT(i), c*ERR(i));
		fprintf (stderr, "mu       = %.5f +/- %.5f\n", FIT(i+1), c*ERR(i+1));
		fprintf (stderr, "sigma    = %.5f +/- %.5f\n", FIT(i+2), c*ERR(i+2));
    }
  }

  fprintf (stderr, "status = %s\n", gsl_strerror (status));

  /* print out data and fitted model */
  for (i = 0; i < N; ++i)
    {
      printf("%f %f %f\n",
             gaussian_X[i],
             gaussian_Y[i],
             gsl_vector_get(res_f, i) + gaussian_Y[i]);
    }

  gsl_multifit_fdfsolver_free (s);
  gsl_matrix_free (covar);
  gsl_matrix_free (J);
  return 0;
}