#include "i3_minimizer.h"
#include "i3_math.h"
#include "i3_model.h"
// #include "i3_powell.h"
#include "i3_praxis.h"
#include "i3_image_fits.h"
#include "levmar/levmar.h"
#define LEVMAR_NITS 10
#include "gsl/gsl_version.h"
#ifdef I3_USE_DOUBLE
#define levmar_bc_dif dlevmar_bc_dif
#define levmar_bc_der dlevmar_bc_der
#else
#define levmar_bc_dif slevmar_bc_dif
#define levmar_bc_der slevmar_bc_der
#endif

#define SCALE_TO_UNIT 0
#define USE_CONSTRAINED_OPTIMIZATION 0

typedef struct i3_1D_minimizer_data{int n; i3_minimizer * minimizer; i3_flt * point;} i3_1D_minimizer_data;


i3_minimizer * i3_minimizer_create(i3_model * model, i3_options * options){
        int nparam  = i3_model_number_varied_params(model);
        i3_minimizer * minimizer = (i3_minimizer*)malloc(sizeof(i3_minimizer));
        minimizer->model = model;
        minimizer->nparam = nparam;
        minimizer->model_image = NULL;
        // minimizer->minimizer = gsl_multimin_fminimizer_alloc (gsl_multimin_fminimizer_nmsimplex2, minimizer->nparam);
        // minimizer->function_wrapper = (gsl_multimin_function*)malloc(sizeof(gsl_multimin_function));
        // minimizer->function_wrapper->n = minimizer->nparam;
        // minimizer->function_wrapper->f = i3_posterior_wrapper;
        // minimizer->function_wrapper->params = minimizer;
        minimizer->step_size = i3_model_option_widths(model,options);
        minimizer->max_iterations = options->minimizer_max_iterations;
        minimizer->tolerance = options->minimizer_tolerance;
        minimizer->iterations = 0;
        minimizer->start = (i3_parameter_set*)malloc(model->nbytes);
        // minimizer->powell_best_point=(i3_flt*)malloc(nparam*sizeof(i3_flt));
        minimizer->best_like=0;
        // minimizer->golden_best_point=(i3_flt*)malloc(nparam*sizeof(i3_flt));
        // minimizer->best_like=0;
        // minimizer->best_like=0;
		minimizer->covariance_estimate = (i3_flt*) malloc(sizeof(i3_flt)*nparam*nparam);
        minimizer->verbosity=options->minimizer_verbosity;
		minimizer->has_covariance_estimate = false;

	minimizer->options = options;

        return minimizer;
}


void i3_minimizer_destroy(i3_minimizer * minimizer){
        free(minimizer->step_size);
		free(minimizer->covariance_estimate);
        // free(minimizer->function_wrapper);
        // free(minimizer->powell_best_point);
        // free(minimizer->golden_best_point);
        // gsl_multimin_fminimizer_free (minimizer->minimizer); 
		free(minimizer->start);
        if (minimizer->model_image) i3_image_destroy(minimizer->model_image);
        free(minimizer);
}

gsl_vector * i3_minimizer_pointer_to_vector(i3_minimizer * minimizer, i3_parameter_set * x){
        /* This function takes parameters from a parameter_set and turns them into a gsl_vector.
           It only takes the parameters which are listed as being varied in the model.
           Since the minimizer cannot use discrete parameters it will cause the program to exit
           if given a bool or int parameter that is not listed as fixed.
         */
        i3_model * model = minimizer->model;
        gsl_vector * v = gsl_vector_alloc(minimizer->nparam);  /* The minimizer already knows the number of varied parameters. */
        int j=0;
        int offset; 
        double value;
        for(int i=0;i<model->nparam;i++){
                offset = model->byte_offsets[i];  /* The memory position of the parameter in the parameter object. */
                if (model->param_fixed[i]) continue;
                if (!model->param_type[i]==i3_parameter_type_flt) I3_FATAL("Tried to use minimizer on non-float parameter.",I3_MATH_ERROR);
                value = (double)(*((i3_flt*)(((char*)x)+offset)));  /* The memory position of the parameter in the parameter object. */
                gsl_vector_set(v,j++,value);
        }
        return v;
}



void i3_minimizer_vector_to_pointer(i3_minimizer * minimizer, const gsl_vector * v, i3_parameter_set * x){
        i3_model * model = minimizer->model;
        i3_model_copy_parameters(minimizer->model,x,minimizer->start);
        int j=0;
        for(int i=0;i<model->nparam;i++){
                if (model->param_fixed[i]) continue;
                int offset = model->byte_offsets[i];
                *((i3_flt*)(((char*)x)+offset)) = gsl_vector_get(v,j++);
        }
        
}




void i3_minimizer_extract_varied_parameters(i3_minimizer * minimizer, i3_parameter_set * x, i3_flt * output){
        i3_model * model = minimizer->model;
        int j=0;
        int offset; 
        double value;
        for(int i=0;i<model->nparam;i++){
                offset = model->byte_offsets[i];  /* The memory position of the parameter in the parameter object. */
                if (model->param_fixed[i]) continue;
                if (!model->param_type[i]==i3_parameter_type_flt) I3_FATAL("Tried to use minimizer on non-float parameter.",I3_MATH_ERROR);
                value = (i3_flt)(*((i3_flt*)(((char*)x)+offset)));  /* The memory position of the parameter in the parameter object. */
                output[j++]=value;
        }
}

void i3_minimizer_extract_varied_parameters_double(i3_minimizer * minimizer, i3_parameter_set * x, double * output){
        i3_flt output_float[minimizer->nparam];
        i3_minimizer_extract_varied_parameters(minimizer,x,output_float);
        for(int i=0;i<minimizer->nparam;i++) output[i] = output_float[i];
}


void i3_minimizer_input_varied_parameters(i3_minimizer * minimizer, i3_flt * input, i3_parameter_set * x){
        i3_model * model = minimizer->model;
        i3_model_copy_parameters(minimizer->model,x,minimizer->start);
        
        int j=0;
        for(int i=0;i<model->nparam;i++){
                if (model->param_fixed[i]) continue;
                int offset = model->byte_offsets[i];
                *((i3_flt*)(((char*)x)+offset)) = input[j++];
        }
        
}

void i3_minimizer_input_varied_parameters_double(i3_minimizer * minimizer, double * input, i3_parameter_set * x){
        i3_flt input_float[minimizer->nparam];
        for(int i=0;i<minimizer->nparam;i++) input_float[i] = input[i];
        i3_minimizer_input_varied_parameters(minimizer,input_float,x);
}


// i3_flt i3_posterior_wrapper_powell(i3_flt * x, void * params){
//      i3_minimizer * minimizer = (i3_minimizer*) params;
//      i3_model * model = minimizer->model;
//      i3_parameter_set * parameter_set = alloca(model->nbytes);
//      i3_minimizer_input_varied_parameters(minimizer,x,parameter_set);
//      i3_flt posterior = i3_model_posterior(model,minimizer->model_image,parameter_set,minimizer->data_set);
//      return -posterior;
// }

