package otherSupport;

import oneLiners.OneLiners;
import binaryMatrixFile.BinaryMatrixFile;

/** 2D Phase unwrapping ported from C (csrc/phaseUnwrap2D/phaseUnwrap2D.c
// Taken from http://www.ljmu.ac.uk/GERI/CEORG_Docs/Miguel_2D_unwrapper_with_wrap_around_option.cpp
// Can't find a license, but the original journal article and author are below.
//
// oliford
//

//This program was written by Munther Gdeisat and Miguel Arevallilo HerraŽez to program the two-dimensional unwrapper
//entitled "Fast two-dimensional phase-unwrapping algorithm based on sorting by 
//reliability following a noncontinuous path"
//by  Miguel Arevallilo HerraŽez, David R. Burton, Michael J. Lalor, and Munther A. Gdeisat
//published in the Journal Applied Optics, Vol. 41, No. 35, pp. 7437, 2002.
//This program was written by Munther Gdeisat, Liverpool John Moores University, United Kingdom.
//Date 18th August 2007
//The wrapped phase map is assumed to be of floating point data type. The resultant unwrapped phase map is also of floating point type.
//This program takes into consideration the image wrap around problem encountered in MRI imaging.
*/
public class PhaseUnwrap2D {
	
	private boolean x_connectivity;
	private boolean y_connectivity;
	private int edges_counter = 0;
	private int No_of_Edges = 0;	
	
	private int image_width;
	private int image_height;
	
	private PixelInfo pixels[];
	private EdgeInfo edges[];
	
	private double img[];

	/** pixel information */
	private class PixelInfo {
		int increment;			//No. of 2*pi to add to the pixel to unwrap it
	    int number_of_pixels_in_group;	//No. of pixels in the pixel group
	    //double value;			//value of the pixel
	    double reliability;
	    /** pointer to the first pixel in the group in the linked list */
	    PixelInfo head;		
	    /** ointer to the last pixel in the group */
	    PixelInfo last;		
	    /** pointer to the next pixel in the group */
	    PixelInfo next;		
	};


	/** the EDGE is the line that connects two pixels.
	  if we have S pixels, then we have S horizontal edges and S vertical edges */
	private class EdgeInfo {    
		/** reliabilty of the edges and it depends on the two pixels */
		double reliab;			
		/** pinter to the first pixel */
		int pointer_1;	
		/** pointer to the second pixel */
		int pointer_2;	
		/** No. of 2*pi to add to one of the pixels to unwrap it with respect to the second */
	    int increment;			 
	};

	private final double find_pivot(EdgeInfo edges[], int left, int right) {
		int a, b, c;

		a = left;
		b = left + (right - left) /2;
		c = right;

		if (edges[a].reliab > edges[b].reliab){ int t = a; a = b; b = t; }
		if (edges[a].reliab > edges[c].reliab){ int t = a; a = c; c = t; }
		if (edges[b].reliab > edges[c].reliab){ int t = b; b = c; c = t; }
		
		if (edges[a].reliab < edges[b].reliab) {
			return edges[b].reliab;
		}

		if (edges[b].reliab < edges[c].reliab) {
			return edges[c].reliab;
		}

		for (int p = left + 1; p <= right; ++p) {
			if (edges[p].reliab != edges[left].reliab) {
				return (edges[p].reliab < edges[left].reliab) ? edges[left].reliab : edges[p].reliab;
			}
			return Double.NaN;
		}

		return Double.NaN; //oliford: This wasn't here, which gave a warning. Not sure what to do with it.
	}

	private final int partition(EdgeInfo edges[], int left, int right, double pivot) {
		while (left <= right)
		{
			while (edges[left].reliab < pivot)
				++left;
			while (edges[right].reliab >= pivot)
				--right;
			
			if (left < right) {
				EdgeInfo tmp = edges[left];
				edges[left] = edges[right];
				edges[right] = tmp;
				++left;
				--right;
			}
		}
		return left;
	}

