#include "i3_model_tools.h"
#include "i3_math.h"

#ifdef __APPLE__
#ifdef USE_VECLIB
#include <Accelerate/Accelerate.h>
#endif
#endif

#ifdef USE_MKL
#include "mkl.h"
#ifdef USE_VECLIB
#error Cannot use both veclib and mkl. Choose one.
#endif
#endif

#include "i3_mcmc.h"

#include "gsl/gsl_sf_gamma.h"

#define FABS(x) (((x)>=0.0)? (x) : -(x))

extern i3_flt i3_sersic_kappa(i3_flt n);


void i3_add_real_space_gaussian(i3_image * image, i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0){
	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);

	for(int j=0;j<image->ny;j++){
		for(int i=0;i<image->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt r2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
			image->row[j][i] += A*i3_exp(-r2/ab);
		}
	}
}

/* 
Every source I find for this profile gives a different functional form!
*/
void i3_add_real_space_king_profile(i3_image * image, i3_flt rt, i3_flt rc, i3_flt e1, i3_flt e2, i3_flt k, i3_flt x0, i3_flt y0){
	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	i3_flt rc2 = rc*rc;
	i3_flt rc_m2 = 1.0/rc2;
	i3_flt ft = 1.0/i3_sqrt(1+rt*rt*rc_m2);
	i3_flt rt2 = rt*rt;

	for(int j=0;j<image->ny;j++){
		for(int i=0;i<image->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt r2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
			if (r2<rt2){
				i3_flt fc = 1.0/i3_sqrt(1+r2*rc_m2);
				image->row[j][i] += k*(fc-ft);
			}
		}
	}
	
	
}

void i3_add_real_space_beta_profile(i3_flt rc, i3_flt e1, i3_flt e2, i3_flt A, i3_flt beta, i3_flt x0, i3_flt y0, i3_image * image){
	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	i3_flt rc_m2 = 1.0/(rc*rc);
	i3_flt ind = -3*beta+0.5;

	for(int j=0;j<image->ny;j++){
		for(int i=0;i<image->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt r2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
			image->row[j][i] += A*pow(1+r2*rc_m2, ind);
		}
	}	
}





void i3_add_real_space_sersic(i3_image * image, i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n){

	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	
	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);


	for(int j=0;j<image->ny;j++){
		for(int i=0;i<image->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt r2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
			image->row[j][i] += A*i3_exp(-kappa*i3_pow(r2/ab, sers_exponent));
		}
	}
}

void i3_add_fourier_space_sersic(i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_fourier * fourier_image){

	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix_fourier(e1, e2, &m1, &m2, &m3);
    
    int nx = fourier_image->nx_real;
    int ny = fourier_image->ny_real;

	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			i3_flt ux,uy;
			i3_image_uxuy_from_ij(i, j, nx, ny, &ux, &uy);
			i3_flt u2 = (m1*ux*ux+2*m2*ux*uy+m3*uy*uy);
			fourier_image->row[j][i] += A; //lookup, do x0 y0 offsets e.g. e^k_x*x0;
		}
	}
}


void i3_add_real_space_sersic_truncated_radius(i3_image * image, i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor){

	if (truncation_factor==0) {
		i3_add_real_space_sersic(image, ab, e1, e2, A, x0, y0, sersic_n);
		return;
	}

	i3_flt truncation2 = truncation_factor*truncation_factor;
	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	//SLB: !!! Currently this fails if sersic_n=0 (for some strange reason). Ideally would catch this somewhere
	//printf("\nkappa sersic_n sers_exponent:%f\t%f\t%f\n\n",kappa,sersic_n,sers_exponent);
	
	
	i3_flt m1;
	i3_flt m2;
	i3_flt m3;
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);


	for(int j=0;j<image->ny;j++){
		for(int i=0;i<image->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt x2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy)/ab;
			
			if (x2<truncation2){
				i3_flt ex = -kappa*i3_pow(x2, sers_exponent);
				//printf("\ni j x2 ex A:%i\t%i\t%f\t%f\t%f\t%f\t%f\n\n",i,j,x2,ex,kappa,sers_exponent,A);
				if (ex>-35) image->row[j][i] += A*i3_exp(ex);
			}
		}
	}
}

inline static void square_root_array(i3_flt * input_array, i3_flt * output_array, int n){
#ifdef USE_MKL
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vdSqrt(n,input_array,output_array);
#else
	vsSqrt(n,input_array,output_array);
#endif
#elif defined USE_VECLIB
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vvsqrt(output_array, input_array, &n);
#else
	vvsqrtf(output_array, input_array, &n);
#endif

#else
	for (int i=0;i<n; i++) output_array[i] = i3_sqrt(input_array[i]);
#endif	
}




inline static void exp_array(i3_flt * input_array, i3_flt * output_array, int n){
#ifdef USE_MKL
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vdExp(n,input_array,output_array);
#else
	vsExp(n,input_array,output_array);
#endif
#elif defined USE_VECLIB
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vvexp(output_array, input_array, &n);
#else
	vvexpf(output_array, input_array, &n);
#endif
#else
	for (int i=0;i<n; i++) output_array[i] = i3_exp(input_array[i]);
#endif	
}

inline static void pow_array(i3_flt * input_array, i3_flt * output_array, i3_flt b, int n){
#ifdef USE_MKL
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vdPowx(n,input_array,b,output_array);
#else 
	vsPowx(n,input_array,b,output_array);
#endif
#else
	for (int i=0;i<n; i++) output_array[i] = i3_pow(input_array[i],b);
#endif	
}

inline static void inv_array(i3_flt * input_array, i3_flt * output_array, int n){
#ifdef USE_MKL
#ifdef I3_USE_DOUBLE
	//In-place operations are okay.
	vdInv(n,input_array,output_array);
#else 
	vsInv(n,input_array,output_array);
#endif
#else
	for (int i=0;i<n; i++) output_array[i] = 1./input_array[i];
#endif	
}

static i3_flt * buffer_1 = NULL;
static i3_flt * buffer_2 = NULL;
static i3_flt * buffer_3 = NULL;
static i3_image * image_buffer_1 = NULL;

static void square_distance_buffer(i3_image * image, i3_flt x0, i3_flt y0, i3_flt m1, i3_flt m2, i3_flt m3, i3_flt * buffer)
{
	int p=0;
	int nx = image->nx;
	int ny = image->ny;
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			i3_flt x, y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt x2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
			buffer[p++] = x2;
		}
	}	
	
}

static void jac_square_distance_buffer(i3_image * image, i3_flt x0, i3_flt y0, i3_flt m1, i3_flt m2, i3_flt * buffer)
{
	int p=0;
	int nx = image->nx;
	int ny = image->ny;
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			i3_flt x, y;
			i3_image_xy_from_ij(image, i, j, &x, &y);
			i3_flt dx = x-x0;
			i3_flt dy = y-y0;
			i3_flt x2 = (m1*dx+m2*dy);
			buffer[p++] = x2;
		}
	}	
	
}
void i3_add_real_space_sersic_truncated_radius_upsample_central(i3_image * image, i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor, int n_central_pixels_to_upsample, int n_central_upsampling){

	// If no truncation, revert to original function
	// Or if truncation is so large that it is outside the image range
	if (truncation_factor==0.0 || truncation_factor>i3_sqrt(image->nx*image->nx + image->ny*image->ny)){
		i3_add_real_space_sersic_upsample_central(image, ab,e1,e2,A,x0,y0,sersic_n,n_central_pixels_to_upsample,n_central_upsampling);
		return;
	}


	// Otherwise, a very similar function with a few modifications
	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	
	
	i3_flt m1, m2, m3;
	
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	m1/=ab;
	m2/=ab;
	m3/=ab;
	
	int i_central, j_central;
	i3_image_ij_from_xy(image, x0, y0, &i_central, &j_central);

	int n = image->ny * image->nx;

	buffer_1 = realloc(buffer_1, n*sizeof(i3_flt));
	buffer_2 = realloc(buffer_2, n*sizeof(i3_flt));
	buffer_3 = realloc(buffer_3, n*sizeof(i3_flt));
	
	if (image_buffer_1==NULL || image_buffer_1->nx!=image->nx || image_buffer_1->ny!=image->ny){
		if (image_buffer_1) i3_image_destroy(image_buffer_1);
		image_buffer_1 = i3_image_like(image);
	}
	
	// Pay attention - these next two bits are confusing.
	// The number we are eventaully going to compare to is the value x2
	// which is actually x^T M x, where M includes the factor 1/ab.
	// So we need to divide the truncation radius^2 by the same factor
	// as otherwise the truncation radius would depend on the galaxy radius.
	i3_flt truncation2 = truncation_factor * truncation_factor / ab;
	i3_flt modifiedTruncation2 = truncation2;
	// Okay, to save some time we are going to use buffer_1, which contains a distance 
	// measure x^2, to test whether the pixel is outside the truncation radius.
	// Unfortunately, in one of the three cases below that buffer is re-used
	// and so instrad of containing x^2 instead contains sqrt(sqrt(x2)).
	// So in the bulge case below we also need to modify the truncation scale so it matches
	// the actual distance measure instead of x^2
	// We need to keep truncation2 as well because we use it in the upsampled center below.


	square_distance_buffer(image, x0, y0, m1, m2, m3, buffer_1);
	
	if (sersic_n==1.0) {
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(x2) = (x2)**(0.5/1)
	}
	else if (sersic_n==4.0) { //NB This overwrites buffer_1
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(x2)
		square_root_array(buffer_2,buffer_1,n); //Now buffer 1 contains sqrt(sqrt(x2))
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(sqrt(sqrt(x2))) = (x2)**(0.5/4)
		modifiedTruncation2 = i3_sqrt(i3_sqrt(modifiedTruncation2)); // See explanation above
	}
	else pow_array(buffer_1,buffer_2,sers_exponent,n); //Now buffer 2 contains (x2)**(0.5/n_s)

	i3_flt mKappa = -kappa;
	for (int p=0;p<n;p++) buffer_2[p]*= mKappa; //Now buffer 2 contains -kappa * (x2)**(0.5/n_s)

	exp_array(buffer_2,buffer_3,n); //Now buffer 3 contains exp[-kappa * (x2)**(0.5/n_s)]

	i3_image_zero(image_buffer_1);
	for (int p=0;p<n;p++) {
		if (buffer_1[p]<modifiedTruncation2) {
			image_buffer_1->data[p]=A*buffer_3[p];
		}//Now image_buffer_1 contains A*exp[-kappa * (x2)**(0.5/n_s)]
	}
	// Now do the central upsampling bit.
	// Determine which central pixels to loop over
	int i_min = i_central - n_central_pixels_to_upsample - 1;
	int i_max = i_central + n_central_pixels_to_upsample + 1;
	int j_min = j_central - n_central_pixels_to_upsample - 1;
	int j_max = j_central + n_central_pixels_to_upsample + 1;
	if (i_min<0) i_min = 0;
	if (i_max>image->nx-1) i_max=image->nx-1;
	if (j_min<0) j_min = 0;
	if (j_max>image->ny-1) j_max=image->ny-1;

	// First loop over the subpixels
	for(int j=j_min;j<j_max;j++){
		for(int i=i_min;i<i_max;i++){
			//Check first if the pixel is actaually inside the upsampled region.
			//Not sure this is necessary?
			if((abs(i - i_central) < n_central_pixels_to_upsample) && (abs(j - j_central) < n_central_pixels_to_upsample)){
				// We have set this pixel already to the (non-double-upsampled value, so zero it)
				image_buffer_1->row[j][i] = 0.0;
				int upsample_count=0; // Because of the truncation we now cannot assume that all
				                      // the pixels in the upsampled region are actually used
				                      // Though this is certainly unlikely
				// Now loop over the sub-sub pixels.
				for(int j_sub  = 0; j_sub <n_central_upsampling; j_sub++){
					for(int i_sub  = 0; i_sub <n_central_upsampling; i_sub++){
						i3_flt x, y;
						i3_image_xy_from_ij_subpix(image, i, j, i_sub, j_sub, n_central_upsampling, &x, &y);
						
						i3_flt dx = x-x0;
						i3_flt dy = y-y0;
						
						i3_flt x2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
						if (x2>truncation2) continue;
						upsample_count++;
						i3_flt ex = -kappa*i3_pow(x2, sers_exponent);
						image_buffer_1->row[j][i] += A*i3_exp(ex);
					}
				}
				// Divide by the number of sub-sub-pixels contributing to the sub-pixel
				image_buffer_1->row[j][i] /= upsample_count;  
			}
		}
	}
	
	//Add this component into the result.
	i3_image_add_image_into(image, image_buffer_1);


}