double i3_posterior_wrapper_double(double * x, void * params){
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_model * model = minimizer->model;
        i3_parameter_set * parameter_set = alloca(model->nbytes);
        i3_flt p[minimizer->nparam];
        for(int i=0;i<minimizer->nparam;i++) p[i]=x[i];
        i3_minimizer_input_varied_parameters(minimizer,p,parameter_set);
        // i3_flt posterior = i3_model_posterior(model,minimizer->model_image,parameter_set,minimizer->data_set);
        i3_flt posterior = model->likelihood(minimizer->model_image,parameter_set,minimizer->data_set);

        return -((double) posterior);
}

i3_flt i3_posterior_wrapper_flt(i3_flt * x, void * params){
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_model * model = minimizer->model;
        i3_parameter_set * parameter_set = malloc(model->nbytes);
        i3_minimizer_input_varied_parameters(minimizer,x,parameter_set);
		// i3_model_printf_parameters(minimizer->model,parameter_set);printf("\n");
        i3_flt posterior = model->likelihood(minimizer->model_image,parameter_set,minimizer->data_set);
		free(parameter_set);
        return -posterior;
}


/* Return the negative of the posterior of the parameters in the vector x, cast to double for gsl. */
double i3_posterior_wrapper_gsl(const gsl_vector * x, void * params){
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_model * model = minimizer->model;
        i3_parameter_set * parameter_set = alloca(model->nbytes);
        I3_ENSURE_FLOAT;
        i3_minimizer_vector_to_pointer(minimizer, x, parameter_set);
        i3_flt posterior = i3_model_posterior(model,minimizer->model_image,parameter_set,minimizer->data_set);
        return (double) -posterior;
}



double i3_posterior_wrapper_double_1D(double x, void * params){
        i3_1D_minimizer_data * data = (i3_1D_minimizer_data*) params;
        i3_minimizer * minimizer = data->minimizer;
        int n = data->n;
        i3_model * model = minimizer->model;
        i3_parameter_set * parameter_set = alloca(model->nbytes);
        i3_flt parameters[minimizer->nparam];
        memcpy(parameters,data->point,sizeof(i3_flt)*minimizer->nparam);
        parameters[n]=x;
        
        i3_minimizer_input_varied_parameters(minimizer,parameters,parameter_set);
        i3_flt posterior = i3_model_posterior(model,minimizer->model_image,parameter_set,minimizer->data_set);
        return -(double)posterior;
}



int run_count=0;


i3_parameter_set * i3_minimizer_run(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set,i3_data_set * data_set, i3_minimizer_method method)
{
    i3_minimizer_setup_model_image(minimizer,data_set->image);
        switch(method){
                case i3_minimizer_method_simplex:
                return i3_minimizer_run_simplex(minimizer, initial_parameter_set, data_set);
                // case i3_minimizer_method_powell:
                // return i3_minimizer_run_powell(minimizer, initial_parameter_set, data_set);
                case i3_minimizer_method_praxis:
                return i3_minimizer_run_praxis(minimizer, initial_parameter_set, data_set);
                case i3_minimizer_method_golden:
                return i3_minimizer_run_golden(minimizer, initial_parameter_set, data_set);
                case i3_minimizer_method_levmar:
                return i3_minimizer_run_levmar(minimizer, initial_parameter_set, data_set);
                break;
                default:
                I3_FATAL("Unknown minimizer type.",1);  
        }
        return NULL;
}

i3_parameter_set * i3_minimizer_run_praxis(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set,i3_data_set * data_set)
{
        minimizer->data_set=data_set;
        i3_model_copy_parameters(minimizer->model,minimizer->start,initial_parameter_set);
        i3_flt x0_float[minimizer->nparam];
        i3_minimizer_extract_varied_parameters(minimizer,initial_parameter_set,x0_float);

        i3_flt param_min_float[minimizer->nparam];
        i3_flt param_max_float[minimizer->nparam];

        i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->min,param_min_float);
        i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->max,param_max_float);

        double x0[minimizer->nparam];
        double minima[minimizer->nparam];
        double maxima[minimizer->nparam];
        
        for(int i=0;i<minimizer->nparam;i++){
                x0[i]=(double)x0_float[i];      
                minima[i]=(double)param_min_float[i];
                maxima[i]=(double)param_max_float[i];
        }

        double minimum_value;
        double best_fit[minimizer->nparam];
        i3_flt best_fit_float[minimizer->nparam];
        
//      int i3_praxis_minimization(praxis_function f, double * start_position, double step_size, void * args, int n, double tolerance, double * minimum_value, double * minimum_location)
        i3_praxis_minimization((praxis_function)&i3_posterior_wrapper_double, x0, minimizer->max_iterations, minima, maxima, (void*)minimizer, minimizer->nparam, minimizer->tolerance, &minimum_value, best_fit, minimizer->verbosity);

        for(int i=0;i<minimizer->nparam;i++) best_fit_float[i] = (i3_flt) best_fit[i];

        minimizer->best_like = minimum_value;

        i3_parameter_set * p = malloc(minimizer->model->nbytes);
        i3_minimizer_input_varied_parameters(minimizer,best_fit_float,p);
        return p;
        
}

/*
static void i3_minimizer_lbf_function(
        double * params,
    double * like,
        double * gradient,
        void * args
    )
{
        
        i3_minimizer * minimizer = (i3_minimizer*) args;
        
        i3_parameter_set * p = (i3_parameter_set*) malloc(minimizer->model->nbytes);
        i3_parameter_set * pprime = (i3_parameter_set*) malloc(minimizer->model->nbytes);
        
        //copy from an array into a parameter set
        i3_minimizer_input_varied_parameters_double(minimizer, params, p);
        i3_flt like_float;

        if (minimizer->model->posterior_derivative!=NULL){
                like_float = i3_model_posterior(minimizer->model,minimizer->model_image,p,minimizer->data_set);
                minimizer->model->posterior_derivative(minimizer->model_image,like_float,minimizer->data_set,p,pprime);
        }
        else{
                i3_flt epsilon = 1.0e-6;
                i3_model_posterior_derivative_approximation(minimizer->model, p, minimizer->model_image, minimizer->data_set, pprime, &like_float, epsilon);    
        }

        i3_model_scale_parameters(minimizer->model, pprime, -1.0);
        *like = -like_float;
        // //copy from a parameter set to an array
        i3_minimizer_extract_varied_parameters_double(minimizer,pprime,gradient);  //This fills in the gradient.
        

        // printf("---------------------\n Gradient:\n");       
        // i3_model_pretty_printf_parameters(minimizer->model,pprime);printf("\n");
        // printf("like = %g\n",*like);
        // printf("---------------------\n");

        free(p);
        free(pprime);
}


i3_parameter_set * i3_minimizer_run_lbf(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set,i3_data_set * data_set){
        minimizer->data_set=data_set;
        double x0[minimizer->nparam];
        i3_model_copy_parameters(minimizer->model,minimizer->start,initial_parameter_set);

        double lower_bound[minimizer->nparam];
        double upper_bound[minimizer->nparam];
        i3_minimizer_extract_varied_parameters_double(minimizer,initial_parameter_set,x0);
        i3_minimizer_extract_varied_parameters_double(minimizer,minimizer->model->min,lower_bound);
        i3_minimizer_extract_varied_parameters_double(minimizer,minimizer->model->max,upper_bound);

        int lbf_verbosity = -1;
        if (minimizer->verbosity>0) lbf_verbosity = 0;
        if (minimizer->verbosity>2) lbf_verbosity = 1;

        lbf_solve(minimizer->nparam,x0, lower_bound, upper_bound, 
                                        i3_minimizer_lbf_function, minimizer, lbf_verbosity, 
                                        minimizer->max_iterations, minimizer->tolerance);


        i3_parameter_set * result = malloc(minimizer->model->nbytes);
        i3_minimizer_input_varied_parameters_double(minimizer, x0, result);
        return result;
}
*/
// i3_parameter_set * i3_minimizer_run_powell(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set,i3_data_set * data_set){
//         minimizer->data_set=data_set;
//         i3_model_copy_parameters(minimizer->model,minimizer->start,initial_parameter_set);
//         i3_flt x0[minimizer->nparam];
//         i3_flt param_min[minimizer->nparam];
//         i3_flt param_max[minimizer->nparam];
//         i3_flt x[minimizer->nparam];
//         fprintf(stderr,"Minimizing for nparam = %d\n",minimizer->nparam);
//         i3_minimizer_extract_varied_parameters(minimizer,initial_parameter_set,x0);
//         i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->min,param_min);
//         i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->max,param_max);
//         i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->max,x);
// 
//         int result = i3_powell_minimization(&i3_posterior_wrapper_flt, x0, param_min,param_max, (void*)minimizer, minimizer->nparam, minimizer->max_iterations, minimizer->tolerance, &(minimizer->best_like), x);
//         if (result) return NULL;
//         i3_parameter_set * output = (i3_parameter_set *) malloc(minimizer->model->nbytes);
//         i3_minimizer_input_varied_parameters(minimizer,x,output);
//         return output;
// }