	private final void quicker_sort(EdgeInfo edges[], int left, int right) {
		double pivot = find_pivot(edges, left, right);

		if (!Double.isNaN(pivot)) {
			int p = partition(edges, left, right, pivot);
			quicker_sort(edges, left, p - 1);
			quicker_sort(edges, p, right);
		}
	}

	//--------------end quicker_sort algorithm -----------------------------------

	/** --------------------start initialse pixels ----------------------------------
	//initialse pixels. See the explination of the pixel class above.
	//initially every pixel is assumed to belong to a group consisting of only itself */
	private final void initialisePIXELs() {
		int pixelIndex = 0;
		int i, j;

	    for (i=0; i < image_height; i++) {
			for (j=0; j < image_width; j++){
				pixels[pixelIndex] = new PixelInfo();
				pixels[pixelIndex].increment = 0;
				pixels[pixelIndex].number_of_pixels_in_group = 1;
				pixels[pixelIndex].reliability = 9999999. + pixelIndex; //RandomManager.instance().nextUniform(0, 1);
				pixels[pixelIndex].head = pixels[pixelIndex];
				pixels[pixelIndex].last = pixels[pixelIndex];
				pixels[pixelIndex].next = null;			
				pixelIndex++;
	         }
		}
	}
	//-------------------end initialise pixels -----------

	/** gamma function in the paper */
	private final double wrap(double pixel_value) {
		double wrapped_pixel_value;
		if (pixel_value > Math.PI)	
			wrapped_pixel_value = pixel_value - 2*Math.PI;
		
		else if (pixel_value < -Math.PI)	
			wrapped_pixel_value = pixel_value + 2*Math.PI;
		
		else 
			wrapped_pixel_value = pixel_value;
		
		return wrapped_pixel_value;
	}

	// pixelL_value is the left pixel,	pixelR_value is the right pixel
	private final int find_wrap(double pixelL_value, double pixelR_value) {
		int wrap_value;
		double difference = pixelL_value - pixelR_value;

		if (difference > Math.PI)	
			wrap_value = -1;
		
		else if (difference < -Math.PI)	
			wrap_value = 1;
		else wrap_value = 0;

		return wrap_value;
	}

	private final void calculate_reliability() {
		int pixel_index = (image_width+1);
		int WIindex = (image_width+1); //WIP is the wrapped image pointer
		double H, V, D1, D2;
		int i, j;
		
		for (i = 1; i < image_height -1; ++i)
		{
			for (j = 1; j < image_width - 1; ++j)
			{
				H = wrap(img[WIindex-1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex+1]);
				V = wrap(img[WIindex - image_width] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + image_width]);
				D1 = wrap(img[WIindex - (image_width+1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width+1)]);
				D2 = wrap(img[WIindex - (image_width-1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width-1)]);
				pixels[pixel_index].reliability = H*H + V*V + D1*D1 + D2*D2;
				pixel_index++;
				WIindex++;
			}
			pixel_index += 2;
			WIindex += 2;
		}

		if (x_connectivity) {
			//calculating the reliability for the left border of the image
			pixel_index = image_width;
			WIindex = image_width; 
		
			for (i = 1; i < image_height - 1; ++i)
			{
				H = wrap(img[WIindex + image_width - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + 1]);
				V = wrap(img[WIindex - image_width] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + image_width]);
				D1 = wrap(img[WIindex - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width+1)]);
				D2 = wrap(img[WIindex - (image_width-1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + 2* image_width - 1]);
				pixels[pixel_index].reliability = H*H + V*V + D1*D1 + D2*D2;
				pixel_index += image_width;
				WIindex += image_width;
			}

			//calculating the reliability for the right border of the image
			pixel_index = 2 * image_width - 1;
			WIindex = 2 * image_width - 1; 
		
