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

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


struct data {
	size_t n;
	double *y;
	double *sigma;
} ;


int print_state (size_t iter, gsl_multifit_fdfsolver * s) {
	printf ("iter: %3u x = % 15.8f % 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_vector_get (s->x, 3), 
          gsl_vector_get (s->x, 4), 
          gsl_blas_dnrm2 (s->f));
}

int func_f(const gsl_vector *x, void *params, gsl_vector *f) {
	size_t n = ((struct data *)params)->n;
	double *y = ((struct data *)params)->y;
	double *sigma = ((struct data *)params)->sigma;
	size_t i;

	double Ar = gsl_vector_get(x, 0);
	double Ai = gsl_vector_get(x, 1);
	double Br = gsl_vector_get(x, 2);
	double Bi = gsl_vector_get(x, 3);
	double k  = gsl_vector_get(x, 4);

	for (i = 0; i < n; i++ ) {
		gsl_vector_set(f, i, (Ar*cos(k*i) - Ai*sin(k*i) + Br*cos(k*i) + Bi*sin(k*i) - y[i])/sigma[i]);
	}

	return GSL_SUCCESS;
}

int func_df(const gsl_vector *x, void *params, gsl_matrix *J) {
	size_t n = ((struct data *)params)->n;
	double *sigma = ((struct data *)params)->sigma;
	size_t i;

	double Ar = gsl_vector_get(x, 0);
	double Ai = gsl_vector_get(x, 1);
	double Br = gsl_vector_get(x, 2);
	double Bi = gsl_vector_get(x, 3);
	double k  = gsl_vector_get(x, 4);

	for(i = 0; i < n; i++) {
		gsl_matrix_set(J, i, 0, cos(k*i)/sigma[i]);
		gsl_matrix_set(J, i, 1, -sin(k*i)/sigma[i]);
		gsl_matrix_set(J, i, 2, cos(k*i)/sigma[i]);
		gsl_matrix_set(J, i, 3, sin(k*i)/sigma[i]);
		gsl_matrix_set(J, i, 4, (-i*Ar*sin(k*i) - i*Ai*cos(k*i) - i*Br*sin(k*i) + i*Bi*cos(k*i))/sigma[i]);
	}

	return GSL_SUCCESS;
}

int func_fdf(const gsl_vector *x, void *params, gsl_vector *f, gsl_matrix *J) {
	func_f(x, params, f);
	func_df(x, params, J);

	return GSL_SUCCESS;
}

int main() {
	FILE *fp; float t1, t2, t3, t4, t5, t6, t7, t8, t9;
	const gsl_multifit_fdfsolver_type *T;
	gsl_multifit_fdfsolver *s;
	int status;
	size_t i, iter = 0;
	const size_t n = N;
	const size_t p = 5;
	gsl_matrix *covar = gsl_matrix_alloc(p,p);
	double y[N], sigma[N];
	struct data d = {n, y, sigma};
	gsl_multifit_function_fdf f;
	double x_init[5] = {1.0, 2.0, 3.0, 4.0, 0.5}; /* actual values */
	gsl_vector_view x = gsl_vector_view_array(x_init, p);

	f.f      = &func_f;
	f.df     = &func_df;
	f.fdf    = &func_fdf;
	f.n      = n;
	f.p      = p;
	f.params = &d;

	fp = fopen("out", "r");
	for(i = 0; i < n; i++) {
		fscanf(fp, "%f %f %f %f %f %f %f %f %f", &t1,&t2,&t3,&t4,&t5,&t6,&t7,&t8,&t9);
		y[i] = (double)t2;
		sigma[i] = 0.1;
	}
	fclose(fp);

	T = gsl_multifit_fdfsolver_lmsder;
	s = gsl_multifit_fdfsolver_alloc(T, n, p);
	gsl_multifit_fdfsolver_set(s, &f, &x.vector);

	do {
		iter++;
		status = gsl_multifit_fdfsolver_iterate(s);
		print_state (iter, s);
		if(status) { fprintf(stderr, "iterate failed -- %s\n", gsl_strerror(status)); break; }
		if ((status = gsl_multifit_test_delta(s->dx, s->x, 1e-4, 1e-4))) { fprintf(stderr, "test_delta failed -- %s\n", gsl_strerror(status)); };
	} while(status == GSL_CONTINUE && iter < 500);

	gsl_multifit_covar(s->J, 0.0, covar);
	gsl_matrix_fprintf(stdout, covar, "%g");

	printf("Ar   =   %.5f   +/-   %.5f\n", FIT(0), ERR(0));
	printf("Ai   =   %.5f   +/-   %.5f\n", FIT(1), ERR(1));
	printf("Br   =   %.5f   +/-   %.5f\n", FIT(2), ERR(2));
	printf("Bi   =   %.5f   +/-   %.5f\n", FIT(3), ERR(3));
	printf("k    =   %.5f   +/-   %.5f\n", FIT(4), ERR(4));

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

	gsl_multifit_fdfsolver_free(s);

	return EXIT_SUCCESS;
}