i3_parameter_set * i3_minimizer_run_golden(i3_minimizer * minimizer, i3_parameter_set * initial,i3_data_set * data_set)
{
    const gsl_min_fminimizer_type *T = gsl_min_fminimizer_brent;
        minimizer->data_set = data_set;
        i3_model_copy_parameters(minimizer->model,minimizer->start,initial);
    gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (T);

                
    gsl_function F;  
        i3_1D_minimizer_data data;
        data.minimizer=minimizer;
    F.function = &i3_posterior_wrapper_double_1D;
    F.params = &data;
        i3_flt min_vals[minimizer->nparam];
        i3_flt max_vals[minimizer->nparam];
        i3_flt best_point[minimizer->nparam];
        data.point = best_point;
        i3_minimizer_extract_varied_parameters(minimizer,initial,best_point);
        i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->min,min_vals);
        i3_minimizer_extract_varied_parameters(minimizer,minimizer->model->max,max_vals);

        minimizer->best_like  = i3_model_posterior(minimizer->model,minimizer->model_image,initial,data_set);
        gsl_error_handler_t * handler = gsl_set_error_handler_off();
        i3_flt delta = FLT_MAX;
        i3_flt previous_best_like=-minimizer->best_like;
        int it=0;
        while (delta>0.01){
                for (int p=0;p<minimizer->nparam;p++){
                        data.n=p;
                        
                        int setup_status = gsl_min_fminimizer_set(s, &F, (double)best_point[p], (double)min_vals[p], (double)max_vals[p]);
                        if (setup_status==GSL_EINVAL) continue;
                        for (int i=0;i<100;i++){
                                int status = gsl_min_fminimizer_iterate(s);
                                if (status==GSL_EBADFUNC) I3_FATAL("Error in minimizer",6745);
                                if (gsl_min_fminimizer_x_upper(s)-gsl_min_fminimizer_x_lower(s)<1e-6) break;
                        }
                        best_point[p]=gsl_min_fminimizer_x_minimum(s);
                        minimizer->best_like = gsl_min_fminimizer_f_minimum(s);
                }
                delta = previous_best_like-minimizer->best_like;
                previous_best_like=minimizer->best_like;
                it++;
        }

        gsl_set_error_handler(handler);
        gsl_min_fminimizer_free(s);
        i3_parameter_set * result = malloc(minimizer->model->nbytes);
        i3_minimizer_input_varied_parameters(minimizer,best_point,result);
        return result;

}

i3_parameter_set * i3_minimizer_run_simplex(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set,i3_data_set * data_set)
{
	/* A lot of people (i.e. Barney) are running slightly older versions of gsl, which do not have gsl_multimin_fminimizer_nmsimplex2.
	This means this bit does not compile.  The version 2 is better but use version 1 if not available.	
	It also turns out that GSL_MAJOR_VERSION is only defined in later versions!  So we also check for that.
	*/
#ifdef GSL_MAJOR_VERSION
#if (GSL_MAJOR_VERSION==1 && GSL_MINOR_VERSION<14)
		gsl_multimin_fminimizer * engine = gsl_multimin_fminimizer_alloc (gsl_multimin_fminimizer_nmsimplex, minimizer->nparam);  
#else
        gsl_multimin_fminimizer * engine = gsl_multimin_fminimizer_alloc (gsl_multimin_fminimizer_nmsimplex2, minimizer->nparam);
#endif
#else
        gsl_multimin_fminimizer * engine = gsl_multimin_fminimizer_alloc (gsl_multimin_fminimizer_nmsimplex, minimizer->nparam);
#endif

        minimizer->data_set = data_set;
        i3_model_copy_parameters(minimizer->model,minimizer->start,initial_parameter_set);      
        
        
        gsl_multimin_function function_wrapper;
        function_wrapper.n = minimizer->nparam;
        function_wrapper.f = i3_posterior_wrapper_gsl;
        function_wrapper.params = minimizer;
        
        

        /* Convert the starting point parameter structure to gsl_vector */
        
        gsl_vector * x0 = i3_minimizer_pointer_to_vector(minimizer,initial_parameter_set);      
        gsl_vector * step_size = i3_minimizer_pointer_to_vector(minimizer,minimizer->step_size);


        /* Set up the GSL mimimizer objects */
        gsl_multimin_fminimizer_set (engine, &function_wrapper, x0, step_size);

        int status=0;
        int converged=0;
        
        /* Iterate towards the solution */
        for(int iter=0;iter<minimizer->max_iterations;iter++){
                status = gsl_multimin_fminimizer_iterate(engine);

                /* Test for some kind of serious failure in the gsl code. */
                if (status) break;

                /* Test for convergence */
                double convergence = gsl_multimin_fminimizer_size(engine);
          if (i3_fabs(convergence) < minimizer->tolerance){
                        converged=1;
                        break;
        }
        }
        
        gsl_vector_free (x0);
        gsl_vector_free (step_size);

        if (status) return NULL;
        i3_parameter_set * p = malloc(minimizer->model->nbytes);
        i3_minimizer_vector_to_pointer(minimizer, engine->x, (i3_flt*)p );
        gsl_multimin_fminimizer_free(engine);
        return p;
}




// double (* f) (const gsl_vector * x, void * params)
// void (* df) (const gsl_vector * x, void * params, gsl_vector * g)
// void (* fdf) (const gsl_vector * x, void * params, double * f, gsl_vector * g);

double i3_posterior_derivative_wrapper_function(const gsl_vector * x, void * params)
{
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_model * model = minimizer->model;
        i3_parameter_set * parameter_set = alloca(model->nbytes);
        I3_ENSURE_FLOAT;
        i3_minimizer_vector_to_pointer(minimizer, x, parameter_set);
        i3_flt posterior = i3_model_posterior(model,minimizer->model_image,parameter_set,minimizer->data_set);


//      i3_quadratic_test_parameter_set * pp = (i3_quadratic_test_parameter_set *) parameter_set;

        return (double) -posterior;
}

