/*
 * peakdetect.c
 *
 * Peakdetection routines, utilities and automation
 *
 * (c) 2007 Gordon Ball <gfb21@cam.ac.uk>
 *  dtr - Diffraction Tomography Reconstruction
 *
 */

#include <math.h>
#include "reflections.h"
#include "control.h"
#include "imagedisplay.h"
#include "utils.h"
#include "peakdetect.h"

/*
 * Renormalise a gsl_matrix to 0->1
 * Re-written to use gsl_matrix native functions
 */
void renormalise(gsl_matrix *m) {
	double max,min;
	gsl_matrix_minmax(m,&min,&max);
	printf("Renormalising min=%lf max=%lf\n",min,max);
	gsl_matrix_add_constant(m,0.-min);
	gsl_matrix_scale(m,1./(max-min));
}

/*
 * Create a gsl_matrix for performing image operations on
 * from a raw image and control context
 * Converting to gsl_matrix because many of the required operations
 * can be done as matrices to save effort
 * Renormalises matrix to 0->1
 */
gsl_matrix* createImageMatrix(ControlContext *ctx, int16_t *image) {
	gsl_matrix *raw;
	raw = gsl_matrix_alloc(ctx->width,ctx->height);
	
	int i,j;
	for ( i=0; i<raw->size1;i++) {
		for (j=0; j<raw->size2;j++) {
			gsl_matrix_set(raw,i,j,(double)image[i+j*ctx->width]);
		}
	}
	printf("Created %dx%d matrix\n",ctx->width,ctx->height);
	renormalise(raw);
	return raw;
}



/*
 * Find and return the mean value of the matrix
 */
double image_mean(gsl_matrix *m) {
	double mean=0.;
	int i,j;
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			mean += gsl_matrix_get(m,i,j);
		}
	}
	return mean / (m->size1 * m->size2);
}

/*
 * Return the standard deviation, sigma, of the matrix values
 * \sqrt(\sum((x-mean)^2)/n)
 */
double image_sigma(gsl_matrix *m, double mean) {
	double diff2=0;
	int i,j;
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			diff2 += (gsl_matrix_get(m,i,j)-mean)*(gsl_matrix_get(m,i,j)-mean);
		}
	}
	return sqrt(diff2/(m->size1 * m->size2));
}

/*
 * Filter all pixels with value < mean + k*sigma to 0
 * Set all matching pixels to 1
 */
void sigma_filter(gsl_matrix *m, double k) {
	double mean,sigma;
	int i,j;
	mean = image_mean(m);
	sigma = image_sigma(m,mean);
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			if (gsl_matrix_get(m,i,j) >= mean+k*sigma) {
				gsl_matrix_set(m,i,j,1.);
			} else {
				gsl_matrix_set(m,i,j,0.);
			}
		}
	}
}

/*
 * Calculate the mean within a circle centred (x,y) of radius r
 *
 * TODO: Use a mask instead of calculating valid points
 */
double circle_mean(gsl_matrix *m, int x, int y, int r, gsl_matrix *mask) {
	double mean=0.;
	int i,j,n=0;
	for (i=x-r;i<=x+r;i++) {
		for (j=y-r;j<=y+r;j++) {
			//printf("cm: ij=(%d,%d) mask=(%d,%d)\n",i,j,i-x+r,j-y+r);
			if (gsl_matrix_get(mask,i-x+r,j-y+r)>0.) {
				mean += gsl_matrix_get(m,i,j);
				//printf("cm: (%d,%d) mean=%lf val=%lf\n",i,j,mean,gsl_matrix_get(m,i,j));
				n++;
			}
		}
	}
	//printf("cm: (%d,%d) summean=%lf n=%d\n",x,y,mean,n);
	return mean/n;
}

/*
 * Calculate sigma within a circle centred (x,y) of radius r
 *
 * TODO: Use a mask instead of calculating valid points
 */