void i3_add_real_space_sersic_upsample_central(i3_image * image, i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, int n_central_pixels_to_upsample, int n_central_upsampling){
//void i3_add_real_space_sersic_truncated_radius_upsample_central(i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor, int n_central_pixels_to_upsample, int n_central_upsampling,  i3_image * image){


	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	
	
	i3_flt m1, m2, m3;
	
	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	m1/=ab;
	m2/=ab;
	m3/=ab;
	
	int i_central, j_central;
	i3_image_ij_from_xy(image, x0, y0, &i_central, &j_central);

	i3_flt n_central_upsampling2 = n_central_upsampling*n_central_upsampling;

	int n = image->ny * image->nx;

	buffer_1 = realloc(buffer_1, n*sizeof(i3_flt));
	buffer_2 = realloc(buffer_2, n*sizeof(i3_flt));
	buffer_3 = realloc(buffer_3, n*sizeof(i3_flt));
	
	if (image_buffer_1==NULL || image_buffer_1->nx!=image->nx || image_buffer_1->ny!=image->ny){
		if (image_buffer_1) i3_image_destroy(image_buffer_1);
		image_buffer_1 = i3_image_like(image);
	}
	


	square_distance_buffer(image, x0, y0, m1, m2, m3, buffer_1);
	
	if (sersic_n==1.0) {
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(x2) = (x2)**(0.5/1)
	}
	else if (sersic_n==4.0) { //NB This overwrites buffer_1
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(x2)
		square_root_array(buffer_2,buffer_1,n); //Now buffer 1 contains sqrt(sqrt(x2))
		square_root_array(buffer_1,buffer_2,n); //Now buffer 2 contains sqrt(sqrt(sqrt(x2))) = (x2)**(0.5/4)
	}
	else pow_array(buffer_1,buffer_2,sers_exponent,n); //Now buffer 2 contains (x2)**(0.5/n_s)

	i3_flt mKappa = -kappa;
	for (int p=0;p<n;p++) buffer_2[p]*= mKappa; //Now buffer 2 contains -kappa * (x2)**(0.5/n_s)

	exp_array(buffer_2,buffer_3,n); //Now buffer 3 contains exp[-kappa * (x2)**(0.5/n_s)]

	for (int p=0;p<n;p++) image_buffer_1->data[p]=A*buffer_3[p];//Now image_buffer_2 contains A*exp[-kappa * (x2)**(0.5/n_s)]

	// Now do the central upsampling bit.
	// Can we re-order this loop so that we just loop over the central pixels?  Or is that not possible?
	int i_min = i_central - n_central_pixels_to_upsample - 1;
	int i_max = i_central + n_central_pixels_to_upsample + 1;
	int j_min = j_central - n_central_pixels_to_upsample - 1;
	int j_max = j_central + n_central_pixels_to_upsample + 1;
	if (i_min<0) i_min = 0;
	if (i_max>image->nx-1) i_max=image->nx-1;
	if (j_min<0) j_min = 0;
	if (j_max>image->ny-1) j_max=image->ny-1;
	for(int j=j_min;j<j_max;j++){
		for(int i=i_min;i<i_max;i++){
			if((abs(i - i_central) < n_central_pixels_to_upsample) && (abs(j - j_central) < n_central_pixels_to_upsample)){
				image_buffer_1->row[j][i] = 0.0;
				for(int j_sub  = 0; j_sub <n_central_upsampling; j_sub++){
					for(int i_sub  = 0; i_sub <n_central_upsampling; i_sub++){
						i3_flt x, y;
						i3_image_xy_from_ij_subpix(image, i, j, i_sub, j_sub, n_central_upsampling, &x, &y);
						
						i3_flt dx = x-x0;
						i3_flt dy = y-y0;
						
						i3_flt x2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
						i3_flt ex = -kappa*i3_pow(x2, sers_exponent);
						image_buffer_1->row[j][i] += A*i3_exp(ex) / n_central_upsampling2;
					}
				}
			}
		}
	}
	
	//Add this component into the result.
	i3_image_add_image_into(image, image_buffer_1);
	
}