void i3_posterior_derivative_wrapper_derivative(const gsl_vector * x, void * params, gsl_vector * gradient)
{
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_model * model = minimizer->model;

        if (!(model->posterior_derivative)) I3_FATAL("Tried to minimize a function without derivative using gradient method",1);
        double like = i3_posterior_derivative_wrapper_function(x, params) ; //This ensures that minimizer->model_image has the right model in.

        I3_ENSURE_FLOAT;
        i3_parameter_set * p = alloca(model->nbytes);
        i3_parameter_set * p_prime = alloca(model->nbytes);
        i3_minimizer_vector_to_pointer(minimizer, x, p);

        model->posterior_derivative(minimizer->model_image,-like,minimizer->data_set,p,p_prime);
//      i3_quadratic_test_parameter_set * pp = (i3_quadratic_test_parameter_set *) p_prime;
        gsl_vector * p_prime_vector = i3_minimizer_pointer_to_vector(minimizer,p_prime);
        gsl_vector_memcpy(gradient,p_prime_vector);
        gsl_vector_free(p_prime_vector);
        gsl_vector_scale(gradient,-1.0);
        minimizer->gradient_like = like; //We do this to ensure that the "both" function below can get it.


        

}

void i3_posterior_derivative_wrapper_both(const gsl_vector * x, void * params, double * value, gsl_vector * gradient)
{
        i3_minimizer * minimizer = (i3_minimizer*) params;
        i3_posterior_derivative_wrapper_derivative(x, params, gradient);
        *value = minimizer->gradient_like;
        
}



i3_parameter_set * i3_minimizer_with_derivatives(i3_minimizer * minimizer, i3_parameter_set * initial, i3_data_set * data_set)
{
        gsl_multimin_fdfminimizer * M = gsl_multimin_fdfminimizer_alloc(gsl_multimin_fdfminimizer_vector_bfgs2, minimizer->nparam);
        
        gsl_multimin_function_fdf F;
        F.f = i3_posterior_derivative_wrapper_function;
        F.df = i3_posterior_derivative_wrapper_derivative;
        F.fdf = i3_posterior_derivative_wrapper_both;
        F.n = minimizer->nparam;
        F.params = minimizer;

        double step_size = 1.0;
        double internal_tolerance = 0.01;
        double gradient_tolerance = minimizer->tolerance;
        gsl_vector * x0 = i3_minimizer_pointer_to_vector(minimizer,initial);
        gsl_multimin_fdfminimizer_set(M, &F, x0, step_size, internal_tolerance);

        int status=0;
        int converged=0;
        /* Iterate towards the solution */
        for(int iter=0;iter<minimizer->max_iterations;iter++){
                status = gsl_multimin_fdfminimizer_iterate(M);
                gsl_vector * gradient = gsl_multimin_fdfminimizer_gradient(M);
                gsl_vector * value = gsl_multimin_fdfminimizer_x(M);
                printf("Recorded value = (%lf,%lf,%lf,%lf)   gradient = (%lf,%lf,%lf,%lf)\n",
                        gsl_vector_get(value,0),gsl_vector_get(value,1),gsl_vector_get(value,2),gsl_vector_get(value,3),
                        gsl_vector_get(gradient,0),gsl_vector_get(gradient,1),gsl_vector_get(gradient,2),gsl_vector_get(gradient,3)
                                                                                                                        );
                /* Test for some kind of serious failure in the gsl code. */
                if (status) {
                        I3_WARNING(gsl_strerror(status));
                        //break;
                }

                /* Test for convergence */
                converged = gsl_multimin_test_gradient(gradient, gradient_tolerance);
                if (converged==GSL_SUCCESS) break;
        }
        
        
        i3_parameter_set * result;
        if (status){
                result=NULL;
        }
        else{
                gsl_vector * x = gsl_multimin_fdfminimizer_x(M);
                result = malloc(minimizer->model->nbytes);
                i3_minimizer_vector_to_pointer(minimizer,x,result);
        }

        gsl_multimin_fdfminimizer_free(M);      
        gsl_vector_free (x0);


        return result;

}

void i3_scale_to_unit(int n, i3_flt * minima, i3_flt * maxima, i3_flt * unscaled, i3_flt * scaled)
{
        for (int p=0;p<n;p++){
                i3_flt range = maxima[p]-minima[p];
                scaled[p] = (unscaled[p]-minima[p])/range;
        }
}



void i3_scale_from_unit(int n, i3_flt * minima, i3_flt * maxima, i3_flt * scaled, i3_flt * unscaled){
        for (int p=0;p<n;p++){
                i3_flt range = maxima[p]-minima[p];
                unscaled[p] = scaled[p]*range + minima[p];
        }
}

int icounter = 0;

bool is_milestone(i3_minimizer * minimizer){
        return strcmp(minimizer->model->name,"milestone")==0;
}

void i3_minimizer_wrapper_levmar(i3_flt *p, i3_flt *hx, int m, int n, void *adata)
{
        i3_minimizer * minimizer = (i3_minimizer*) adata;
        i3_flt p_unscaled[minimizer->nparam];
#if SCALE_TO_UNIT	
		i3_scale_from_unit(minimizer->nparam, minimizer->lower_flt, minimizer->upper_flt, p, p_unscaled);
#else
	for (int i=0;i<minimizer->nparam;i++) p_unscaled[i] = p[i];
#endif
        // }
        i3_flt like = i3_posterior_wrapper_flt(p_unscaled, adata);
        i3_image * model_image = minimizer->model_image;
        if (minimizer->data_set==NULL || minimizer->data_set->weight==NULL || minimizer->data_set->image==NULL ) I3_FATAL("You need to have an image and weight to use levmar",1);
        i3_image * data_image = minimizer->data_set->image;
        i3_image * weight = minimizer->data_set->weight;
        if (n!=data_image->n) I3_FATAL("LevMar - n should equal npix",2);
        if (m!=minimizer->nparam) I3_FATAL("LevMar - m should equal nparam",3);
		// printf("like = %f\n");
        if (isnan(like)||like==BAD_LIKELIHOOD){
                i3_print(i3_verb_noisy, "i3_minimizer_wrapper_levmar bad like (%d)", isnan(like) );
                for(int i=0;i<n;i++){
                    // hx[i] =  -2*data_image->data[i]*i3_sqrt(weight->data[i]);
                    hx[i] =  -data_image->data[i]*i3_sqrt(weight->data[i]);
                }
        } else{
                // FILE * f = fopen("diff.txt","w");
                for(int i=0;i<n;i++){
                    hx[i] = model_image->data[i]*i3_sqrt(weight->data[i]);
                    // hx[i] = (model_image->data[i] - data_image->data[i])*i3_sqrt(weight->data[i]);
                        // fprintf(f,"%e\n",hx[i]);
                }
                // fclose(f);
        }
}