double circle_sigma(gsl_matrix *m, int x, int y, int r, gsl_matrix *mask, double mean) {
	double diff2=0.;
	int i,j,n=0;
	for (i=x-r;i<=x+r;i++) {
		for (j=y-r;j<=y+r;j++) {
			if (gsl_matrix_get(mask,i-x+r,j-y+r)>0) {
				diff2 += (gsl_matrix_get(m,i,j)-mean)*(gsl_matrix_get(m,i,j)-mean);
				n++;
			}
		}
	}
	return sqrt(diff2/n);
}

/*
 * Calculate a circular mask to save recalculating it every time
 */
gsl_matrix* circle_mask(int r) {
	gsl_matrix *m;
	m = gsl_matrix_calloc(2*r+1,2*r+1);
	int i,j;
	for (i=0;i<2*r+1;i++) {
		for (j=0;j<2*r+1;j++) {
			if (sqrt((r-i)*(r-i)+(r-j)*(r-j))<=(double)r) {
				 gsl_matrix_set(m,i,j,1.);
			}
			
		}
	}
	return m;
}

/*
 * Variation on the above filter where instead of using
 * sigma and mean for the whole image, it is found for a local
 * circle of radius r pixels.
 * The central point is also calculated as an approximately gaussian
 * average over local pixels rather than just a single pixel.
 * This takes a long time - 20-30 seconds for a 1024^2 image and r=10,
 * obviously large values of r will make it even slower.
 * The appropriate r value needs to be considered carefully - it needs to
 * be somewhat smaller than the average inter-reflection seperation.
 * 10 pixels worked well for the sapphire.mrc images, but this might
 * not be the case for other images. Images for which this would be very
 * large probably need to be resampled to a lower resolution.
 *
 * TODO: Pass a mask to the ancilliary functions instead of having
 * them calculate several hundred million sqrts
 * 
 * self-referencing problem being dealt with - output being written onto the array before the next point it computed
 * problem carried over from the OO version where a new object was created by each operation
 */

void local_sigma_filter(gsl_matrix *m, int r, double k) {
	//printf("lsf: starting\n");
	double mean,sigma;
	double local;
	//printf("lsf: generating circle mask\n");
	gsl_matrix *mask = circle_mask(r);
	gsl_matrix *new;
	new = gsl_matrix_alloc(m->size1,m->size2);
	int i,j;
	int interval = (m->size1-r)/20;
	//printf("lsf: starting loop\n");
	printf("lsf: ");
	//for (i=r;i<m->size1-r;i++) {
	//	for (j=r;j<m->size2-r;j++) {
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			if ((i>=r && i<m->size1-r) && (j>=r && j<m->size2-r)) {
				//printf("lsf: evaluating (%d,%d)\n",i,j);
				mean = circle_mean(m,i,j,r,mask);
				//printf("lsf: mean=%lf",mean);
				sigma = circle_sigma(m,i,j,r,mask,mean);
				//printf(" sigma=%lf",sigma);
				local = gsl_matrix_get(m,i,j);
				local += gsl_matrix_get(m,i+1,j) + gsl_matrix_get(m,i-1,j) + gsl_matrix_get(m,i,j+1) + gsl_matrix_get(m,i,j-1);
				local += .5*(gsl_matrix_get(m,i+1,j+1) + gsl_matrix_get(m,i-1,j+1) + gsl_matrix_get(m,i+1,j-1) + gsl_matrix_get(m,i-1,j-1));
				local /= 7.;
				//printf(" local=%lf\n",local);
				if (local > mean+k*sigma) {
					gsl_matrix_set(new,i,j,1.);
				} else {
					gsl_matrix_set(new,i,j,0.);
				}
			} else {
				gsl_matrix_set(new,i,j,0.);
			}
		}
		if (i % interval == 0) printf(".");
	}
	printf("done\n");
	gsl_matrix_memcpy(m,new);
	gsl_matrix_free(new);
}