// The below implementation computes all derivatives except those for e1 and e2 analytically. 
// The Jacobians in e1 and e2 are approximated via finite differences
void i3_add_real_space_sersic_truncated_radius_upsample_central_jac(i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor, int n_central_pixels_to_upsample, int n_central_upsampling,  i3_image ** image){

	i3_flt truncation2 = truncation_factor*truncation_factor;

	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	i3_flt sersic_unchanged;

	i3_flt sersic_unchanged_de1;
	i3_flt sersic_unchanged_de2;

	// Initialize and compute unit shear matrix and its Jacobian
	i3_flt  m1,     m2,     m3;
	
	// Commented out for now until gradient wrt e1/e2 work properly
	//i3_flt dm1_e1, dm2_e1, dm3_e1;
	//i3_flt dm1_e2, dm2_e2, dm3_e2;

	i3_flt m1_de1, m2_de1, m3_de1;
	i3_flt m1_de2, m2_de2, m3_de2;

	i3_flt delta = 1e-6;
	i3_flt d1=1E-04*e1; // force evaluation
	d1=FABS(d1);
	if(d1<delta)
	  d1=delta;

	i3_flt d2=1E-04*e2; // force evaluation
	d2=FABS(d2);
	if(d2<delta)
	  d2=delta;

	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	i3_unit_shear_matrix(e1+d1, e2, &m1_de1, &m2_de1, &m3_de1);
	i3_unit_shear_matrix(e1, e2+d2, &m1_de2, &m2_de2, &m3_de2);

	// Commented out for now until gradient wrt e1/e2 work properly
	// i3_unit_shear_matrix_jac(e1, e2, &dm1_e1, &dm2_e1, &dm3_e1, &dm1_e2, &dm2_e2, &dm3_e2);
	
	//printf("e1=%f\n e2=%f\n dm1_e1=%f\n dm2_e1=%f\n dm3_e1=%f\n dm1_e2=%f\n dm2_e2=%f\n dm3_e2=%f\n",e1, e2, dm1_e1, dm2_e1, dm3_e1,dm1_e2, dm2_e2, dm3_e2);

	int i_central, j_central;
	i3_image_ij_from_xy(image[0], x0, y0, &i_central, &j_central);
	
	i3_flt n_central_upsampling2 = n_central_upsampling*n_central_upsampling;

	for(int j=0;j<image[0]->ny;j++){
	  for(int i=0;i<image[0]->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image[0], i, j, &x, &y);
			
			// this finds if we are within n_central_pixels_to_upsample from the central pixel
				
			if((abs(i - i_central) < n_central_pixels_to_upsample) && (abs(j - j_central) < n_central_pixels_to_upsample)){
				
			// divide the pixel into n_upsampling * n_upsampling pixels, and compute their average
				//printf("I am upsampling pixel %d %d\n",i,j);
				
				for(int j_sub  = 0; j_sub <n_central_upsampling; j_sub++){
					for(int i_sub  = 0; i_sub <n_central_upsampling; i_sub++){

						i3_image_xy_from_ij_subpix(image[0], i, j, i_sub, j_sub, n_central_upsampling, &x, &y);
						
						i3_flt dx = x-x0;
						i3_flt dy = y-y0;
												
						i3_flt x2     = ( m1   *dx*dx+2* m2   *dx*dy+ m3   *dy*dy)/ab;
						i3_flt x2_de1 = (m1_de1*dx*dx+2*m2_de1*dx*dy+m3_de1*dy*dy)/ab;
						i3_flt x2_de2 = (m1_de2*dx*dx+2*m2_de2*dx*dy+m3_de2*dy*dy)/ab;
						
						// Commented out for now until gradient wrt e1/e2 work properly
						// i3_flt dx2_e1 = (dm1_e1*dx*dx+2*dm2_e1*dx*dy+dm3_e1*dy*dy)/ab; 
						// i3_flt dx2_e2 = (dm1_e2*dx*dx+2*dm2_e2*dx*dy+dm3_e2*dy*dy)/ab; 

						i3_flt dx0 = (m1*dx+m2*dy)/ab;
						i3_flt dy0 = (m3*dy+m2*dx)/ab;

						if (x2<truncation2){

						  i3_flt ex     = -kappa*i3_pow(x2    , sers_exponent);
						  i3_flt ex_de1 = -kappa*i3_pow(x2_de1, sers_exponent);
						  i3_flt ex_de2 = -kappa*i3_pow(x2_de2, sers_exponent);
						  //if (ex>-35) 
						  sersic_unchanged     = A*i3_exp(ex)     / n_central_upsampling2;
						  sersic_unchanged_de1 = A*i3_exp(ex_de1) / n_central_upsampling2;
						  sersic_unchanged_de2 = A*i3_exp(ex_de2) / n_central_upsampling2;
						  
						  i3_flt dx2;
						  if (sers_exponent == 1)
						    dx2 = 1.0;
						  else						     
						    dx2 = i3_pow(x2, sers_exponent-1.0);

						  i3_flt dex = -kappa*sers_exponent*dx2 * sersic_unchanged;

						  i3_flt jac_x0_ij = -2 * dx0 * dex;
						  i3_flt jac_y0_ij = -2 * dy0 * dex;
						  // Commented out for now until gradient wrt e1/e2 work properly
						  //i3_flt jac_e1_ij =   dx2_e1 * dex;
						  //i3_flt jac_e2_ij =   dx2_e2 * dex;
						  //i3_flt jac_e1_ij = sersic_unchanged_de1/delta; 
						  //i3_flt jac_e2_ij = sersic_unchanged_de2/delta;   
						  i3_flt jac_e1_ij = (sersic_unchanged_de1-sersic_unchanged)/d1; 
						  i3_flt jac_e2_ij = (sersic_unchanged_de2-sersic_unchanged)/d2;   
						  i3_flt jac_ab_ij = -x2 / ab * dex;
						  i3_flt jac_A_ij  = sersic_unchanged;

						  image[0]->row[j][i] += jac_x0_ij;	
						  image[1]->row[j][i] += jac_y0_ij;	
						  image[2]->row[j][i] += jac_e1_ij;	
						  image[3]->row[j][i] += jac_e2_ij;	
						  image[4]->row[j][i] += jac_ab_ij;	
						  image[5]->row[j][i] += jac_A_ij;	
						  image[6]->row[j][i] += jac_A_ij;	
						}
					}
				}
				
			}
			else{ // this pixel will not be upsampled
				i3_flt dx = x-x0;
				i3_flt dy = y-y0;

				i3_flt x2     = (m1    *dx*dx+2*m2    *dx*dy+m3    *dy*dy)/ab;
				i3_flt x2_de1 = (m1_de1*dx*dx+2*m2_de1*dx*dy+m3_de1*dy*dy)/ab;
				i3_flt x2_de2 = (m1_de2*dx*dx+2*m2_de2*dx*dy+m3_de2*dy*dy)/ab;
						
				// Commented out for now until gradient wrt e1/e2 work properly
				// i3_flt dx2_e1 = (dm1_e1*dx*dx+2*dm2_e1*dx*dy+dm3_e1*dy*dy)/ab; 
				// i3_flt dx2_e2 = (dm1_e2*dx*dx+2*dm2_e2*dx*dy+dm3_e2*dy*dy)/ab; 

				i3_flt dx0 = (m1*dx+m2*dy)/ab;
				i3_flt dy0 = (m3*dy+m2*dx)/ab;
				
				if (x2<truncation2){
				  i3_flt ex     = -kappa*i3_pow(x2,    sers_exponent);
				  i3_flt ex_de1 = -kappa*i3_pow(x2_de1, sers_exponent);
				  i3_flt ex_de2 = -kappa*i3_pow(x2_de2, sers_exponent);
				  //if (ex>-35) 
				  sersic_unchanged     = A*i3_exp(ex);
				  sersic_unchanged_de1 = A*i3_exp(ex_de1);				 
				  sersic_unchanged_de2 = A*i3_exp(ex_de2);
				  
				  i3_flt dx2;
				  if (sers_exponent == 1)
				    dx2 = 1.0;
				  else						     
				    dx2 = i3_pow(x2, sers_exponent-1.0);
				  
				  i3_flt dex = -kappa*sers_exponent*dx2 * sersic_unchanged;				  

				  i3_flt jac_x0_ij = -2 * dx0 * dex;
				  i3_flt jac_y0_ij = -2 * dy0 * dex;
				  // Commented out for now until gradient wrt e1/e2 work properly
				  //i3_flt jac_e1_ij =   dx2_e1 * dex;
				  //i3_flt jac_e2_ij =   dx2_e2 * dex;
				  i3_flt jac_e1_ij = (sersic_unchanged_de1-sersic_unchanged)/delta; 
				  i3_flt jac_e2_ij = (sersic_unchanged_de2-sersic_unchanged)/delta;   
				  i3_flt jac_ab_ij = -x2 / ab * dex;
				  i3_flt jac_A_ij  = sersic_unchanged;
						  
				  image[0]->row[j][i] += jac_x0_ij;	
				  image[1]->row[j][i] += jac_y0_ij;	
				  image[2]->row[j][i] += jac_e1_ij;	
				  image[3]->row[j][i] += jac_e2_ij;	
				  image[4]->row[j][i] += jac_ab_ij;	
				  image[5]->row[j][i] += jac_A_ij;	
				  image[6]->row[j][i] += jac_A_ij;	
				}			       
				
			}
		}
	}
	
}


// Full analytic computation of Jacobians
void i3_add_real_space_sersic_truncated_radius_upsample_central_jac_exact(i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor, int n_central_pixels_to_upsample, int n_central_upsampling,  i3_image ** image){

	i3_flt truncation2 = truncation_factor*truncation_factor;

	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;
	i3_flt sersic_unchanged;

	// Initialize and compute unit shear matrix and its Jacobian
	i3_flt  m1, m2, m3; 
	i3_flt dm1_e1, dm2_e1, dm3_e1;
	i3_flt dm1_e2, dm2_e2, dm3_e2;

	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	i3_unit_shear_matrix_jac(e1, e2, &dm1_e1, &dm2_e1, &dm3_e1, &dm1_e2, &dm2_e2, &dm3_e2);
	
	//printf("e1=%f\n e2=%f\n dm1_e1=%f\n dm2_e1=%f\n dm3_e1=%f\n dm1_e2=%f\n dm2_e2=%f\n dm3_e2=%f\n",e1, e2, dm1_e1, dm2_e1, dm3_e1,dm1_e2, dm2_e2, dm3_e2);

	int i_central, j_central;
	i3_image_ij_from_xy(image[0], x0, y0, &i_central, &j_central);
	
	i3_flt n_central_upsampling2 = n_central_upsampling*n_central_upsampling;

	for(int j=0;j<image[0]->ny;j++){
	  for(int i=0;i<image[0]->nx;i++){
			i3_flt x,y;
			i3_image_xy_from_ij(image[0], i, j, &x, &y);
			
			// this finds if we are within n_central_pixels_to_upsample from the central pixel
				
			if((abs(i - i_central) < n_central_pixels_to_upsample) && (abs(j - j_central) < n_central_pixels_to_upsample)){
				
			// divide the pixel into n_upsampling * n_upsampling pixels, and compute their average
				//printf("I am upsampling pixel %d %d\n",i,j);
				
				for(int j_sub  = 0; j_sub <n_central_upsampling; j_sub++){
					for(int i_sub  = 0; i_sub <n_central_upsampling; i_sub++){

						i3_image_xy_from_ij_subpix(image[0], i, j, i_sub, j_sub, n_central_upsampling, &x, &y);
						
						i3_flt dx = x-x0;
						i3_flt dy = y-y0;
												
						i3_flt x2     = ( m1   *dx*dx+2* m2   *dx*dy+ m3   *dy*dy)/ab;						
						i3_flt dx2_e1 = (dm1_e1*dx*dx+2*dm2_e1*dx*dy+dm3_e1*dy*dy)/ab; 
						i3_flt dx2_e2 = (dm1_e2*dx*dx+2*dm2_e2*dx*dy+dm3_e2*dy*dy)/ab; 

						i3_flt dx0 = (m1*dx+m2*dy)/ab;
						i3_flt dy0 = (m3*dy+m2*dx)/ab;

						if (x2<truncation2){

						  i3_flt ex;
						  if (sers_exponent == 0.5)
						    ex = i3_sqrt(x2);
						  else
						    ex = i3_pow(x2, sers_exponent);

						  //if (ex>-35) 
						  sersic_unchanged = A*i3_exp(-kappa*ex) / n_central_upsampling2;
						  
						  i3_flt dx2;
						  if (sers_exponent == 1)
						    dx2 = 1.0;
						  else if (sers_exponent == 0.5)
						    dx2 = 1.0 / ex; 
						  else						     
						    dx2 = i3_pow(x2, sers_exponent-1.0);

						  i3_flt dex = -kappa*sers_exponent*dx2 * sersic_unchanged;

						  i3_flt jac_x0_ij = -2 * dx0 * dex;
						  i3_flt jac_y0_ij = -2 * dy0 * dex;
						  i3_flt jac_e1_ij =   dx2_e1 * dex;
						  i3_flt jac_e2_ij =   dx2_e2 * dex;
						  i3_flt jac_ab_ij = -x2 / ab * dex;
						  i3_flt jac_A_ij  = sersic_unchanged;

						  image[0]->row[j][i] += jac_x0_ij;	
						  image[1]->row[j][i] += jac_y0_ij;	
						  image[2]->row[j][i] += jac_e1_ij;	
						  image[3]->row[j][i] += jac_e2_ij;	
						  image[4]->row[j][i] += jac_ab_ij;	
						  image[5]->row[j][i] += jac_A_ij;	
						  image[6]->row[j][i] += jac_A_ij;	
						}
					}
				}
				
			}
			else{ // this pixel will not be upsampled
				i3_flt dx = x-x0;
				i3_flt dy = y-y0;

				i3_flt x2     = (m1    *dx*dx+2*m2    *dx*dy+m3    *dy*dy)/ab;
				i3_flt dx2_e1 = (dm1_e1*dx*dx+2*dm2_e1*dx*dy+dm3_e1*dy*dy)/ab; 
			        i3_flt dx2_e2 = (dm1_e2*dx*dx+2*dm2_e2*dx*dy+dm3_e2*dy*dy)/ab; 

				i3_flt dx0 = (m1*dx+m2*dy)/ab;
				i3_flt dy0 = (m3*dy+m2*dx)/ab;
				
				if (x2<truncation2){

				  i3_flt ex;
				  if (sers_exponent == 0.5)
				    ex = i3_sqrt(x2);
				  else
				    ex = i3_pow(x2, sers_exponent);

				  //if (ex>-35) 
				  sersic_unchanged = A*i3_exp(-kappa*ex);
				  
				  i3_flt dx2;
				  if (sers_exponent == 1)
				    dx2 = 1.0;
				  else if (sers_exponent == 0.5)
				    dx2 = 1.0 / ex;
				  else						     
				    dx2 = i3_pow(x2, sers_exponent-1.0);
				  
				  i3_flt dex = -kappa*sers_exponent*dx2 * sersic_unchanged;

				  i3_flt jac_x0_ij = -2 * dx0 * dex;
				  i3_flt jac_y0_ij = -2 * dy0 * dex;
				  i3_flt jac_e1_ij =   dx2_e1 * dex;
				  i3_flt jac_e2_ij =   dx2_e2 * dex;
				  i3_flt jac_ab_ij = -x2 / ab * dex;
				  i3_flt jac_A_ij  = sersic_unchanged;
						  
				  image[0]->row[j][i] += jac_x0_ij;	
				  image[1]->row[j][i] += jac_y0_ij;	
				  image[2]->row[j][i] += jac_e1_ij;	
				  image[3]->row[j][i] += jac_e2_ij;	
				  image[4]->row[j][i] += jac_ab_ij;	
				  image[5]->row[j][i] += jac_A_ij;	
				  image[6]->row[j][i] += jac_A_ij;	
				}			       
				
			}
		}
	}
	
}