static void i3_levmar_fdif_forw_jac_approx(void (*func)(i3_flt *p, i3_flt *hx, int m, int n, void *adata), i3_flt *p, i3_flt *hxm, i3_flt *hxp, i3_flt delta, i3_flt *jac, int m, int n, void *adata){

register int i, j;
i3_flt tmp;
register i3_flt d;

  for(j=0; j<m; ++j){
    /* determine d=max(1E-04*|p[j]|, delta), see HZ */
    d=1E-04*p[j]; // force evaluation
    d=FABS(d);
    if(d<delta)
      d=delta;
    //printf("p[%d]=%f\n", j, p[j]);
    tmp=p[j];
    p[j]+=d;

    (*func)(p, hxp, m, n, adata);

    p[j]=tmp; /* restore */

    d=1.0/d; /* invert so that divisions can be carried out faster as multiplications */
    for(i=0; i<n; ++i){
      jac[i*m+j]=(hxp[i]-hxm[i])*d;
      //jac[i*m+j]=hxp[i];
    }
  }
}

// static void i3_levmar_fdif_forw_jac_approx_j(void (*func)(i3_flt *p, i3_flt *hx, int m, int n, void *adata), i3_flt *p, i3_flt *hxm, i3_flt *hxp, i3_flt delta, i3_flt *jac, int m, int n, int j, void *adata){

// register int i;
// i3_flt tmp;
// register i3_flt d;

//  determine d=max(1E-04*|p[j]|, delta), see HZ 
//  d=1E-04*p[j]; // force evaluation
//  d=FABS(d);
//  if(d<delta)
//    d=delta;

//  tmp=p[j];
//  p[j]+=d;
 
//  (*func)(p, hxp, m, n, adata);
    
//  p[j]=tmp; /* restore */

//  d=1.0/d; /* invert so that divisions can be carried out faster as multiplications */
//  for(i=0; i<n; ++i){
//    jac[i*m+j]=(hxp[i]-hxm[i])*d;
//   }
// }

static void i3_levmar_fdif_cent_jac_approx(void (*func)(i3_flt *p, i3_flt *hx, int m, int n, void *adata), i3_flt *p, i3_flt *hxm, i3_flt *hxp, i3_flt delta, i3_flt *jac, int m, int n, void *adata){

register int i, j;
i3_flt tmp;
register i3_flt d;

  for(j=0; j<m; j++){
    // MH: commented this out from the original finite difference code as done in levmar/misc_core.c
    /* determine d=max(1E-04*|p[j]|, delta), see HZ */
    d=1E-04*p[j]; // force evaluation
    d=FABS(d);
    if(d<delta)
      d=delta;

    //d=delta;

    tmp=p[j];
    p[j]-=d;
    (*func)(p, hxm, m, n, adata);

    p[j]=tmp+d;
    (*func)(p, hxp, m, n, adata);
    p[j]=tmp; /* restore */

    d=0.5/d; /* invert so that divisions can be carried out faster as multiplications */
    for(i=0; i<n; ++i){
      jac[i*m+j]=(hxp[i]-hxm[i])*d;
    }
  }
}

// static void i3_levmar_fdif_cent_jac_approx_j(void (*func)(i3_flt *p, i3_flt *hx, int m, int n, void *adata), i3_flt *p, i3_flt *hxm, i3_flt *hxp, i3_flt delta, i3_flt *jac, int m, int n, int j, void *adata){

// register int i;
// i3_flt tmp;
// register i3_flt d;
//  d=1E-04*p[j]; // force evaluation
//  d=FABS(d);
//  if(d<delta)
//    d=delta;
 
//  //d=delta;
 
//  tmp=p[j];
//  p[j]-=d;
//  (*func)(p, hxm, m, n, adata);
 
//  p[j]=tmp+d;
//  (*func)(p, hxp, m, n, adata);
//  p[j]=tmp; /* restore */

//  d=0.5/d; /* invert so that divisions can be carried out faster as multiplications */
//  for(i=0; i<n; ++i){
//    jac[i*m+j]=(hxp[i]-hxm[i])*d;
//  }
// }

void i3_minimizer_wrapper_levmar_jacobian(i3_flt *p, i3_flt *jac, int m, int n, void *adata){
#if NO_SERSICS
    I3_FATAL("Jacobian code currently assumes sersics model.", 3);
#else

        i3_minimizer * minimizer = (i3_minimizer*) adata; 
        i3_flt p_unscaled[minimizer->nparam];
#if SCALE_TO_UNIT	
	i3_scale_from_unit(minimizer->nparam, minimizer->lower_flt, minimizer->upper_flt, p, p_unscaled);
#else
	for (int i=0;i<minimizer->nparam;i++) p_unscaled[i] = p[i];
#endif		

	// ==================================
	// The below is done in i3_minimizer_wrapper_levmar which calls i3_posterior_wrapper_flt and i3_sersics_likelihood in turn
	// before calling i3_sersics_model_image which is the equivalent to i3_sersics_model_jacobian used below
	i3_model * model = minimizer->model;
	i3_parameter_set * parameter_set = malloc(model->nbytes);
	i3_minimizer_input_varied_parameters(minimizer,p_unscaled,parameter_set);

	
	// prepare the beermatted param set which is done in i3_sersic.c: i3_sersics_likelihood called by i3_minimizer_wrapper_levmar
	i3_sersics_parameter_set * paramset_beermat = i3_sersics_beermat_params(parameter_set,minimizer->options);

	// ==================================

	int sanity_check = 0;
	if(sanity_check){	  
	    // Sanity check: compute Jacobian with finite differences, should give same results like 
	    // calling levmar without analytically computed Jacobian function
	    i3_flt * hxm = malloc(sizeof(i3_flt)*n);
	    i3_flt * hxp = malloc(sizeof(i3_flt)*n);

	    // Depending on sign of levmar_tau either forward or central differences are used
	    i3_flt delta = fabs(minimizer->data_set->options->levmar_tau);

	    // Note that i3_minimizer_wrapper_levmar transforms p into p_unscaled and calls i3_posterior_wrapper_flt which transforms p_unscaled into parameter_set
	    if (minimizer->data_set->options->levmar_tau >= 0.0){
	      i3_minimizer_wrapper_levmar(p, hxm, m, n, minimizer);
	      i3_levmar_fdif_forw_jac_approx(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, minimizer); 
	      }
	    else{	      
	      i3_levmar_fdif_cent_jac_approx(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, minimizer); 
	    }

	    free(hxm);
	    free(hxp);
	  }
	else{        
	  // get the jacobian of the model image    
	  //i3_sersics_model_jacobian_exact(paramset_beermat, minimizer->data_set, jac);
	  i3_sersics_model_jacobian_exact(paramset_beermat, minimizer->data_set, jac);

	  // Weight jacobians according to weight function
	  i3_image * weight = minimizer->data_set->weight;
	  for(int j=0; j<m; j++){
	    for(int i=0; i<n; i++){
	      jac[i*m+j]*=i3_sqrt(weight->data[i]);
	    }	    
	  }

	  // !!!!!!!!! With Tomek's new covariance parametrisation it seems that we don't need this any longer !!!!!!
	  // For small values of e1 and e2 finite fifference approximation seems more stable because of discontinuities of i3_unit_shear_matrix_jac along axes
	  /*
	  i3_flt delta = fabs(minimizer->data_set->options->levmar_tau);
	  if((fabs(paramset_beermat->e1)<delta)||(fabs(paramset_beermat->e2)<delta)){

	    i3_flt * hxm = malloc(sizeof(i3_flt)*n);
	    i3_flt * hxp = malloc(sizeof(i3_flt)*n);	
	     
	    if (minimizer->data_set->options->levmar_tau >= 0.0){
	      i3_minimizer_wrapper_levmar(p, hxm, m, n, minimizer);
	      i3_levmar_fdif_forw_jac_approx_j(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, 2, minimizer); 
	      i3_levmar_fdif_forw_jac_approx_j(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, 3, minimizer); 
	    }
	    else{	      
	      i3_levmar_fdif_cent_jac_approx_j(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, 2, minimizer); 
	      i3_levmar_fdif_cent_jac_approx_j(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac, m, n, 3, minimizer); 
	      }
	     
	    free(hxm);
	    free(hxp);
	  }
	*/

	}

	//Free memory
	free(parameter_set);
	free(paramset_beermat);

// sersics test
#endif
}