			for (i = 1; i < image_height - 1; ++i)
			{
				H = wrap(img[WIindex - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex - (image_width-1)]);
				V = wrap(img[WIindex - image_width] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + image_width]);
				D1 = wrap(img[WIindex - (image_width+1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + 1]);
				D2 = wrap(img[WIindex - 2 * image_width + 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width-1)]);
				pixels[pixel_index].reliability = H*H + V*V + D1*D1 + D2*D2;
				pixel_index += image_width;
				WIindex += image_width;
			}
		}

		if (y_connectivity) {
			//calculating the reliability for the top border of the image
			pixel_index = 1;
			WIindex = 1; 
		
			for (i = 1; i < image_width - 1; ++i) {
				H =  wrap(img[WIindex - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + 1]);
				V =  wrap(img[WIindex + image_width*(image_height - 1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + image_width]);
				D1 = wrap(img[WIindex + image_width*(image_height - 1) - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width+1)]);
				D2 = wrap(img[WIindex + image_width*(image_height - 1) + 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + (image_width-1)]);
				pixels[pixel_index].reliability = H*H + V*V + D1*D1 + D2*D2;
				pixel_index++;
				WIindex++;
			}

			//calculating the reliability for the bottom border of the image
			pixel_index = (image_height - 1) * image_width + 1;
			WIindex = (image_height - 1) * image_width + 1; 
		
			for (i = 1; i < image_width - 1; ++i) {
				H =  wrap(img[WIindex - 1] - img[WIindex]) - wrap(img[WIindex] - img[WIindex + 1]);
				V =  wrap(img[WIindex - image_width] - img[WIindex]) - wrap(img[WIindex] - img[WIindex -(image_height - 1) * (image_width)]);
				D1 = wrap(img[WIindex - (image_width+1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex - (image_height - 1) * (image_width) + 1]);
				D2 = wrap(img[WIindex - (image_width-1)] - img[WIindex]) - wrap(img[WIindex] - img[WIindex - (image_height - 1) * (image_width) - 1]);
				pixels[pixel_index].reliability = H*H + V*V + D1*D1 + D2*D2;
				pixel_index++;
				WIindex++;
			}
		}
	}

	/** calculate the reliability of the horizontal EDGEs of the image
	//it is calculated by adding the reliability of pixels and the relibility of 
	//its right-hand neighbour
	//edge is calculated between a pixel and its right-hand neighbour */
	private final void horizontalEDGEs() {
		int i, j;
		int edge_index = 0;
		int pixel_index = 0;
		
		for (i = 0; i < image_height; i++)
		{
			for (j = 0; j < image_width - 1; j++) 
			{
				edges[edge_index] = new EdgeInfo();
				edges[edge_index].pointer_1 = pixel_index;
				edges[edge_index].pointer_2 = (pixel_index+1);
				edges[edge_index].reliab = pixels[pixel_index].reliability + pixels[pixel_index + 1].reliability;
				edges[edge_index].increment = find_wrap(img[pixel_index], img[pixel_index + 1]);
				pixel_index++;
				edge_index++;
				edges_counter++;
			}
			pixel_index++;
		}

		if (x_connectivity)
		{
			pixel_index = image_width - 1;
			for (i = 0; i < image_height; i++)
			{
				edges[edge_index] = new EdgeInfo();
				edges[edge_index].pointer_1 = pixel_index;
				edges[edge_index].pointer_2 = (pixel_index - image_width + 1);
				edges[edge_index].reliab = pixels[pixel_index].reliability + pixels[pixel_index - image_width + 1].reliability;
				edges[edge_index].increment = find_wrap(img[pixel_index], img[pixel_index  - image_width + 1]);
				pixel_index += image_width;
				edge_index++;
				edges_counter++;
			}
		}
	}

	/** calculate the reliability of the vertical edges of the image
	//it is calculated by adding the reliability of pixels and the relibility of 
	//its lower neighbour in the image. */
	private final void verticalEDGEs() {
		int i, j;
		
		int pixel_index = 0;
		int edge_index = edges_counter;

		for (i=0; i<image_height - 1; i++) {
			for (j=0; j < image_width; j++) {
				edges[edge_index] = new EdgeInfo();
				edges[edge_index].pointer_1 = pixel_index;
				edges[edge_index].pointer_2 = (pixel_index + image_width);
				edges[edge_index].reliab = pixels[pixel_index].reliability + pixels[pixel_index + image_width].reliability;
				edges[edge_index].increment = find_wrap(img[pixel_index], img[pixel_index + image_width]);
				pixel_index++;
				edge_index++;
				edges_counter++;
			} //j loop
		} // i loop

		if (y_connectivity)
		{
			pixel_index = image_width *(image_height - 1);
			for (i = 0; i < image_width; i++)
			{
				edges[edge_index] = new EdgeInfo();
				edges[edge_index].pointer_1 = pixel_index;
				edges[edge_index].pointer_2 = (pixel_index - image_width *(image_height - 1));
				edges[edge_index].reliab = pixels[pixel_index].reliability + pixels[pixel_index - image_width *(image_height - 1)].reliability;
				edges[edge_index].increment = find_wrap(img[pixel_index], img[pixel_index - image_width *(image_height - 1)]);
				pixel_index++;
				edge_index++;
				edges_counter++;
			}
		}
	}

	/** gather the pixels of the image into groups */ 
	private final void gatherPIXELs() {
		//Number of rialiable edges (not at the borders of the image)
		//int no_EDGEs = No_of_Edges;//(image_width - 1) * (image_height) + (image_width) * (image_height - 1); 
		int PIXEL1;   
		int PIXEL2;

		int pointer_edge = 0;
		int incremento;

		for (int k = 0; k < No_of_Edges; k++) {
			PIXEL1 = edges[pointer_edge].pointer_1;
			PIXEL2 = edges[pointer_edge].pointer_2;

			//PIXEL 1 and PIXEL 2 belong to different groups
			//initially each pixel is in a group by itself and one pixel can construct a group
			//no else or else if to this if
			if (pixels[PIXEL2].head != pixels[PIXEL1].head) {
				//PIXEL 2 is alone in its group
				//merge this pixel with PIXEL 1 group and find the number of 2 pi to add 
				//to or subtract to unwrap it
				if ((pixels[PIXEL2].next == null) && (pixels[PIXEL2].head == pixels[PIXEL2])) {
					
					pixels[PIXEL1].head.last.next = pixels[PIXEL2];
					pixels[PIXEL1].head.last = pixels[PIXEL2];
					(pixels[PIXEL1].head.number_of_pixels_in_group)++;
					pixels[PIXEL2].head = pixels[PIXEL1].head;
					pixels[PIXEL2].increment = pixels[PIXEL1].increment - edges[pointer_edge].increment;
					
				} else if ((pixels[PIXEL1].next == null) && (pixels[PIXEL1].head == pixels[PIXEL1])) {
					//PIXEL 1 is alone in its group
					//merge this pixel with PIXEL 2 group and find the number of 2 pi to add 
					//to or subtract to unwrap it
					
					pixels[PIXEL2].head.last.next = pixels[PIXEL1];
					pixels[PIXEL2].head.last = pixels[PIXEL1];
					(pixels[PIXEL2].head.number_of_pixels_in_group)++;
					pixels[PIXEL1].head = pixels[PIXEL2].head;
					pixels[PIXEL1].increment = pixels[PIXEL2].increment + edges[pointer_edge].increment;
				
				} else { //PIXEL 1 and PIXEL 2 both have groups
					PixelInfo group1 = pixels[PIXEL1].head;
					PixelInfo group2 = pixels[PIXEL2].head;
					//if the no. of pixels in PIXEL 1 group is larger than the no. of pixels
					//in PIXEL 2 group.   Merge PIXEL 2 group to PIXEL 1 group
					//and find the number of wraps between PIXEL 2 group and PIXEL 1 group
					//to unwrap PIXEL 2 group with respect to PIXEL 1 group.
					//the no. of wraps will be added to PIXEL 2 grop in the future
					if (group1.number_of_pixels_in_group > group2.number_of_pixels_in_group) {
						//merge PIXEL 2 with PIXEL 1 group
						group1.last.next = group2;
						group1.last = group2.last;
						group1.number_of_pixels_in_group = group1.number_of_pixels_in_group + group2.number_of_pixels_in_group;
						incremento = pixels[PIXEL1].increment-edges[pointer_edge].increment - pixels[PIXEL2].increment;
						//merge the other pixels in PIXEL 2 group to PIXEL 1 group
						while (group2 != null)
						{
							group2.head = group1;
							group2.increment += incremento;
							group2 = group2.next;
						}
					} 

					//if the no. of pixels in PIXEL 2 group is larger than the no. of pixels
					//in PIXEL 1 group.   Merge PIXEL 1 group to PIXEL 2 group
					//and find the number of wraps between PIXEL 2 group and PIXEL 1 group
					//to unwrap PIXEL 1 group with respect to PIXEL 2 group.
					//the no. of wraps will be added to PIXEL 1 grop in the future
					else
	                {
						//merge PIXEL 1 with PIXEL 2 group
						group2.last.next = group1;
						group2.last = group1.last;
						group2.number_of_pixels_in_group = group2.number_of_pixels_in_group + group1.number_of_pixels_in_group;
						incremento = pixels[PIXEL2].increment + edges[pointer_edge].increment - pixels[PIXEL1].increment;
						//merge the other pixels in PIXEL 2 group to PIXEL 1 group
						while (group1 != null) {
							group1.head = group2;
							group1.increment += incremento;
							group1 = group1.next;
						} // while

	                } // else
	            } //else
	        } ;//if

	        pointer_edge++;
		}
	}

	//unwrap the image 
	private final void applyUnwrap() {
		int image_size = image_width * image_height;

		for (int i = 0; i < image_size; i++)
			img[i] += 2*Math.PI * (double)(pixels[i].increment);
	}
	
	public PhaseUnwrap2D(double img[], int width, int height, boolean x_connectivity, boolean y_connectivity) {
		this.image_width = width;
		this.image_height = height;
		this.x_connectivity = x_connectivity;
		this.y_connectivity = y_connectivity;
		this.img = img;
		
		int image_size = image_height * image_width;

		if (!x_connectivity && !y_connectivity)
			//horizontal + vertical edges
			No_of_Edges = (image_width - 1)*(image_height) + (image_height - 1)*(image_width);

		else if (x_connectivity && !y_connectivity)
			No_of_Edges = (image_width)*(image_height) + (image_height - 1)*(image_width);
		
		else if (!x_connectivity && y_connectivity)
			No_of_Edges = (image_width - 1)*(image_height) + (image_height)*(image_width);

		else // (x_connectivity && y_connectivity)
			No_of_Edges = (image_width)*(image_height) + (image_height)*(image_width);


		pixels = new PixelInfo[image_size];
		edges = new EdgeInfo[No_of_Edges]; 
				
		initialisePIXELs();
		calculate_reliability();
		horizontalEDGEs();
		verticalEDGEs();

		//sort the EDGEs depending on their reiability. The PIXELs with higher relibility (small value) first
		quicker_sort(edges, 0, No_of_Edges - 1);

		//gather PIXELs into groups
		gatherPIXELs();

		//unwrap the whole image
		applyUnwrap();
	}
	
	public double[] getImage(){ return img; }
	
	public static double[] unwrap(double img[], int width, int height, boolean x_connectivity, boolean y_connectivity){
		return (new PhaseUnwrap2D(img, width, height, x_connectivity, y_connectivity)).getImage();
	}

	//simple test with provided map
	public static void main(String args[]){  
	
		double img[][] = BinaryMatrixFile.mustLoad("csrc/phaseUnwrap2D/wrapped-test.bin", false);
		int h = img.length;
		int w = img[0].length;
		
		double wrapped[] = OneLiners.flatten(img);
		
		double unwrapped[] = unwrap(wrapped, w, h, true, true);

		BinaryMatrixFile.mustWrite("csrc/phaseUnwrap2D/unwrapped-checkJava.bin", 
				OneLiners.unflatten(unwrapped, h, w), false);

	}
}