/*
 * Apply an arbitary kernel to the image - each point takes the value
 * of the sum of the kernel convolved with the surrounding pixels.
 * The kernel needs to be a (2n+1)^2 array (centred on (n,n)). It needs
 * to be square and should probably be normalised.
 * Don't do daft things like rectangular kernels or kernels larger
 * than the image - this doesn't check such things and will cry.
 * 
 * Also suffers from self-reference problem
 */
void apply_kernel(gsl_matrix *m, gsl_matrix *kernel) {
	int size = kernel->size1;
	int half = (size-1)/2;
	gsl_matrix *l;
	gsl_matrix_view lv;
	gsl_matrix *new;
	new = gsl_matrix_calloc(m->size1,m->size2);
	double val;
	int i,j,x,y;
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			if ((i>=half && i<m->size1-half) && (j>=half && j<m->size2-half)) {
				lv = gsl_matrix_submatrix(m,i-half,j-half,size,size);
				l = &lv.matrix;
				val = 0.;
				for (x=0;x<size;x++) {
					for (y=0;y<size;y++) {
						val += gsl_matrix_get(l,x,y)*gsl_matrix_get(kernel,x,y);
					}
				}
				//gsl_matrix_free(l);
				gsl_matrix_set(new,i,j,val);
			}
		}
	}
	gsl_matrix_memcpy(m,new);
	gsl_matrix_free(new);
}

/*
 * Generate the simplist possible kernel - a flat one
 */
gsl_matrix* generate_flat_kernel(int half) {
	gsl_matrix *k;
	k = gsl_matrix_alloc(2*half+1,2*half+1);
	gsl_matrix_set_all(k,1./((2*half+1)*(2*half+1)));
	return k;
}

/*
 * expands or contracts a gsl_matrix by copying the columns to a new one
 */
gsl_matrix* matrix_expand(gsl_matrix *m, int oldsize, int newsize) {
	gsl_matrix *new;
	
	printf("me: %d->%d\n",oldsize,newsize);
	
	new = gsl_matrix_calloc(2,newsize);
	//printf("me: calloc(2,%d)\n",newsize);
	int j;
	for (j = 0; j < oldsize; j++) {
		if (j < newsize) {
			//printf("me: copying col %d\n",j);
			gsl_matrix_set(new,0,j,gsl_matrix_get(m,0,j));
			gsl_matrix_set(new,1,j,gsl_matrix_get(m,1,j));
		}
	}
	//printf("me: freeing old matrix\n");
	gsl_matrix_free(m);
	//printf("me: new s1=%d s2=%d\n",new->size1,new->size2);
	return new;
	//printf("me: m s1=%d s2=%d\n",m->size1,m->size2);
}

/*
 * Stack-based flood-fill iteration routine 
 * have to return a pointer to com each time because if the matrix size has to be changed then we need to keep
 * track of the location of the resized instance
 */
 
gsl_matrix* do_ff(int i, int j, int* mask, double threshold, gsl_matrix *m, gsl_matrix *com, int *com_n, int *com_size) {
	if (i>=0 && i<m->size1) {
		if (j>=0 && j<m->size2) {
			if (mask[i+j*m->size1]==0) {
				if (gsl_matrix_get(m,i,j)>threshold) {
					//printf("dff: found valid point (%d,%d)\n",i,j);
					gsl_matrix_set(com,0,*com_n,(double)i);
					gsl_matrix_set(com,1,*com_n,(double)j);
					*com_n=*com_n+1;
					if (*com_n == *com_size) {
						com = matrix_expand(com,*com_size,*com_size*2);
						*com_size *= 2;
					}
					mask[i+j*m->size1]=1;
					com = do_ff(i+1,j,mask,threshold,m,com,com_n,com_size);
					com = do_ff(i-1,j,mask,threshold,m,com,com_n,com_size);
					com = do_ff(i,j+1,mask,threshold,m,com,com_n,com_size);
					com = do_ff(i,j-1,mask,threshold,m,com,com_n,com_size);
				}
			}
		}
	}
	return com;
}