static void i3_minimizer_wrapper_levmar_jacobian_vec(i3_flt *p, i3_flt *jac, int m, int n, void *adata){
#if NO_SERSICS
    I3_FATAL("Jacobian code currently assumes sersics model.", 3);
#else

        i3_minimizer * minimizer = (i3_minimizer*) adata; 
        i3_flt p_unscaled[minimizer->nparam];
#if SCALE_TO_UNIT	
	i3_scale_from_unit(minimizer->nparam, minimizer->lower_flt, minimizer->upper_flt, p, p_unscaled);
#else
	for (int i=0;i<minimizer->nparam;i++) p_unscaled[i] = p[i];
#endif		

	// ==================================
	// The below is done in i3_minimizer_wrapper_levmar which calls i3_posterior_wrapper_flt and i3_sersics_likelihood in turn
	// before calling i3_sersics_model_image which is the equivalent to i3_sersics_model_jacobian used below
	i3_model * model = minimizer->model;
	i3_parameter_set * parameter_set = malloc(model->nbytes);
	i3_minimizer_input_varied_parameters(minimizer,p_unscaled,parameter_set);
	
	// prepare the beermatted param set which is done in i3_sersic.c: i3_sersics_likelihood called by i3_minimizer_wrapper_levmar
	i3_sersics_parameter_set * paramset_beermat = i3_sersics_beermat_params(parameter_set,minimizer->options);
	// ==================================

	// get the jacobian of the model image    
	//i3_sersics_model_jacobian_exact(paramset_beermat, minimizer->data_set, jac);
	i3_sersics_model_jacobian_exact_vec(paramset_beermat, minimizer->data_set, jac);

	// Weight jacobians according to weight function
	i3_image * weight = minimizer->data_set->weight;
	for(int j=0; j<m; j++){
	  for(int i=0; i<n; i++){
	    jac[i*m+j]*=i3_sqrt(weight->data[i]);
	  }	    
	}

	//Free memory
	free(parameter_set);
	free(paramset_beermat);
// No sersics loop
#endif
}


void i3_check_jacobian(
    void (*func)(i3_flt *p, i3_flt *hx, int m, int n, void *adata),
    void (*jacf)(i3_flt *p, i3_flt *j,  int m, int n, void *adata),
    i3_flt *p, int m, int n, void *adata, i3_flt tau, i3_flt *err){

  i3_minimizer * minimizer = (i3_minimizer*) adata; 

  //i3_data_set * dataset = (i3_data_set*)adata;
  int nx = minimizer->model_image->nx;
  int ny = minimizer->model_image->ny;

  // Allocate memory
  i3_flt * jac_approx = malloc(sizeof(i3_flt)*m*n);
  i3_flt * jac_exact  = malloc(sizeof(i3_flt)*m*n);

  i3_flt * hxm = malloc(sizeof(i3_flt)*n);
  i3_flt * hxp = malloc(sizeof(i3_flt)*n);

  // Compute finite difference approximation
  i3_flt delta = fabs(minimizer->data_set->options->levmar_tau);
  if (minimizer->data_set->options->levmar_tau >= 0.0){
    // Forward finite difference
    i3_minimizer_wrapper_levmar(p, hxm, m, n, minimizer);	
    i3_levmar_fdif_forw_jac_approx(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac_approx, m, n, minimizer); 
  }else{
    // Central finite difference
    i3_levmar_fdif_cent_jac_approx(i3_minimizer_wrapper_levmar, p, hxm, hxp, delta, jac_approx, m, n, minimizer); 
  }

  // Compute analytic jacobain
  i3_minimizer_wrapper_levmar_jacobian(p, jac_exact, m, n, minimizer);
  
  // =====================================================================
  // Set variable save to true if you want to save Jacobian images to disk
  int save = true;
  if (save == 1){
    
      i3_image * approx = i3_image_create( nx, ny ); i3_image_zero( approx );
      i3_image * exact  = i3_image_create( nx, ny ); i3_image_zero( exact  );

      char fname_approx[20];
      char fname_exact[20];

      register int i;
      register int j;
      register int k;
      register int prm; // choose which component to plot
      for (prm=0; prm<m; ++prm){
	k = 0;
	for (i=0; i<nx; ++i){
	  for (j=0; j<ny; ++j){
	    approx->row[i][j] = jac_approx[k*m+prm];
	    exact->row[i][j]  = jac_exact[k*m+prm];
	    k += 1;
	  }
	}
	sprintf(fname_approx, "jac_approx_%d.fits",prm);
	sprintf(fname_exact, "jac_exact_%d.fits",prm );
	i3_image_save_fits( approx, fname_approx);
	i3_image_save_fits( exact, fname_exact);
      }

      i3_image_destroy(approx);
      i3_image_destroy(exact);
  }
  // =====================================================================
  // Compute difference between numerical and analytical Jacobians
  register int i;
  for(i=0; i<n*m; ++i){
    err[i]=jac_approx[i] - jac_exact[i];
  }	
  
  // Free memory 
  free(jac_approx);
  free(jac_exact);
  free(hxm);
  free(hxp);
}

bool user_has_been_warned_about_eps5 = false;


static i3_flt check_eps5_model(i3_model * model, i3_data_set * data_set)
{
    // if eps5<0 then it does not matter as we will not use it no matter what
    if (data_set->options->levmar_eps5<0) return data_set->options->levmar_eps5;

    // Check for wrong models
    int model_isnt_sersics = strcmp(model->name,"sersics");
    int model_isnt_logsersics = strcmp(model->name,"logsersics");
    int model_isnt_multisersics = strcmp(model->name,"multisersics");
    if (model_isnt_sersics && model_isnt_logsersics && model_isnt_multisersics){
        if (!user_has_been_warned_about_eps5){
            user_has_been_warned_about_eps5=true;
            I3_WARNING("You specified a value of eps5 in the ini file < 0. But your model was not sersics or logsersics.  We will ignore it.");
            return -1.0;
        }
    }

    // Check for fixed parameters
    int parameters_fixed = (
           model->param_fixed[0]==1 
        || model->param_fixed[1]==1 
        || model->param_fixed[2]==1 
        || model->param_fixed[3]==1);

    if (parameters_fixed){
        if (!user_has_been_warned_about_eps5){
            user_has_been_warned_about_eps5=true;
            I3_WARNING("You specified a value of eps5 in the ini file < 0. But your model has fixed x0, y0, e1, or e2.  We will ignore it.");
            return -1.0;
        }
    }

    // Otherwise everything is fine and use the specified value.
    return data_set->options->levmar_eps5;
}