static i3_flt * x2 = NULL;
static i3_flt *	ex = NULL;
static i3_flt *	dex = NULL;
static i3_flt *	dx0 = NULL;
static i3_flt *	dy0 = NULL;
static i3_flt *	dx2_e1 = NULL;
static i3_flt *	dx2_e2 = NULL;
static i3_flt *	sersic_unchanged = NULL;

void i3_add_real_space_sersic_truncated_radius_upsample_central_jac_exact_vec(i3_flt ab,i3_flt e1,i3_flt e2,i3_flt A,i3_flt x0,i3_flt y0,i3_flt sersic_n, i3_flt truncation_factor, int n_central_pixels_to_upsample, int n_central_upsampling,  i3_image ** image){

	// i3_flt truncation2 = truncation_factor*truncation_factor;

	i3_flt kappa = i3_sersic_kappa(sersic_n);
	i3_flt sers_exponent = 0.5 / sersic_n;

	// Initialize and compute unit shear matrix and its Jacobian
	i3_flt  m1, m2, m3; 
	i3_flt dm1_e1, dm2_e1, dm3_e1;
	i3_flt dm1_e2, dm2_e2, dm3_e2;

	i3_unit_shear_matrix(e1, e2, &m1, &m2, &m3);
	i3_unit_shear_matrix_jac(e1, e2, &dm1_e1, &dm2_e1, &dm3_e1, &dm1_e2, &dm2_e2, &dm3_e2);

	m1 /= ab; dm1_e1 /= ab; dm1_e2 /= ab;
	m2 /= ab; dm2_e1 /= ab; dm2_e2 /= ab;
	m3 /= ab; dm3_e1 /= ab; dm3_e2 /= ab;	

	int i_central, j_central;
	i3_image_ij_from_xy(image[0], x0, y0, &i_central, &j_central);
	
	i3_flt n_central_upsampling2 = n_central_upsampling*n_central_upsampling;

	int n = image[0]->ny * image[0]->nx;

	x2  = realloc(x2 , n*sizeof(i3_flt));
	ex  = realloc(ex , n*sizeof(i3_flt));
	dex = realloc(dex, n*sizeof(i3_flt));
	dx0 = realloc(dx0, n*sizeof(i3_flt));
	dy0 = realloc(dy0, n*sizeof(i3_flt));
	dx2_e1 = realloc(dx2_e1 , n*sizeof(i3_flt));
	dx2_e2 = realloc(dx2_e2 , n*sizeof(i3_flt));
	sersic_unchanged = realloc(sersic_unchanged, n*sizeof(i3_flt));
	
	// Computation of Mahalanobis distance arrays					      
	square_distance_buffer(image[0], x0, y0, m1, m2, m3, x2);
	square_distance_buffer(image[0], x0, y0, dm1_e1, dm2_e1, dm3_e1, dx2_e1);
	square_distance_buffer(image[0], x0, y0, dm1_e2, dm2_e2, dm3_e2, dx2_e2);

	jac_square_distance_buffer(image[0], x0, y0, m1, m2, dx0);
	jac_square_distance_buffer(image[0], x0, y0, m2, m3, dy0);

	if (sersic_n==0.5){
	  for (int p=0;p<n;p++){
	    ex[p]  = x2[p]; //Now ex contains x2 = (x2)**(0.5/0.5)
	    dex[p] = 1.0;   //Now dex contains d(ex)/dx2 = dx2/dx2 = 1.0
	  }
	}
	else if (sersic_n==1.0){
	  square_root_array(x2, ex, n); //Now ex contains sqrt(x2) = (x2)**(0.5/1)
	  inv_array(ex, dex, n);        //Now dex contains d(ex)/dx2 = 1/sqrt(x2) = (x2)**[(0.5/1)-1]
	}
	else{
	  pow_array(x2,  ex, sers_exponent    ,n); //Now ex contains (x2)**(0.5/n_s)
	  pow_array(x2, dex, sers_exponent-1.0,n); //Now dex contains (x2)**[(0.5/n_s)-1]
	}

	i3_flt mKappa = -kappa;
	for (int p=0;p<n;p++) ex[p]*= mKappa; //Now ex contains -kappa * (x2)**(0.5/n_s)

	exp_array(ex,sersic_unchanged,n); //Now sersic_unchanged contains exp[-kappa * (x2)**(0.5/n_s)]
	
	i3_flt dsersic_unchanged;
	for (int p=0;p<n;p++){
	  sersic_unchanged[p] *= A; //Now sersic_unchanged contains A*exp[-kappa * (x2)**(0.5/n_s)]
	  dsersic_unchanged = mKappa * sers_exponent * dex[p] * sersic_unchanged[p];
	  	  
	  image[0]->data[p] += -2 * dx0[p] * dsersic_unchanged;
	  image[1]->data[p] += -2 * dy0[p] * dsersic_unchanged;
	  image[2]->data[p] +=	 dx2_e1[p] * dsersic_unchanged;
	  image[3]->data[p] +=	 dx2_e2[p] * dsersic_unchanged;
	  image[4]->data[p] += -x2[p] / ab * dsersic_unchanged;
	  image[5]->data[p] += sersic_unchanged[p];
	  image[6]->data[p] += sersic_unchanged[p];
	}

	// Now do the central upsampling bit.
	// Can we re-order this loop so that we just loop over the central pixels?  Or is that not possible?
	int i_min = i_central - n_central_pixels_to_upsample - 1;
	int i_max = i_central + n_central_pixels_to_upsample + 1;
	int j_min = j_central - n_central_pixels_to_upsample - 1;
	int j_max = j_central + n_central_pixels_to_upsample + 1;
	if (i_min<0) i_min = 0;
	if (i_max>image[0]->nx-1) i_max=image[0]->nx-1;
	if (j_min<0) j_min = 0;
	if (j_max>image[0]->ny-1) j_max=image[0]->ny-1;
	for(int j=j_min;j<j_max;j++){
		for(int i=i_min;i<i_max;i++){
			if((abs(i - i_central) < n_central_pixels_to_upsample) && (abs(j - j_central) < n_central_pixels_to_upsample)){
			  
			  for(int p=0;p<7;p++) image[p]->row[j][i] = 0.0;

				for(int j_sub  = 0; j_sub <n_central_upsampling; j_sub++){
					for(int i_sub  = 0; i_sub <n_central_upsampling; i_sub++){
						i3_flt x, y;
						i3_image_xy_from_ij_subpix(image[0], i, j, i_sub, j_sub, n_central_upsampling, &x, &y);
						
						i3_flt dx = x-x0;
						i3_flt dy = y-y0;
						
						i3_flt x2 = (m1*dx*dx+2*m2*dx*dy+m3*dy*dy);
						i3_flt dx2_e1 = (dm1_e1*dx*dx+2*dm2_e1*dx*dy+dm3_e1*dy*dy);
						i3_flt dx2_e2 = (dm1_e2*dx*dx+2*dm2_e2*dx*dy+dm3_e2*dy*dy);

						i3_flt dx0 = (m1*dx+m2*dy);					
						i3_flt dy0 = (m3*dy+m2*dx);
					       
						i3_flt ex;
						i3_flt dex;

						if (sers_exponent == 0.5){
						  ex  = i3_sqrt(x2);
						  dex = 1.0 / ex; 
						}
						else if (sers_exponent == 1){
						  ex  = x2;
						  dex = 1.0;
						}
						else{
						  ex  = i3_pow(x2, sers_exponent);
						  dex = i3_pow(x2, sers_exponent-1.0);
						}
						 
						i3_flt sersic_unchanged = A*i3_exp(-kappa*ex) / n_central_upsampling2;
						  
						i3_flt dsersic_unchanged = -kappa*sers_exponent*dex * sersic_unchanged;

						image[0]->row[j][i] += -2 * dx0 * dsersic_unchanged;
						image[1]->row[j][i] += -2 * dy0 * dsersic_unchanged;
						image[2]->row[j][i] +=	 dx2_e1 * dsersic_unchanged;
						image[3]->row[j][i] +=	 dx2_e2 * dsersic_unchanged;
						image[4]->row[j][i] += -x2 / ab * dsersic_unchanged;
						image[5]->row[j][i] += sersic_unchanged;
						image[6]->row[j][i] += sersic_unchanged;
					}
				}
			}
		}
	}
}