/*
 * Find points by floodfilling all pixels above threshold
 * Implements a stack-based flood-filling method which may
 * cause stack overflows if the blocks are extremely large -
 * dependent on implementation (default 4MiB stack for unix?)
 * Implements a crude variable-sized array method which hopefully works
 * Returns a gsl_matrix with x coordinates in row 0 and y
 * coordinates in row 1, which should be of the right length
 * Variable count is set to the number of points found
 */

gsl_matrix* floodfill(gsl_matrix *m, double threshold, int *count) {
	//printf("ff: starting\n");
	int *mask;
	mask = calloc(m->size1*m->size2,sizeof(int));
	
	int size=32,com_size;
	int i,j,k,n=0;
	int com_n;
	gsl_matrix *p;
	gsl_matrix *com;
	p = gsl_matrix_calloc(2,size);
	
	double com_x,com_y;
	printf("ff: starting loop\n");
	for (i=0;i<m->size1;i++) {
		for (j=0;j<m->size2;j++) {
			if (gsl_matrix_get(m,i,j)>threshold) {
				if (mask[i+j*m->size1]==0) {
					//printf("ff: found starting point (%d,%d)\n",i,j);
					com_size=32;
					com_n=0;
					com_x = com_y = 0.;					
					com = gsl_matrix_calloc(2,com_size); //this is going to hold the points found for this location
					//printf("ff: starting floodfill stack\n");
					com = do_ff(i, j, mask, threshold, m, com, &com_n, &com_size);
					//printf("ff: ended floodfill stack\n");
					for (k=0;k<com_n;k++) {
						com_x += gsl_matrix_get(com,0,k);
						com_y += gsl_matrix_get(com,1,k);
					}
					com_x /= com_n;
					com_y /= com_n;
					printf("ff: point CoM (%lf,%lf)\n",com_x,com_y);
					gsl_matrix_set(p,0,n,com_x);
					gsl_matrix_set(p,1,n,com_y);
					n++;
					if (n==size) {
						p = matrix_expand(p,size,size*2);
						size *= 2;
					}
				}
			}
		}
	}
	printf("ff: ending loop, found %d\n",n);
	*count = n;
	printf("pcheck s1=%d s2=%d\n",p->size1,p->size2);
	p = matrix_expand(p,size,n);
	printf("pcheck s1=%d s2=%d\n",p->size1,p->size2);
	return p;
}



/*
 * implements the iteration based automatic method
 * returns a gsl_matrix formatted as described in flood-fill
 */
gsl_matrix* iterate(gsl_matrix *m, int *count) {
	printf("Iterate: starting\n");
	int old = m->size1*m->size2;
	int cur;
	double k;
	double mean,sigma;
	gsl_matrix *p;
	gsl_matrix *kernel;
	
	mean = image_mean(m);
	sigma = image_sigma(m,mean);
	printf("Iterate: mean=%lf sigma=%lf\n",mean,sigma);
	
	
	printf("Iterate: generating kernel\n");
	kernel = generate_flat_kernel(1);
	printf("Iterate: performing local_sigma_filter\n");
	local_sigma_filter(m,10,1.);
	
	mean = image_mean(m);
	sigma = image_sigma(m,mean);
	printf("Iterate: mean=%lf sigma=%lf\n",mean,sigma);
	
	
	printf("Iterate: starting loop\n");
	while (1) {
		printf("Iterate: smoothing");
		apply_kernel(m,kernel);
		printf(" (1)");
		apply_kernel(m,kernel);
		printf(" (2)\n");
		mean = image_mean(m);
		sigma = image_sigma(m,mean);
		printf("Iterate: mean=%lf sigma=%lf\n",mean,sigma);
		k = (0.5-mean)/sigma;
		printf("Iterate: applying sigma_filter(%lf)\n",k);
		sigma_filter(m,k);
		printf("Iterate: floodfilling\n");
		p = floodfill(m,0,&cur);
		printf("Iterate: %d points found\n",cur);
		if (old < 1.05*cur) break;
		old = cur;
	}
	gsl_matrix_free(kernel); 
	printf("Iterate: finished\n");
	*count = cur;
	return p;
}