/***********
function f(x) = ax^2 + bx +c
three parameters: a, b, c
7 data points: (1,1) (2,2) (3,1) (4,5) (5,10) (6,12) (7,20)

so p=3 (a, b, c)
n=7
df/da = 2ax
df/db = b
df/dc = 1
************/

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

size_t n = 7;
size_t p = 3;
double xdata[7] = {1.0,2.0,3.0,4.0,5.0,6.0,7.0};
double ydata[7] = {1.12,4.13,8.94,15.75,24.86,36.27,49.28};
double edata[7] = {0.5,0.2,0.3,0.4,0.5,0.6,0.7};

int myf (const gsl_vector *x, void *data, gsl_vector *f) {
  size_t i;
  double a = gsl_vector_get(x, 0); /* get the parameters from the input
				      vector */
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  double func, xthis;
  printf("myf params: %f %f %f\n", a, b, c);

  for (i = 0; i < n; i++) {
    xthis = xdata[i];
    func = a*xthis*xthis + b*xthis + c;
    printf("function %d %f\n", i, (ydata[i] - func)/edata[i]);
    gsl_vector_set(f, i, func - ydata[i]);
  }
  return GSL_EDOM;
  /*return GSL_SUCCESS;*/
}

int mydf (const gsl_vector *x, void *data, gsl_matrix *J) {
  size_t i;
  double a = gsl_vector_get(x, 0); /* get the parameters from the input
				      vector */
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  double func, xthis, ethis;
  double dfda, dfdb, dfdc;
  printf("mydf params: %f %f %f\n", a, b, c);
  /* Jacobian matrix J(i,j) = dfi / dxj */
  for (i = 0; i < n; i++) {
    xthis = xdata[i];
    ethis = edata[i];
    dfda = xthis*xthis/ethis;
    dfdb = xthis/ethis;
    dfdc = 1.0/ethis;
    printf("derivs %d %f %f %f\n", i, dfda, dfdb, dfdc);
    gsl_matrix_set (J, i, 0, dfda); 
    gsl_matrix_set (J, i, 1, dfdb);
    gsl_matrix_set (J, i, 2, dfdc);
  }
  return GSL_SUCCESS;
}

int myfdf (const gsl_vector *x, void *data, gsl_vector *f, gsl_matrix *J) {
  myf (x, data, f);
  mydf (x, data, J);
  return GSL_SUCCESS;
}

void print_state (size_t iter, gsl_multifit_fdfsolver * s)
{
  printf ("iter: %3u x = % 15.8f % 15.8f % 15.8f "
	  "|f(x)| = %g\n",
	  iter,
	  gsl_vector_get (s->x, 0), 
	  gsl_vector_get (s->x, 1),
	  gsl_vector_get (s->x, 2), 
	  gsl_blas_dnrm2 (s->f));
}

int main (void) {

  const gsl_multifit_fdfsolver_type *T; /* pointer to solver type */
  gsl_multifit_fdfsolver *s; /* pointer to solver */
  gsl_multifit_function_fdf f; /* function structure */
  double x_init[3] = { 0.0, 0.0, 0.0 };
  gsl_vector_view x = gsl_vector_view_array(x_init, p);
  unsigned int iter; /* fit iteration */
  int status; /* status from fit */
  gsl_matrix *covar = gsl_matrix_alloc (p, p); /* covariance matrix */

  f.f = &myf;
  f.df = &mydf;
  f.fdf = &myfdf;
  f.n = n;
  f.p = p;
  f.params = NULL; /* pointer to something that gets the data, data is global
		      here so it is not needed */
  T = gsl_multifit_fdfsolver_lmsder; /* choose the solver */
  s = gsl_multifit_fdfsolver_alloc (T, n, p); /* allocate work space,
						 get solver pointer */
  gsl_multifit_fdfsolver_set (s, &f, &x.vector); /* initialize the solver */

  iter = 0;
  print_state(iter, s);
  do {
    iter++;
    printf("before solver iterate\n");
    print_state(iter, s);
    status = gsl_multifit_fdfsolver_iterate(s); /* go to next point */
    printf("after solver iterate, status = %d, %s\n", status, gsl_strerror (status));
    print_state(iter, s);
    if (status) continue;
    status = gsl_multifit_test_delta(s->dx, s->x,
				     1e-4, 1e-4);
    printf("after test delta, status = %d, %s\n", status, gsl_strerror (status));
  }
  while (status == GSL_CONTINUE && iter < 100);
  
  gsl_multifit_covar (s->J, 0.0, covar);

  gsl_multifit_fdfsolver_free (s); /* free solver space */
  gsl_matrix_free (covar); /* free covariance matrix space */
}