i3_parameter_set * i3_minimizer_run_levmar(i3_minimizer * minimizer, i3_parameter_set * initial_parameter_set, i3_data_set * data_set){
	minimizer->data_set = data_set;
	i3_model_copy_parameters(minimizer->model,minimizer->start,initial_parameter_set);

	minimizer->upper_flt = malloc(sizeof(i3_flt) * minimizer->nparam);
	minimizer->lower_flt = malloc(sizeof(i3_flt) * minimizer->nparam);
        
	i3_flt start[minimizer->nparam];
	i3_flt start_scaled[minimizer->nparam];
	i3_flt lower_scaled[minimizer->nparam];
	i3_flt upper_scaled[minimizer->nparam];
	// i3_flt scale_factor[minimizer->nparam];

	for (int i=0;i<minimizer->nparam;i++){
		lower_scaled[i]=0.0;
		upper_scaled[i]=1.0;
	}



	i3_minimizer_extract_varied_parameters(minimizer,initial_parameter_set,start);
	i3_minimizer_extract_varied_parameters(minimizer, minimizer->model->min, minimizer->lower_flt);
	i3_minimizer_extract_varied_parameters(minimizer, minimizer->model->max, minimizer->upper_flt);


#if SCALE_TO_UNIT
	i3_scale_to_unit(minimizer->nparam, minimizer->lower_flt, minimizer->upper_flt, start, start_scaled);
#else 

	for (int i=0;i<minimizer->nparam;i++) start_scaled[i]=start[i];

#endif

	i3_flt info[LM_INFO_SZ];
	i3_flt levmar_options[LM_OPTS_SZ];

    levmar_options[0]= data_set->options->levmar_LM_INIT_MU;
    levmar_options[1]= data_set->options->levmar_eps1;
	levmar_options[2]= data_set->options->levmar_eps2;
	levmar_options[3]= data_set->options->levmar_eps3;
    levmar_options[4]= data_set->options->levmar_eps4;
    levmar_options[5]= check_eps5_model(minimizer->model,data_set);
    levmar_options[6]= data_set->options->levmar_tau;
    levmar_options[7]= data_set->options->minimizer_verbosity;

	

		// We now run levmar with zero as the input data and 
	i3_image * target_image = i3_image_like(minimizer->model_image);
	i3_image_zero(target_image);
	for (int i=0;i<target_image->n;i++) target_image->data[i] = data_set->image->data[i] * i3_sqrt(data_set->weight->data[i]);

	if (minimizer->verbosity>1) printf("Starting minimizer with mu=% 2.2e\ttau=% 2.2e\teps1=% 2.2e\teps2=% 2.2e\teps3=% 2.2e\tmaxiter=%d\n",levmar_options[0],levmar_options[4],levmar_options[1],levmar_options[2],levmar_options[3],minimizer->max_iterations);


	// =======================================================================================================
	// Check jacobians with finite difference approximation 
        if (data_set->options->levmar_check_analytic_gradients){
       
	  // With plot_e12_diagnostics set True the Frobenius norm is calculated between Jacobian images computed
          // by forward difference approximation and analytic expressions. The difference is evaluated for a grid 
	  // of e1, e2 values between -1.0 and 1.0, N sets the number of intermediate steps 

	  bool plot_e12_diagnostics=0;

	  if (plot_e12_diagnostics){
	    printf("========================================\n");
	    printf("Computing Frobenius norm between Jacobian\n"); 
	    printf("images computed by finite differences\n");
	    printf("and analytic expressions\n");
	    printf("----------------------------------------\n");

            int n_params = minimizer->nparam;
            int n_pix =	 minimizer->model_image->n;
	    i3_flt * err = malloc(sizeof(i3_flt)*n_params*n_pix);
	     
	    int N=10;
	    register int k,l;
	     
	    i3_image * e1_jac_errs = i3_image_create( N+1, N+1 ); i3_image_zero( e1_jac_errs );
	    i3_image * e2_jac_errs = i3_image_create( N+1, N+1 ); i3_image_zero( e2_jac_errs );
	     
	    i3_flt err_tot=0.0;
	    i3_flt errs_tot[n_params];
	    i3_flt errs[n_params];
	    register int i,j;
	     
	    for(k=0; k<=N; ++k){	     
	      start_scaled[2] = -1.0 + (2.0 / N) * k;
	      printf("e1=%f\n",start_scaled[2]);
	     
	      for(l=0; l<=N; ++l){	     
	        start_scaled[3] = -1.0 + (2.0 / N) * l;
	     	     
		i3_check_jacobian(i3_minimizer_wrapper_levmar, i3_minimizer_wrapper_levmar_jacobian, start_scaled, n_params, n_pix, minimizer, minimizer->data_set->options->levmar_tau, err);	    	       
       	       
	        for(j=0; j<n_params; ++j){
	          errs[j] = 0.0;
	          errs_tot[j] = 0.0;
	          for(i=0; i<n_pix; ++i){
	            // Compute max. error
	            if(i3_fabs(err[i*n_params+j] - errs[j]) > 0){
	         	errs[j] = err[i*n_params+j];
	            }
	            // Compute total error
	            err_tot += err[i*n_params+j];
	            errs_tot[j] += err[i*n_params+j];
	          }
	        }
	         
	        e1_jac_errs->row[k][l] = errs_tot[2];
	        e2_jac_errs->row[k][l] = errs_tot[3];       	       
	      }
	    }
	    char fname_e1[40];
	    char fname_e2[40];
	    sprintf(fname_e1, "e1_jac_eps_cent23_forw_errs_%d.fits",N);
	    sprintf(fname_e2, "e2_jac_eps_cent23_forw_errs_%d.fits",N);
	    i3_image_save_fits(e1_jac_errs , fname_e1);
	    i3_image_save_fits(e2_jac_errs , fname_e2);
	    i3_image_destroy(e1_jac_errs);
	    i3_image_destroy(e2_jac_errs);

	    printf("----------------------------------------\n");
	    printf("Jacobian diagnostic images saved to\n %s\n %s\n", fname_e1, fname_e2);
	    printf("========================================\n");	     
	    exit(0);
            return 0;

	  }else{
	    // =====================================================================================================
	    // MH: Check computation of Jacobian with levmar builtin function
            printf("========================================\n");
	    printf("Checking Jacobian with\n");
	     
	    if (minimizer->data_set->options->levmar_tau >= 0.0){
	      printf("forward finite difference approximation\n");
	    }else{
	      printf("central finite difference approximation\n");
	    }

            int n_params = minimizer->nparam;
            int n_pix =	 minimizer->model_image->n;
	    i3_flt * err = malloc(sizeof(i3_flt)*n_params*n_pix);
	    i3_flt delta = fabs(minimizer->data_set->options->levmar_tau);
	     
	    i3_flt err_tot=0.0;
	    i3_flt errs[n_params];
	    register int i,j;
	     
	    i3_check_jacobian(i3_minimizer_wrapper_levmar, i3_minimizer_wrapper_levmar_jacobian, start_scaled, n_params, n_pix, minimizer, delta, err);	      	       
       	       
	    for(j=0; j<n_params; ++j){
	      errs[j] = 0.0;
	      for(i=0; i<n_pix; ++i){
	        // Compute max. error
	        if(i3_fabs(err[i*n_params+j] - errs[j]) > 0)
		  errs[j] = err[i*n_params+j];

	        // Compute total error
	        err_tot += err[i*n_params+j];
	      }
	    }
	     
	    printf("----------------------------------------\n");
	    printf("Max error in x0        %.10f\n", errs[0]); 
	    printf("Max error in y0        %.10f\n", errs[1]); 
	    printf("Max error in e1        %.10f\n", errs[2]); 
	    printf("Max error in e2        %.10f\n", errs[3]); 
	    printf("Max error in radius    %.10f\n", errs[4]); 
	    printf("Max error in bulge_A   %.10f\n", errs[5]); 
	    printf("Max error in disc_A    %.10f\n", errs[6]); 
	     
	    printf("----------------------------------------\n");
	    printf("Total error %f\n", err_tot);
	    printf("========================================\n");

	    exit(0);
	    return 0;
	  }
	}
	// =======================================================================================================

	if(USE_CONSTRAINED_OPTIMIZATION){
	  // Use analytic derivatives
	  if (data_set->options->levmar_use_analytic_gradients){
	    if (data_set->options->verbosity>10) {
	      printf("Use box constrained jac optimization\n");
	    }
	    levmar_bc_der(i3_minimizer_wrapper_levmar,  i3_minimizer_wrapper_levmar_jacobian_vec, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, lower_scaled, upper_scaled, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);
	  }else{
	    // Use numerical derivatives 
	    if (data_set->options->verbosity>10) {
	      printf("Use box constrained approx optimization\n");
	    }
	    levmar_bc_dif(i3_minimizer_wrapper_levmar, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, lower_scaled, upper_scaled, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);}

	}else{
	  if (minimizer->verbosity>3) printf("No optimizer upper/lower limits\n");

	  // Use analytic derivatives
	  if (data_set->options->levmar_use_analytic_gradients){
	    if (data_set->options->verbosity>10) {
	      printf("Use unconstrained jac optimization\n");
	    }
	    levmar_bc_der(i3_minimizer_wrapper_levmar, i3_minimizer_wrapper_levmar_jacobian_vec, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, NULL, NULL, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);
	  }else{
	    // Use numerical derivatives 
	    if (data_set->options->verbosity>10) {
	      printf("Use unconstrained approx optimization\n");
	    }
	    levmar_bc_dif(i3_minimizer_wrapper_levmar, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, NULL, NULL, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);}
	}

	for(int li=0;li<LM_INFO_SZ;li++) data_set->levmar_info[li] = info[li]; 
	// =======================================================================================================
	// If the reason for the levmar to stop is 2, ie. Dp, than jacobian approximated by finite difference seems to yield better results for whatever reason. If maximum number of iterations was not sufficient, Jacobian based minimization has probably failed.
	// This fallback to finite difference approximation seems to work for the following settings on GREAT08 data
	// levmar_eps1=1e-8
	// levmar_eps2=1e-8
	// levmar_eps3=1e-20
	// levmar_eps4=1e-6
	// levmar_tau=1e-8
	// levmar_LM_INIT_MU=1e-6

	//if(data_set->options->levmar_use_analytic_gradients){printf(".\n");};
	int offset;
	  
	if((info[7]==2 || info[7]==3) && (data_set->options->levmar_use_analytic_gradients)){
	  printf("Jacobian minimization Failed\n");
	  if (info[7]==2) offset = 20;
	  else if (info[7]==3) offset = 30;

	  if(USE_CONSTRAINED_OPTIMIZATION){
	      // Use numerical derivatives 
	      if (data_set->options->verbosity>10) {
	        printf("Use box constrained approx optimization\n");
	      }
	      levmar_bc_dif(i3_minimizer_wrapper_levmar, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, lower_scaled, upper_scaled, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);
	   
	  }else{
	    if (minimizer->verbosity>3) printf("No optimizer upper/lower limits\n");
	      // Use numerical derivatives 
	      if (data_set->options->verbosity>10) {
	        printf("Use box constrained approx optimization\n");
	      }
	      levmar_bc_dif(i3_minimizer_wrapper_levmar, start_scaled, target_image->data, minimizer->nparam, minimizer->model_image->n, NULL, NULL, minimizer->max_iterations, levmar_options, info, NULL, minimizer->covariance_estimate, minimizer);}
	   
	  for(int li=0;li<LM_INFO_SZ;li++) data_set->levmar_info[li] = info[li]; 
	  data_set->levmar_info[7] = info[7]+offset;
	}

	// =======================================================================================================


	if (minimizer->verbosity>3) {
		for(int i=0;i<minimizer->nparam;i++){
			printf("C[%d,%d] - %e\n",i,i,minimizer->covariance_estimate[i+minimizer->nparam*i]);
		}
	}
	minimizer->has_covariance_estimate = true;

	if (minimizer->verbosity>2) {
		char * message;
		switch((int) info[7]){
			case 1: 
			message = "stopped by small gradient J^T e";
			break;
			case 2: 
			message = "stopped by small Dp";
			break;
			case 3: 
			message = "stopped by itmax";
			break;
			case 4: 
			message = "singular matrix. Restart from current p with increased mu";
			break;
			case 5: 
			message = "no further error reduction is possible. Restart with increased mu";
			break;
			case 6: 
			message = "stopped by small ||e||_2";
			break;
			case 7: 
			message = "stopped by invalid (i.e. NaN or Inf) func values. This is a user error";
			break;
			case 8: 
			message = "Stopped by small D||e||_2";
			break;
            case 9: 
            message = "Stopped by small D||ellipticity||";
            break;
			default:
			message = "unknown.";
			break;
		}

		printf("Minimizer stopped because: %s\n",message);
		i3_flt norm_start = i3_array_norm(start,minimizer->nparam);
		printf("||J^T e||_inf = % 2.4e (% 2.4e)\n",info[2],levmar_options[1]);
		printf("||Dp||_2 =  % 2.4e  (% 2.4e)\n",info[3]*norm_start,levmar_options[2]);
		printf("||e||_2 = % 2.4e (% 2.4e)\n",info[1],levmar_options[3]);
		printf("D||e||_2 =  % 2.4e  (% 2.4e)\n",info[4],levmar_options[4]);
		printf("mu/max[J^T J]_ii =  % 2.4e\n",info[5]);
		printf("niter =  % 1.0f\n",info[6]);
	}

#if SCALE_TO_UNIT
	i3_scale_from_unit(minimizer->nparam, minimizer->lower_flt, minimizer->upper_flt, start_scaled, start);
#else 
	for (int i=0;i<minimizer->nparam;i++) start[i]=start_scaled[i];

#endif

	i3_parameter_set * p = malloc(minimizer->model->nbytes);
	i3_minimizer_input_varied_parameters(minimizer,start,p);
	minimizer->best_like = data_set->levmar_info[1];
	free(minimizer->upper_flt);
	free(minimizer->lower_flt);
	i3_image_destroy(target_image);

	return p;

}


void i3_minimizer_setup_model_image(i3_minimizer * minimizer, i3_image * data_image){
    if (minimizer->model_image) i3_image_destroy(minimizer->model_image);
    minimizer->model_image = i3_image_like(data_image);
}