i3_flt i3_weighted_residual_variance(i3_image * model_image, i3_image * observed_image, i3_image * weight_map)
{
	i3_flt mu_m = 0.0;
	i3_flt mu_d = 0.0;
	i3_flt w_sum = 0.0;
	for(int pixel=0;pixel<model_image->n;pixel++){
		mu_m += model_image->data[pixel] * weight_map->data[pixel];
		mu_d += observed_image->data[pixel] * weight_map->data[pixel];
		w_sum += weight_map->data[pixel];
	}

	mu_m /= w_sum;
	mu_d /= w_sum;
	
	i3_flt mu_r = mu_m - mu_d;
	i3_flt s2 = 0.0;
	for(int pixel=0;pixel<model_image->n;pixel++){
		s2 += pow(model_image->data[pixel] - observed_image->data[pixel] - mu_r, 2) * weight_map->data[pixel];
	}

	return s2/w_sum;

}

i3_flt i3_chi2_weight_map(i3_image * model_image, i3_image * observed_image, i3_image * weight_image){
	i3_flt chi2=0;
	i3_flt * predicted = model_image->data;
	i3_flt * observed = observed_image->data;
	i3_flt * weight = weight_image->data;
	for(int pixel=0;pixel<model_image->n;pixel++){
		i3_flt diff = predicted[pixel]-observed[pixel]; 
		chi2+= diff*diff*weight[pixel];
	}
	return chi2;
}

i3_flt i3_chi2_white_noise(i3_image * model_image, i3_image * observed_image, i3_flt noise_std){
	i3_flt chi2=0;
	i3_flt noise_var= noise_std*noise_std;
	i3_flt * predicted = model_image->data;
	i3_flt * observed = observed_image->data;
	for(int pixel=0;pixel<model_image->n;pixel++){
		i3_flt diff = predicted[pixel]-observed[pixel]; 
		chi2+= diff*diff;
	}
	return chi2/noise_var;

}

i3_flt i3_chi2_wn_reduced(i3_image * model_image, i3_image * observed_image, i3_flt noise_std){
	i3_flt chi2=0;
	i3_flt noise_var= noise_std*noise_std;
	i3_flt * predicted = model_image->data;
	i3_flt * observed = observed_image->data;
	for(int pixel=0;pixel<model_image->n;pixel++){
		chi2 += predicted[pixel]*predicted[pixel] + 2.*observed[pixel]*predicted[pixel]; 
	}
	return chi2/noise_var/2.;

}

i3_flt i3_pixel_shift_image(i3_image * image, i3_image * shifted, int dx, int dy)
{
	i3_image_zero(shifted);
	for(int j=0; j<shifted->ny; j++){
		if ((j-dy<0) || (j-dy>=image->ny)) continue;
		for(int i=0; i<shifted->nx; i++){
			if ((i-dx<0) || (i-dx>=image->nx)) continue;
			shifted->row[j][i] = image->row[j-dy][i-dx];
		}
	}
}

i3_flt i3_logL_weight_map_marginalize_amplitude(i3_image * model_image, i3_image * observed_image, i3_image * weight_image){
	i3_flt * model = model_image->data;
	i3_flt * observed = observed_image->data;
	i3_flt * weight = weight_image->data;
	
	i3_flt sum_mi = 0.0; 
	i3_flt sum_ii = 0.0; 
	i3_flt sum_mm = 0.0;
	
	for(int pixel=0;pixel<model_image->n;pixel++){
		sum_ii += observed[pixel] * observed[pixel] * weight[pixel];  //I'm just including this for completeness.  It is a constant term if the weights are fixed.
		sum_mi += model[pixel] * observed[pixel] * weight[pixel];
		sum_mm  += model[pixel] * model[pixel] * weight[pixel];
	} 
	
	// A1 = inv_mm11 * sum_mi1 + inv_mm12 * sum_mi2;
	

	i3_flt logL = -0.5*(i3_log(sum_mm) - (sum_mi*sum_mi/sum_mm) + sum_ii);
	
	return logL;
}


