#include "i3_fisher.h"
#include "i3_math.h"
#include "stdlib.h"
#include "i3_model.h"
#include "i3_image.h"
#define DERIVATIVE_REL_DELTA 0.005
static i3_image ** setup_image_array(i3_image * template, int n)
{
	i3_image ** images = malloc(sizeof(i3_image*)*n);
	for (int i=0; i<n; i++){
		images[i] = i3_image_like(template);
	}
	return images;
}

static void destroy_image_array(i3_image ** images, int n)
{
	for (int i=0; i<n; i++){
		i3_image_destroy(images[i]);
	}
	free(images);
}


static void derivative_from_images(i3_image * lower, i3_image * central, i3_image * upper, i3_flt delta, i3_image * derivative)
{
	for (int p = 0; p<lower->n; p++){
		i3_flt p_mean = (upper->data[p] + central->data[p] + lower->data[p]) / 3.0;
		i3_flt S_xy = delta*(upper->data[p]-p_mean) - delta*(lower->data[p]-p_mean);
		i3_flt S_xx = 2*delta*delta;
		derivative->data[p] = S_xy/S_xx;
	}
}

i3_flt *  i3_fisher_compute_gaussian(i3_model * model, i3_data_set * data_set, 
	i3_parameter_set * center, i3_parameter_set * width){

	// Extract varied parameters with width
	int nparam;
	i3_flt *x0, *x;
	int nbytes;
	i3_flt * width_array = NULL;

	if (width) {
		nparam = i3_model_number_varied_params_nonzero_width(model, width);
		nbytes = sizeof(i3_flt)*nparam;
		x0 = malloc(nbytes);
		width_array = malloc(nbytes);
		i3_model_extract_varied_nonzero_width_parameters(model, center, width, x0);
		i3_model_extract_varied_nonzero_width_parameters(model, width, width, width_array);
	}	
	else{
		nparam = i3_model_number_varied_params(model);
		nbytes = sizeof(i3_flt)*nparam;
		x0 = malloc(nbytes);
		i3_model_extract_varied_parameters(model, center, x0);
	}

	x = malloc(nbytes);
	memcpy(x, x0, nbytes);
	i3_parameter_set * x_params = malloc(model->nbytes);
	i3_model_copy_parameters(model,  x_params, center);

	//set up model images for each derivative
	// and buffers for holding the calculation bits
	i3_image ** derivatives = setup_image_array(data_set->image, nparam);
	i3_image * lower = i3_image_like(data_set->image);
	i3_image * central = i3_image_like(data_set->image);
	i3_image * upper = i3_image_like(data_set->image);

	//calculate baseline model image.
	i3_model_posterior(model, central, center, data_set);

	// Calculate dmodel/dp for each parameter p
	for (int p=0; p<nparam; p++){
		//decide values at which to get derivative
		i3_flt delta;
		if (width_array) delta = width_array[p]*DERIVATIVE_REL_DELTA;
		else delta = x0[p]*DERIVATIVE_REL_DELTA;
		i3_flt plow  = x0[p]+delta;
		i3_flt phigh = x0[p]-delta;
		memcpy(x, x0, nbytes);
		x[p] = plow;
		i3_model_copy_parameters(model,  x_params, center);
		i3_model_input_varied_nonzero_width_parameters(model, x, x_params, width, center);
		i3_model_posterior(model, upper, x, data_set);
		x[p] = phigh;
		i3_model_input_varied_nonzero_width_parameters(model, x, x_params, width, center);
		i3_model_posterior(model, lower, x, data_set);
		derivative_from_images(lower, central, upper, delta, derivatives[p]);
	}

	i3_flt * fisher = malloc(nparam*nparam*sizeof(i3_flt));
	for (int i=0; i<nparam; i++){
		for (int j=0; j<nparam; j++){
			i3_flt F = 0.0;
			for (int p=0; p<central->n; p++) 
				F += derivatives[i]->data[p] * derivatives[j]->data[p] * data_set->weight->data[p];
			fisher[i*nparam+j] = -F;
		}
	}
	// Do summation with errors
	if (width_array) free(width_array);
	free(x0);
	destroy_image_array(derivatives, nparam);
	I3_WARNING("This function not finished - things unfreed");
	return fisher;
}

void i3_fisher_compute_poisson(i3_model * model, i3_data_set * data_set, i3_parameter_set * x0){
	// Extract varied parameters with width
	// Calculate dmodel/dp for each parameter p
	// Do summation with errors over unmasked pixels

}

i3_parameter_set * i3_fisher_compute_diagonal(i3_model * model, i3_data_set * data_set, i3_parameter_set * center_params, i3_parameter_set * guess_params){
	/* For each parameter*/
	i3_parameter_set * result_params = malloc(model->nbytes);
	i3_parameter_set * x_params = malloc(model->nbytes);
	i3_model_copy_parameters(model,x_params,center_params);

	i3_flt * center = (i3_flt*) center_params;
	i3_flt * result = (i3_flt*) result_params;
	i3_flt * x = (i3_flt*) x_params;
	i3_flt * guess = (i3_flt*) guess_params;
	
	i3_image * model_image = i3_image_copy(data_set->image);
	i3_flt center_like = i3_model_posterior(model,model_image,x_params,data_set);
	i3_flt like;

	for (int p=0;p<model->nparam;p++){
		i3_flt delta_x = guess[p];
		for(;;){
			x[p]=center[p]+delta_x;
			like = i3_model_posterior(model,model_image,x_params,data_set);
			i3_flt delta_like = like-center_like;
			if (delta_like>0){
				/* Initial "center" value was too wrong.  restart*/
				I3_FATAL("FixMe",53);
				/* 
				center[p]=x[p];
				center_like=like;
				delta_x=delta_x/i3_sqrt(2*delta_like);
				continue;
				*/
			}
			if (isnan(delta_x)) I3_FATAL("NaN delta x in fisher code",666);
			if (delta_like==0) {delta_x*=1.25; continue;}
			else delta_x=delta_x/i3_sqrt(-2*delta_like);
			if (((-delta_like)>FISHER_MIN_JUMP_SIZE) && ((-delta_like)<FISHER_MAX_JUMP_SIZE)){
				/* Jump size is okay.  Work out the values from it. */
				result[p]=delta_x;
				x[p]=center[p];
				break;
			}
		}
	}
	
	
	free(x_params);
	i3_image_destroy(model_image);
	return result_params;
}