/***********
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 = 8;
size_t p = 4;
double xdata[8] = {1.0,2.0,3.0,4.0,5.0,6.0,7.0,9.0};
double ydata[8] = {2.1,6.1,11.9,19.7,29.8,42.2,56.2,87.0};

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 d = gsl_vector_get(x, 3);
  double func, xthis;
  printf("myf params: %f %f %f %f\n", a, b, c, d);

  for (i = 0; i < n; i++) {
    xthis = xdata[i];
    func = a*xthis*xthis*xthis + b*xthis*xthis + c*xthis + d;
    printf("function %d %f\n", i, ydata[i] - func);
    gsl_vector_set(f, i, func - ydata[i]);
  }
  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 d = gsl_vector_get(x, 3);
  double func, xthis;
  double dfda, dfdb, dfdc, dfdd;
  printf("mydf params: %f %f %f %f\n", a, b, c, d);
  /* Jacobian matrix J(i,j) = dfi / dxj */
  for (i = 0; i < n; i++) {
    xthis = xdata[i];
    dfda = xthis*xthis*xthis;
    dfdb = xthis*xthis;
    dfdc = xthis;
    dfdd = 1.0;
    printf("derivs %d %f %f %f %f\n", i, dfda, dfdb, dfdc, dfdd);
    gsl_matrix_set (J, i, 0, dfda); 
    gsl_matrix_set (J, i, 1, dfdb);
    gsl_matrix_set (J, i, 2, dfdc);
    gsl_matrix_set (J, i, 3, dfdd);
  }
  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 % 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[4] = { 0.0, 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("===begin iter = %d===\n", iter);
    status = gsl_multifit_fdfsolver_iterate(s); /* go to next point */
    printf("status from iterate = %d, %s\n", status, gsl_strerror (status));
    print_state(iter, s);
    if (status) break;
    status = gsl_multifit_test_delta(s->dx, s->x, 1e-8, 1e-8);
    printf("status from test delta = %d, %s\n", status, gsl_strerror(status));
  }
  while (status == GSL_CONTINUE && iter < 10);
  
  gsl_multifit_covar (s->J, 0.0, covar);

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