i3_flt i3_logL_weight_map_maximise_amplitude_2components(i3_image * model_image1, i3_image * model_image2, i3_image * observed_image, i3_image * weight_image, i3_flt * amplitude1, i3_flt * amplitude2){
			
	/* Calculate statistics of the image */
	i3_flt sum_ii = 0.0; 
	i3_flt sum_mi1 = 0.0; // Ideally would make this an n-component length vector
	i3_flt sum_mi2 = 0.0; 
	i3_flt sum_mm11 = 0.0; // Ideally would make an n-component x n-component matrix
	i3_flt sum_mm12 = 0.0; // The matrix is symmetric
	i3_flt sum_mm22 = 0.0;
	
	/* Create some temporary space */
	i3_flt observed;
	i3_flt model1;
	i3_flt model2;
	i3_flt weight;
	
	for(int pixel=0;pixel<model_image1->n;pixel++){
		observed = observed_image->data[pixel];
		model1 = model_image1->data[pixel]; // Ideally would loop over components here
		model2 = model_image2->data[pixel];
		weight = weight_image->data[pixel];
		sum_ii += observed * observed * weight;  
		sum_mi1 += model1 * observed * weight;
		sum_mi2 += model2 * observed * weight;
		sum_mm11  += model1 * model1 * weight;
		sum_mm12  += model1 * model2 * weight;
		sum_mm22  += model2 * model2 * weight;
	}
	
	
	/* Invert the 2x2 model component dot product matrix by hand (could use matrix libraries etc) */
	
	i3_flt inv_mm11;
	i3_flt inv_mm12;
	i3_flt inv_mm22;
	i3_flt det_sum_mm;
	i3_flt A1;
	i3_flt A2;
	
	i3_flt very_small_number = 1e-12;

	//if (sum_mm11==0 && sum_mm22==0){ floating point comparison never works
	if(i3_fabs(sum_mm11)<very_small_number && fabs(sum_mm22)<very_small_number) {
		A1 = 0.0;
		A2 = 0.0;
	}
	//else if (sum_mm11==0){
	else if (i3_fabs(sum_mm11)<very_small_number){
		inv_mm22 = 1.0 / sum_mm22; 
		A2 = inv_mm22 * sum_mi2;
		A1=0;
		//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
		//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	}
	//else if (sum_mm22==0){
	else if (i3_fabs(sum_mm22)<very_small_number){
		inv_mm11 = 1.0 / sum_mm11; 
		A1 = inv_mm11 * sum_mi1;
		A2=0;
		//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
		//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	}
	else{
		det_sum_mm = sum_mm11 * sum_mm22 - sum_mm12 * sum_mm12;
		inv_mm11 = sum_mm22 / det_sum_mm;
		inv_mm12 = -sum_mm12 / det_sum_mm;
		inv_mm22 = sum_mm11 / det_sum_mm;
		/* Find the best-fit amplitude of each component */
		/* Do long-hand of: matrix multiply inv_mm matrix with sum_mi vector*/
		A1 = inv_mm11 * sum_mi1 + inv_mm12 * sum_mi2;
		A2 = inv_mm12 * sum_mi1 + inv_mm22 * sum_mi2;
		
	}
	
	if(isnan(A1)){
		A1 = 0.;
		I3_WARNING("model image 1 is nan");
		printf("inv_mm11 %f inv_mm12  %f  inv_mm22 %f sum_mi1 %f sum_mi2 %f sum_mm11 %f sum_mm12 %f sum_mm22 %f\n", inv_mm11,inv_mm12, inv_mm22, sum_mi1, sum_mi2, sum_mm11,sum_mm12,sum_mm22);
	} 
	if(isnan(A2)){ 
		A2 = 0.;
		I3_WARNING("model image 2 is nan");
		printf("inv_mm11 %f inv_mm12  %f  inv_mm22 %f sum_mi1 %f sum_mi2 %f sum_mm11 %f sum_mm12 %f sum_mm22 %f\n", inv_mm11,inv_mm12, inv_mm22, sum_mi1, sum_mi2, sum_mm11,sum_mm12,sum_mm22);
		 
	}
	
		
	/* Get the summed image, weighted by the best-fit amplitude of each component */
	i3_image_weighted_add_image_into(model_image1,A1,model_image2,A2);
	/* model_image1 now contains the full image with best fit amplitudes for each compt */
	
	i3_flt logL = -0.5*i3_chi2_weight_map(model_image1, observed_image, weight_image);
	// Should be able to replace the above using the pre-computed quantities - should work and save computation time
	// In practice gives different answer presumably due wildly different sizes of the terms
	// including giving positive likelihoods.
	//i3_flt sum_mm11_r = sum_mm11 * A1 * A1; // rescaled
	//i3_flt sum_mm22_r = sum_mm22 * A2 * A2;
	//i3_flt sum_mm12_r = sum_mm12 * A1 * A2;
	//i3_flt sum_mi1_r = sum_mi1 * A1;
	//i3_flt sum_mi2_r = sum_mi2 * A2;
	//i3_flt logL_ck = -0.5*( sum_ii - 2.0*sum_mi1_r -2.0*sum_mi2_r + sum_mm11_r + 2.0*sum_mm12_r + sum_mm22_r  );
	// N.B. This ignores the normalisation pre-factor from the log(Likelihood) i.e. the 1/sqrt(2 pi sigma^n) part. Would need to put that back in e.g. if comparing different noise levels or estimating noise level from the image itself
		
	// The below equations apply for 1 component models, left in for this repo update in case ever handy
	//inv_mm11 = 1.0 / sum_mm11; 
	//A1 = inv_mm11 * sum_mi1;
	//A2=0;
	//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
	//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	// we return with model_image1 containing the best fit image

	*amplitude1 = A1;
	*amplitude2 = A2;
	

	return logL;
}

i3_flt i3_logL_weight_map_maximise_amplitude_2components_positive(i3_image * model_image1, i3_image * model_image2, i3_image * observed_image, i3_image * weight_image, i3_flt * amplitude1, i3_flt * amplitude2){
			
	/* Calculate statistics of the image */
	i3_flt sum_ii = 0.0; 
	i3_flt sum_mi1 = 0.0; // Ideally would make this an n-component length vector
	i3_flt sum_mi2 = 0.0; 
	i3_flt sum_mm11 = 0.0; // Ideally would make an n-component x n-component matrix
	i3_flt sum_mm12 = 0.0; // The matrix is symmetric
	i3_flt sum_mm22 = 0.0;
	
	/* Create some temporary space */
	i3_flt observed;
	i3_flt model1;
	i3_flt model2;
	i3_flt weight;
	
	for(int pixel=0;pixel<model_image1->n;pixel++){
		observed = observed_image->data[pixel];
		model1 = model_image1->data[pixel]; // Ideally would loop over components here
		model2 = model_image2->data[pixel];
		weight = weight_image->data[pixel];
		sum_ii += observed * observed * weight;  
		sum_mi1 += model1 * observed * weight;
		sum_mi2 += model2 * observed * weight;
		sum_mm11  += model1 * model1 * weight;
		sum_mm12  += model1 * model2 * weight;
		sum_mm22  += model2 * model2 * weight;
	}
	
	
	/* Invert the 2x2 model component dot product matrix by hand (could use matrix libraries etc) */
	
	i3_flt inv_mm11;
	i3_flt inv_mm12;
	i3_flt inv_mm22;
	i3_flt det_sum_mm;
	i3_flt A1;
	i3_flt A2;
	
	i3_flt very_small_number = 1e-12;

	//if (sum_mm11==0 && sum_mm22==0){ floating point comparison never works
	if(i3_fabs(sum_mm11)<very_small_number && fabs(sum_mm22)<very_small_number) {
		A1 = 0.0;
		A2 = 0.0;
	}
	//else if (sum_mm11==0){
	else if (i3_fabs(sum_mm11)<very_small_number){
		inv_mm22 = 1.0 / sum_mm22; 
		A2 = inv_mm22 * sum_mi2;
		A1=0;
		//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
		//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	}
	//else if (sum_mm22==0){
	else if (i3_fabs(sum_mm22)<very_small_number){
		inv_mm11 = 1.0 / sum_mm11; 
		A1 = inv_mm11 * sum_mi1;
		A2=0;
		//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
		//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	}
	else{
		det_sum_mm = sum_mm11 * sum_mm22 - sum_mm12 * sum_mm12;
		inv_mm11 = sum_mm22 / det_sum_mm;
		inv_mm12 = -sum_mm12 / det_sum_mm;
		inv_mm22 = sum_mm11 / det_sum_mm;
		/* Find the best-fit amplitude of each component */
		/* Do long-hand of: matrix multiply inv_mm matrix with sum_mi vector*/
		A1 = inv_mm11 * sum_mi1 + inv_mm12 * sum_mi2;
		A2 = inv_mm12 * sum_mi1 + inv_mm22 * sum_mi2;
		
	}
	
	// now check if the best fit is outside the bounds

	if(isnan(A1)){
		A1 = 0.;
		I3_WARNING("model image 1 is nan");
		printf("inv_mm11 %f inv_mm12  %f  inv_mm22 %f sum_mi1 %f sum_mi2 %f sum_mm11 %f sum_mm12 %f sum_mm22 %f\n", inv_mm11,inv_mm12, inv_mm22, sum_mi1, sum_mi2, sum_mm11,sum_mm12,sum_mm22);
	} 
	if(isnan(A2)){ 
		A2 = 0.;
		I3_WARNING("model image 2 is nan");
		printf("inv_mm11 %f inv_mm12  %f  inv_mm22 %f sum_mi1 %f sum_mi2 %f sum_mm11 %f sum_mm12 %f sum_mm22 %f\n", inv_mm11,inv_mm12, inv_mm22, sum_mi1, sum_mi2, sum_mm11,sum_mm12,sum_mm22);
		 
	}
	i3_flt logL;
	
	if(A1 < 0. || A2 < 0. || 1){
		// calculate constrained likelihoods with AC amplitudes positive
		
		i3_flt AP1 = 2.*sum_mi1/sum_mm11;
		i3_flt AP2 = 2.*sum_mi2/sum_mm22;

		i3_flt logL1 = -0.5*(sum_ii - 2.0*sum_mi1*AP1 + sum_mm11*AP1*AP1);
		i3_flt logL2 = -0.5*(sum_ii - 2.0*sum_mi2*AP2 + sum_mm22*AP2*AP2);
		
		if(logL1 > logL2 && 0){
 			logL = logL1;
			A1 = AP1;
			A2 = 0.;
			i3_image_weighted_add_image_into(model_image1,A1,model_image2,A2);
		}
		else{
 			logL = logL2;
			A1 = 0.;
			A2 = AP2;
			i3_image_weighted_add_image_into(model_image1,A1,model_image2,A2);

		}
				
		
		
	}else{

		/* Get the summed image, weighted by the best-fit amplitude of each component */
		i3_image_weighted_add_image_into(model_image1,A1,model_image2,A2);
		/* model_image1 now contains the full image with best fit amplitudes for each compt */
		logL = -0.5*i3_chi2_weight_map(model_image1, observed_image, weight_image);

	}
	
		
	
	
	
	// Should be able to replace the above using the pre-computed quantities - should work and save computation time
	// In practice gives different answer presumably due wildly different sizes of the terms
	// including giving positive likelihoods.
	//i3_flt sum_mm11_r = sum_mm11 * A1 * A1; // rescaled
	//i3_flt sum_mm22_r = sum_mm22 * A2 * A2;
	//i3_flt sum_mm12_r = sum_mm12 * A1 * A2;
	//i3_flt sum_mi1_r = sum_mi1 * A1;
	//i3_flt sum_mi2_r = sum_mi2 * A2;
	//i3_flt logL_ck = -0.5*( sum_ii - 2.0*sum_mi1_r -2.0*sum_mi2_r + sum_mm11_r + 2.0*sum_mm12_r + sum_mm22_r  );
	// N.B. This ignores the normalisation pre-factor from the log(Likelihood) i.e. the 1/sqrt(2 pi sigma^n) part. Would need to put that back in e.g. if comparing different noise levels or estimating noise level from the image itself
		
	// The below equations apply for 1 component models, left in for this repo update in case ever handy
	//inv_mm11 = 1.0 / sum_mm11; 
	//A1 = inv_mm11 * sum_mi1;
	//A2=0;
	//i3_flt logL = -0.5*(sum_ii - 2.0*sum_mi1*A1 + sum_mm11*A1*A1);
	//i3_flt logL = -0.5*(sum_ii - sum_mi1*sum_mi1/sum_mm11);
		
	// we return with model_image1 containing the best fit image

	*amplitude1 = A1;
	*amplitude2 = A2;
	

	return logL;
}



i3_flt i3_logL_white_noise_marg_A(i3_image * model_image, i3_image * observed_image, i3_flt noise_std){

	i3_flt noise_var= noise_std * noise_std;
	i3_flt * model = model_image->data;
	i3_flt * observed = observed_image->data;
	
	i3_flt sum_mi = 0.0; 
	i3_flt sum_m = 0.0;

	for(int pixel=0;pixel<model_image->n;pixel++){
//		printf("%d \t",pixel);
		sum_mi += model[pixel] * observed[pixel];
		sum_m += model[pixel] * model[pixel];
	} 
	
	i3_flt logL = sum_mi*sum_mi/sum_m/noise_var/2.f + i3_log(i3_sqrt(M_PI / sum_m * 2.f * noise_var ));

	return logL;

}

i3_flt i3_logL_white_noise_marg_A_xy0(i3_image * model_image, i3_image * observed_image, i3_flt noise_std, int n_additional_pix, int n_sub_pix){

	int N_sub2 = n_sub_pix*n_sub_pix;	
	int N_obs =  observed_image->nx;
	int N_mod = N_obs + n_additional_pix*2;
	int N_mod2 = N_mod*N_mod;

	i3_flt * logL_xy0 = i3_logL_in_xy0(model_image,observed_image,noise_std,n_additional_pix,n_sub_pix);
	i3_flt scale = i3_array_max_value( logL_xy0, N_sub2*N_mod2);
	i3_flt * pdf_xy0 = i3_array_exp_scale( logL_xy0, N_sub2*N_mod2, scale);
	
	i3_flt logL_marg_xy0 = i3_array_sum(pdf_xy0, N_sub2*N_mod2);	
	
	logL_marg_xy0 = i3_log(logL_marg_xy0);


// testing

	// printf("scale = %4.4f \nlogL_marg_xy0 = %4.4f \n",scale,logL_marg_xy0);

	free(logL_xy0);
	free(pdf_xy0);
	return logL_marg_xy0 + scale;





}

i3_flt * i3_logL_in_xy0(i3_image * model_image, i3_image * observed_image, i3_flt noise_std, int n_additional_pix, int n_sub_pix){

	int N_sub = n_sub_pix;
	int N_add = n_additional_pix;

	i3_flt noise_var= noise_std * noise_std;
	i3_flt sum_mi = 0.0; 
	i3_flt sum_m = 0.0;
	
	int N_sub2 = N_sub*N_sub;	
	int N_obs =  observed_image->nx;
	// int N_obs2 = N_obs*N_obs;
	int N_win = (N_add*2+1);
	int N_win2 = N_win*N_win;

	//printf("image_model size %d %d\n",(int)model_image->nx,(int)model_image->nx);
	//printf("image_observed size %d %d\n",(int)observed_image->nx,(int)observed_image->nx);
	//printf("N_sub %d N_add %d N_mod %d\n",N_sub,N_add,N_mod);

	i3_flt * logL_xy0 = malloc(sizeof(i3_flt)*N_sub2*N_win2);	
	for(int i=0;i<N_sub2*N_win2;i++) logL_xy0[i] = 0.0; 

	int ns = 0;

	//printf("starting the loop - we should get %f values of xy_logL\n",pow((N_add*2+1)*N_sub,2));

	for(int na1 = 0; na1<N_win; na1++){
		for(int ns1 = 0; ns1<N_sub; ns1++ ){
			for(int na2 = 0; na2<N_win; na2++){
				for(int ns2 = 0; ns2<N_sub; ns2++ ){
		
					sum_mi = 0.0; 
					sum_m = 0.0;
		
					for(int np1 = 0; np1<N_obs; np1++ ){
						for(int np2 = 0; np2<N_obs; np2++ ){
							//printf("%d %d \t",ns1 + na1*N_sub + np1*N_sub,ns2 + na2*N_sub + np2*N_sub);
							sum_mi += model_image->row[ns1 + na1*N_sub + np1*N_sub][ns2 + na2*N_sub + np2*N_sub] * observed_image->row[np1][np2];
							sum_m  += model_image->row[ns1 + na1*N_sub + np1*N_sub][ns2 + na2*N_sub + np2*N_sub];
							//sum_mi += model_image->row[ns1 + np1*N_sub][ns2 + np2*N_sub] * observed_image->row[np1][np2];
							//sum_m  += model_image->row[ns1 + np1*N_sub][ns2 + np2*N_sub];
						}	
					}
		
					logL_xy0[ns] = sum_mi*sum_mi/sum_m/noise_var/2.f + i3_log(i3_sqrt(M_PI / sum_m * 2.f * noise_var ));
					ns++;	
				}
			}

		}
	}
	
// 	i3_flt * pdf_xy = i3_pdf_from_logL(logL_xy, ns);
// 	i3_flt logL_marg_xy = 0.0;
// 	for(int i=0;i<ns;i++) logL_marg_xy += pdf_xy[i];
// 
// 	#ifdef I3_USE_DOUBLE
// 		logL_marg_xy = log(logL_marg_xy);
// 	#else
// 		logL_marg_xy = logf(logL_marg_xy);
// 	#endif
// 
// 	// for test only
// 	FILE * file;
// 	file = fopen("tests/xy_logL.dat","w"); 
// 	fwrite(logL_xy,sizeof(i3_flt),ns,file);
// 	fclose(file);
// 	file = fopen("tests/xy_pdf.dat","w"); 
// 	fwrite(pdf_xy,sizeof(i3_flt),ns,file);
// 	fclose(file);

	return logL_xy0; 





}

i3_flt i3_chi2_white_noise_fourier(i3_fourier * model_fourier, i3_fourier * observed_fourier, i3_flt sigma){
	i3_flt chi2=0.f;
	i3_flt sigma2= sigma*sigma;
//	i3_cpx * predicted = model_fourier->data;
//	i3_cpx * observed = observed_fourier->data;

	int nx_f = model_fourier->nx_f;
	int ny_f = model_fourier->ny_f;
	for (int i=0;i<ny_f;i++){	
		chi2+=i3_pow(i3_cabs( model_fourier->row[i][0] - observed_fourier->row[i][0] ),2);
		for(int j=1;j<nx_f;j++){
			chi2+=2*i3_pow(i3_cabs( model_fourier->row[i][j] - observed_fourier->row[i][j] ),2);
		}
	}
	return chi2/sigma2/model_fourier->n_real;
	
}

i3_flt * i3_chi2_with_models_white_noise(i3_image ** all_models, int n_models, i3_image * observed_image, i3_flt noise_std){

	if(!all_models) return NULL;
	if(!observed_image) return NULL;

	i3_flt * chisq_set = malloc(sizeof(i3_flt)*n_models);
	
	for(int nm = 0; nm < n_models; nm++){	
		chisq_set[nm] = -1.f * i3_chi2_white_noise( all_models[nm], observed_image, noise_std );
	};

	return chisq_set;
}

i3_flt * i3_logL_with_models_white_noise_marg_A(i3_image ** all_models, int n_models, i3_image * observed_image, i3_flt noise_std){

	if(!all_models) I3_FATAL("no image models cube supplied",1);
	if(!observed_image) I3_FATAL("no noisy image supplied",1);

	i3_flt * logL = malloc(sizeof(i3_flt)*n_models);

	for(int nm = 0; nm < n_models; nm++){
		
		if(!all_models[nm]) I3_FATAL("model in the cube doesn't exist'",1);
	
		logL[nm] = i3_logL_white_noise_marg_A( all_models[nm] , observed_image, noise_std );
	};


	return logL;
}

i3_flt * i3_logL_with_models_white_noise_fast(i3_image ** all_models, int n_models, i3_flt * models_e1, i3_flt * models_e2, i3_image * observed_image, i3_flt e1, i3_flt e2, i3_flt cut_off , i3_flt noise_std, int metric){

	if(!all_models) I3_FATAL("all models array is NULL",1);
	if(!models_e1) I3_FATAL("models_e1 array is NULL",1);
	if(!models_e2) I3_FATAL("models_e2 is NULL",1);
	if(!observed_image) I3_FATAL("observed image array is NULL",1);

	
	
	i3_flt *  logL = malloc(sizeof(i3_flt)*n_models);
	
	for(int nm = 0; nm < n_models; nm++){
		if( i3_fabs( models_e1[nm] - e1 ) <  cut_off && i3_fabs( models_e2[nm] - e2 ) < cut_off )	
			if(metric==0) logL[nm] = -1.f*i3_chi2_white_noise( all_models[nm], observed_image, noise_std );
			if(metric==1) logL[nm] = i3_logL_white_noise_marg_A( all_models[nm], observed_image, noise_std );
		else	logL[nm] = -1e10f;
	};

	return logL;
}

i3_flt * i3_marginalise_r(i3_flt * pdf_full, int n_pdf_full, int n_pdf_marg){
	
	if(n_pdf_full%n_pdf_marg != 0) I3_FATAL("i3_marginalise_r: n_pdf_full mod n_pdf_marg != 0 \n",1); 

	i3_flt * pdf_marg =  malloc(sizeof(i3_flt)*n_pdf_marg);
	for(int npm=0;npm<n_pdf_marg;npm++) pdf_marg[npm] = 0.0;
	
	int n_marg = n_pdf_full/n_pdf_marg;
	
	for(int npf=0;npf<n_pdf_full;npf++){
		pdf_marg[npf/n_marg] += pdf_full[npf];
	}
		
	return pdf_marg;
}

void i3_covariance_matrix_proposal(i3_mcmc * mcmc, i3_parameter_set * current, i3_parameter_set * proposed){
	/* Turn the parameter set into a vector*/
	int np = mcmc->nParam;
	int i = mcmc->nSamples%np;
	i3_flt c[np];
	i3_flt p[np];
	i3_flt w[np];
	/* We take care not to vary the parameters which are supposed to be fixed */
	i3_model_extract_varied_nonzero_width_parameters(mcmc->model, current,mcmc->width, c);
	i3_model_extract_varied_nonzero_width_parameters(mcmc->model, proposed,mcmc->width,  p);
	i3_model_extract_varied_nonzero_width_parameters(mcmc->model, mcmc->width,mcmc->width,  w);

	/* If necessary, create a new random rotation */
	if(i==0 && mcmc->propose_rotation)
	{
		i3_random_rotation(mcmc->random_rotation,mcmc->nParam);
	}

	/* Jump in the random direction by a random normal distance*/
	i3_flt lambda = mcmc->scaling_parameter * i3_random_normal();
	if (mcmc->propose_rotation) lambda /= i3_sqrt(mcmc->nParam);

	// If we do not have a covariance matrix then just use a diagonal one,
	// with widths
	if (mcmc->cov_sqrt==NULL){
		for(int b=0;b<np;b++) p[b] = c[b] + lambda*w[b]*mcmc->random_rotation[i+np*b];		
	}
	// Otherwise use the covmat sqrt.
	else{
		for(int b=0;b<np;b++){
			p[b] = c[b];
			for (int q=0;q<np; q++){
				p[b] += lambda * mcmc->cov_sqrt[b*np+q] * mcmc->random_rotation[i*np+q];	
			}
		} 
	}


	/* Now turn the vector back into a parameter set*/
	i3_model_input_varied_nonzero_width_parameters(mcmc->model, p, proposed, mcmc->width, current);

}


/* Circular Moffat profile function  - rc_in is the truncation
 radius.  Only GREAT08 uses truncated Moffats... For GREAT10 we can just set rc_in to be larger than the images.
 Everything else is self-explanatory, see e.g. Bridle et al 2010.
 
 The function takes scalar input / output... Could be made faster by vector calls using whole images of sheared x, y. 
...
Non-C expert Barney asks, is it quicker to pass these simple arguments by reference?
*/
i3_flt i3_moffat(i3_flt r2_in, i3_flt rd2_in, i3_flt rc2_in, i3_flt beta_in){
  i3_flt ans;

  if (r2_in >= rc2_in)
  {
    ans = 0.;
  }
  else
  {
    /* Get the Moffat profile */
    ans = i3_pow(1.f + r2_in / rd2_in, -beta_in);

  }
  return ans;
}

i3_flt i3_moffat_rd2(i3_flt fwhm_in, i3_flt beta_in){
  /* Calculate the Moffat scale radius rd^2 from the FWHM and beta
   this should be done once then used as input to the Moffat
   function i3_moffat(...)
  */

	
	if (beta_in==3) {
		return 0.25*fwhm_in*fwhm_in/1.203/1.203;
	}
	else I3_FATAL("I do not think i3_moffat_rd2 is consistent with Great10 definitiions.  Need to check",12);
  i3_flt ans;
  
  ans = 0.25f * fwhm_in * fwhm_in / (i3_pow(2.f, 1.f / beta_in) - 1.f);
  return ans;
}

/* Bessel function J1(x) adapted from Numerical recipes, used for Airy PSF models */
i3_flt i3_bessj1(i3_flt x){
  i3_flt ax,z;
  double xx,y,ans,ans1,ans2;

  if ((ax=i3_fabs(x)) < 8.0)
  {
    y=x*x;
    ans1=x*(72362614232.0+y*(-7895059235.0+y*(242396853.1
                             +y*(-2972611.439+y*(15704.48260+y*(-30.16036606))))));
    ans2=144725228442.0+y*(2300535178.0+y*(18583304.74
                                           +y*(99447.43394+y*(376.9991397+y*1.0))));
    ans=ans1/ans2;
  }
  else
  {
    z=8.0/ax;
    y=z*z;
    xx=ax-2.356194491;
    ans1=1.0+y*(0.183105e-2+y*(-0.3516396496e-4
                               +y*(0.2457520174e-5+y*(-0.240337019e-6))));
    ans2=0.04687499995+y*(-0.2002690873e-3
                          +y*(0.8449199096e-5+y*(-0.88228987e-6
                                                 +y*0.105787412e-6)));
    ans=i3_sqrt(0.636619772/ax)*(i3_cos(xx)*ans1-z*i3_sin(xx)*ans2);
    if (x < 0.0) ans = -ans;
  }
  return ans;
}

i3_flt i3_airy(i3_flt x_in, i3_flt y_in, i3_flt fwhm_in, i3_flt rc_in){
  /* Calculates circular airy function - input scalars, output scalars, as for i3_moffat(...) might get better speed using vector libraries on a whole image... */
  i3_flt r2, x, y, ans;

  r2 = x_in * x_in + y_in * y_in;
  if (r2 >= (rc_in * rc_in))
  {
    ans = 0.;
  }
  else
  {
    x = i3_sqrt(r2) * M_PI * 1.028993969962188 / fwhm_in;
    y = i3_bessj1(x);
    ans = 4. * y * y / x / x;
  }
  return ans;
}

i3_flt i3_fwhm_to_ab(i3_flt fwhm, i3_flt sersic_n){
  i3_flt k = i3_sersic_kappa(sersic_n);
#define NATURAL_LOG_2 0.693147181
  i3_flt r_e = fwhm/2.0 / powf(NATURAL_LOG_2/k,sersic_n);
  i3_flt ab = r_e*r_e;
  return ab;
}

i3_flt i3_r_to_fwhm(i3_flt r, i3_flt sersic_n){
  i3_flt k = 1.9992*sersic_n-0.3271; //From Voigt & Bridle 2009
#define NATURAL_LOG_2 0.693147181
  i3_flt fwhm = 2.0 * r * powf(NATURAL_LOG_2/k,sersic_n);
  return fwhm;
}

i3_flt i3_snr_to_noise_std(i3_flt snr, i3_image * image){
	return i3_image_norm(image) / snr;

}

// int factorial(int n){
// 	int r=1;
// 	for(int i=1;i<=n;i++) r*=i;
// 	return r;
// }


// static i3_flt i3_sersic_integrand_function(i3_flt s)
// {
// 	i3_flt s2=s*s;
// 	i3_flt s3=s2*s;
// 	i3_flt s4=s3*s;
// 	i3_flt s5=s4*s;
// 	i3_flt s6=s5*s;
// 	i3_flt s7=s6*s;
// 	return 5040.-exp(-s)*(s7+7*s6+42*s5+210*s4+840*s3+2520*s2+5040*s+5040);
// }


i3_flt i3_sersic_total_flux(i3_flt n, i3_flt A, i3_flt ab)
{
	i3_flt k = i3_sersic_kappa(n);;
	i3_flt gamma = gsl_sf_gamma(2*n);
	i3_flt F = 2*M_PI*n*pow(k,-2*n)*ab*A*gamma;
	return F;
}


i3_flt i3_sersic_flux_to_radius(i3_flt n, i3_flt A, i3_flt ab, i3_flt r)
{
	i3_flt r_e = sqrt(ab);
	i3_flt k = i3_sersic_kappa(n);
	i3_flt gamma = gsl_sf_gamma(2*n) - gsl_sf_gamma_inc(2*n,k*pow(r/r_e,1./n));
	i3_flt F = 2*M_PI*n*pow(k,-2*n)*ab*A*gamma;
	return F;
	
	
}

// 
// 
// i3_flt i3_sersic_total_flux(i3_flt sersic_index, i3_flt amplitude, i3_flt ab)
// {
// 	int s = (int) sersic_index;
// 	if (s!=sersic_index) I3_FATAL("You have called i3_sersic_total_flux with a non-integer sersic index.  You need to add a gamma function implementation to do this,",2);
// 	i3_flt k = 1.9992*s-0.3271;
// 	i3_flt gamma = factorial(2*s-1);
// 	i3_flt F = 2*M_PI*s*pow(k,-2*s)*ab*amplitude*gamma;
// 	return F;
// }


i3_flt * i3_pdf_from_logL(i3_flt * logL, int n){

	i3_flt scale = i3_array_max_value( logL, n);
	i3_flt * pdf = i3_array_exp_scale(logL,n,scale);
	i3_array_normalise( pdf, n );
	return pdf;
}



i3_flt i3_estimate_noise_rms_observed_model(i3_image * observed, i3_image * model, i3_image * mask)
{
	
	i3_flt noise_variance = 0.0;
	int n = 0;
	for(int i=0;i<observed->n;i++){
		if (mask->data[i]!=0){
			n++;
			noise_variance += pow(model->data[i] - observed->data[i],2);
		}
	}
	noise_variance/=n;
	i3_flt noise = i3_sqrt(noise_variance);
	return noise; 
	
}

i3_flt i3_signal_to_noise_with_weight(i3_image * model, i3_image * weight)
{
	i3_flt snr=0.0;
	for (int i=0; i<model->n; i++){
		snr += weight->data[i] * pow(model->data[i],2);
	}
	return i3_sqrt(snr);
}

i3_flt i3_signal_to_noise_with_weight_and_data(i3_image * model, i3_image * image, i3_image * weight)
{
	i3_flt top=0.0;
	i3_flt bottom=0.0;
	for (int i=0; i<model->n; i++){
		top += weight->data[i] * model->data[i] * image->data[i];
		bottom += weight->data[i] * model->data[i] * model->data[i];
	}
	return top/i3_sqrt(bottom);
}


i3_flt i3_signal_to_noise_model_fitted(i3_image * observed, i3_image * model, i3_image * mask)
{
	i3_flt noise=i3_estimate_noise_rms_observed_model(observed, model, mask);
	return i3_signal_to_noise_fit(model, noise, mask);
}

i3_flt i3_signal_to_noise_fit(i3_image * model, i3_flt noise, i3_image * mask)
{
	i3_flt sum_model_squared = 0;
	for(int i=0;i<model->n;i++){
		if (mask->data[i]!=0) sum_model_squared += pow(model->data[i],2);
	}
	i3_flt rms = i3_sqrt(sum_model_squared);
	return rms/noise;
}

void i3_ellipticity_eta_to_e(i3_flt eta1, i3_flt eta2, i3_flt * e1, i3_flt * e2){
	
	i3_flt eta = i3_sqrt(eta1*eta1+eta2*eta2);
	i3_flt e = (1.-i3_exp(-eta))/(1.+i3_exp(-eta));
	i3_flt theta = 0.5*i3_atan2(eta2,eta1);
	*e1 = e*cos(2.*theta);
	*e2 = e*sin(2.*theta);

}

void i3_ellipticity_e_to_eta(i3_flt e1, i3_flt e2, i3_flt * eta1, i3_flt * eta2){
	
	i3_flt e = i3_sqrt(e1*e1+e2*e2);
	i3_flt theta = 0.5*i3_atan2(e2,e1);
	i3_flt eta = i3_log((1.+e)/(1.-e));
	*eta1 = eta*cos(2.*theta);
	*eta2 = eta*sin(2.*theta);

}

i3_flt i3_sersic_log_flux_to_amplitude(i3_flt log_flux, i3_flt sersic_index, i3_flt ab){
	return i3_exp(log_flux) / i3_sersic_total_flux(sersic_index, 1.0, ab);
}



static i3_flt log_factorial(int n)
{
	i3_flt log_fact = 0.0;
	for (int c = 1; c <= n; c++){
		log_fact += i3_log((i3_flt) c);
	}
	return log_fact;

}

i3_flt i3_poisson_like(i3_image * model_image, i3_image * observed_image)
{
	i3_flt loglike = 0.0;
	for (int p=0; p<model_image->n;p++){
		i3_flt lambda = model_image->data[p];
		i3_flt k = observed_image->data[p];
		loglike += k*i3_log(lambda) - lambda - log_factorial(k);
	}
	return loglike;
}


i3_flt i3_poisson_like_exposure_background(i3_image * model_image, i3_image * observed_image, 
	i3_image * exposure_map, i3_flt background)
{
	i3_flt loglike = 0.0;
	for (int p=0; p<model_image->n;p++){
		if (exposure_map->data[p]==0) continue;
		i3_flt lambda = (model_image->data[p]) * exposure_map->data[p] + background;
		i3_flt k = observed_image->data[p];
		loglike += k*i3_log(lambda) - lambda - log_factorial(k);
	}
	return loglike;
}