// 2D Phase unwrapped reading and writing my standard BMF files. 
// 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.
//
// I have no clue how this works, but it does.
//
// 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.


#include <malloc.h>
//#include "stdafx.h"
#include<stdio.h>
#include <stdlib.h>
#include <string.h>
#include "binaryMatrixFile.h"

static float PI = 3.141592654;
static float TWOPI = 6.283185307;
int x_connectivity = 1;
int y_connectivity = 1;
int edges_counter = 0;
int No_of_Edges = 0;

//pixel information
struct PIXELSTRUCT
{
	//int x;					//x coordinate of the pixel
        //int y;					//y coordinate
    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
    float value;			//value of the pixel
    float reliability;
    int group;				//group No.
    int new_group;
    struct PIXELSTRUCT *head;		//pointer to the first pixel in the group in the linked list
    struct PIXELSTRUCT *last;		//pointer to the last pixel in the group
    struct PIXELSTRUCT *next;		//pointer to the next pixel in the group
};


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

typedef struct PIXELSTRUCT PIXEL;
typedef struct EDGESTRUCT EDGE;

//---------------start quicker_sort algorithm --------------------------------
#define swap(x,y) {EDGE t; t=x; x=y; y=t;}
#define order(x,y) if (x.reliab > y.reliab) swap(x,y)
#define o2(x,y) order(x,y)
#define o3(x,y,z) o2(x,y); o2(x,z); o2(y,z)

typedef enum {yes, no} yes_no;
// oliford: ... really? I'd call it reinventing the wheel, but I think reinventing true and false is worse.

yes_no find_pivot(EDGE *left, EDGE *right, float *pivot_ptr)
{
	EDGE a, b, c, *p;

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

	if (a.reliab < b.reliab)
	{
		*pivot_ptr = b.reliab;
		return yes;
	}

	if (b.reliab < c.reliab)
	{
		*pivot_ptr = c.reliab;
		return yes;
	}

	for (p = left + 1; p <= right; ++p)
	{
		if (p->reliab != left->reliab)
		{
			*pivot_ptr = (p->reliab < left->reliab) ? left->reliab : p->reliab;
			return yes;
		}
		return no;
	}

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

EDGE *partition(EDGE *left, EDGE *right, float pivot)
{
	while (left <= right)
	{
		while (left->reliab < pivot)
			++left;
		while (right->reliab >= pivot)
			--right;
		if (left < right)
		{
			swap (*left, *right);
			++left;
			--right;
		}
	}
	return left;
}

void quicker_sort(EDGE *left, EDGE *right)
{
	EDGE *p;
	float pivot;
	if (find_pivot(left, right, &pivot) == yes)
	{
		p = partition(left, right, pivot);
		quicker_sort(left, p - 1);
		quicker_sort(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
void  initialisePIXELs(float *WrappedImage, PIXEL *pixel, int image_width, int image_height)
{
	PIXEL *pixel_pointer = pixel;
	float *wrapped_image_pointer = WrappedImage;
	int i, j;

    for (i=0; i < image_height; i++)
	{
		for (j=0; j < image_width; j++)
        {
			//pixel_pointer->x = j;
  			//pixel_pointer->y = i;
			pixel_pointer->increment = 0;
			pixel_pointer->number_of_pixels_in_group = 1;		
  			pixel_pointer->value = *wrapped_image_pointer;
			pixel_pointer->reliability = 9999999. + ((int)(pixel_pointer - pixel)); //rand();
			pixel_pointer->head = pixel_pointer;
  			pixel_pointer->last = pixel_pointer;
			pixel_pointer->next = NULL;			
            pixel_pointer->new_group = 0;
            pixel_pointer->group = -1;
            pixel_pointer++;
            wrapped_image_pointer++;
         }
	}
}
//-------------------end initialise pixels -----------

//gamma function in the paper
float wrap(float pixel_value)
{
	float wrapped_pixel_value;
	if (pixel_value > PI)	wrapped_pixel_value = pixel_value - TWOPI;
	else if (pixel_value < -PI)	wrapped_pixel_value = pixel_value + TWOPI;
	else wrapped_pixel_value = pixel_value;
	return wrapped_pixel_value;
}

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

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

	return wrap_value;
} 

void calculate_reliability(float *wrappedImage, PIXEL *pixel, int image_width, int image_height)
{
	int image_width_plus_one = image_width + 1;
	int image_width_minus_one = image_width - 1;
	PIXEL *pixel_pointer = pixel + image_width_plus_one;
	float *WIP = wrappedImage + image_width_plus_one; //WIP is the wrapped image pointer
	float 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(*(WIP - 1) - *WIP) - wrap(*WIP - *(WIP + 1));
			V = wrap(*(WIP - image_width) - *WIP) - wrap(*WIP - *(WIP + image_width));
			D1 = wrap(*(WIP - image_width_plus_one) - *WIP) - wrap(*WIP - *(WIP + image_width_plus_one));
			D2 = wrap(*(WIP - image_width_minus_one) - *WIP) - wrap(*WIP - *(WIP + image_width_minus_one));
			pixel_pointer->reliability = H*H + V*V + D1*D1 + D2*D2;
			pixel_pointer++;
			WIP++;
		}
		pixel_pointer+=2;
		WIP+=2;
	}

	if (x_connectivity == 1)
	{
		//calculating the reliability for the left border of the image
		PIXEL *pixel_pointer = pixel + image_width;
		float *WIP = wrappedImage + image_width; 
	
		for (i = 1; i < image_height - 1; ++i)
		{
			H = wrap(*(WIP + image_width - 1) - *WIP) - wrap(*WIP - *(WIP + 1));
			V = wrap(*(WIP - image_width) - *WIP) - wrap(*WIP - *(WIP + image_width));
			D1 = wrap(*(WIP - 1) - *WIP) - wrap(*WIP - *(WIP + image_width_plus_one));
			D2 = wrap(*(WIP - image_width_minus_one) - *WIP) - wrap(*WIP - *(WIP + 2* image_width - 1));
			pixel_pointer->reliability = H*H + V*V + D1*D1 + D2*D2;
			pixel_pointer += image_width;
			WIP += image_width;
		}

		//calculating the reliability for the right border of the image
		pixel_pointer = pixel + 2 * image_width - 1;
		WIP = wrappedImage + 2 * image_width - 1; 
	
		for (i = 1; i < image_height - 1; ++i)
		{
			H = wrap(*(WIP - 1) - *WIP) - wrap(*WIP - *(WIP - image_width_minus_one));
			V = wrap(*(WIP - image_width) - *WIP) - wrap(*WIP - *(WIP + image_width));
			D1 = wrap(*(WIP - image_width_plus_one) - *WIP) - wrap(*WIP - *(WIP + 1));
			D2 = wrap(*(WIP - 2 * image_width + 1) - *WIP) - wrap(*WIP - *(WIP + image_width_minus_one));
			pixel_pointer->reliability = H*H + V*V + D1*D1 + D2*D2;
			pixel_pointer += image_width;
			WIP += image_width;
		}

	}

	if (y_connectivity == 1)
	{
		//calculating the reliability for the top border of the image
		PIXEL *pixel_pointer = pixel + 1;
		float *WIP = wrappedImage + 1; 
	
		for (i = 1; i < image_width - 1; ++i) {

			H =  wrap(*(WIP - 1) - *WIP) - wrap(*WIP - *(WIP + 1));
			V =  wrap(*(WIP + image_width*(image_height - 1)) - *WIP) - wrap(*WIP - *(WIP + image_width));
			D1 = wrap(*(WIP + image_width*(image_height - 1) - 1) - *WIP) - wrap(*WIP - *(WIP + image_width_plus_one));
			D2 = wrap(*(WIP + image_width*(image_height - 1) + 1) - *WIP) - wrap(*WIP - *(WIP + image_width_minus_one));

			pixel_pointer->reliability = H*H + V*V + D1*D1 + D2*D2;
			pixel_pointer++;
			WIP++;
		}

		//calculating the reliability for the bottom border of the image
		pixel_pointer = pixel + (image_height - 1) * image_width + 1;
		WIP = wrappedImage + (image_height - 1) * image_width + 1; 
	
		for (i = 1; i < image_width - 1; ++i)
		{
			H =  wrap(*(WIP - 1) - *WIP) - wrap(*WIP - *(WIP + 1));
			V =  wrap(*(WIP - image_width) - *WIP) - wrap(*WIP - *(WIP -(image_height - 1) * (image_width)));
			D1 = wrap(*(WIP - image_width_plus_one) - *WIP) - wrap(*WIP - *(WIP - (image_height - 1) * (image_width) + 1));
			D2 = wrap(*(WIP - image_width_minus_one) - *WIP) - wrap(*WIP - *(WIP - (image_height - 1) * (image_width) - 1));
			pixel_pointer->reliability = H*H + V*V + D1*D1 + D2*D2;
			pixel_pointer++;
			WIP++;
		}
	}
}

//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
void  horizontalEDGEs(PIXEL *pixel, EDGE *edge, int image_width, int image_height)
{
	int i, j;
	EDGE *edge_pointer = edge;
	PIXEL *pixel_pointer = pixel;
	
	for (i = 0; i < image_height; i++)
	{
		for (j = 0; j < image_width - 1; j++) 
		{
			edge_pointer->pointer_1 = pixel_pointer;
			edge_pointer->pointer_2 = (pixel_pointer+1);
			edge_pointer->reliab = pixel_pointer->reliability + (pixel_pointer + 1)->reliability;
			edge_pointer->increment = find_wrap(pixel_pointer->value, (pixel_pointer + 1)->value);
			pixel_pointer++;
			edge_pointer++;
			edges_counter++;
		}
		pixel_pointer++;
	}

	if (x_connectivity == 1)
	{
		pixel_pointer = pixel + image_width - 1;
		for (i = 0; i < image_height; i++)
		{
			edge_pointer->pointer_1 = pixel_pointer;
			edge_pointer->pointer_2 = (pixel_pointer - image_width + 1);
			edge_pointer->reliab = pixel_pointer->reliability + (pixel_pointer - image_width + 1)->reliability;
			edge_pointer->increment = find_wrap(pixel_pointer->value, (pixel_pointer  - image_width + 1)->value);
			pixel_pointer+=image_width;
			edge_pointer++;
			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.
void  verticalEDGEs(PIXEL *pixel, EDGE *edge, int image_width, int image_height)
{
	int i, j;
	
	PIXEL *pixel_pointer = pixel;
	EDGE *edge_pointer = edge + edges_counter;

	for (i=0; i<image_height - 1; i++)
	{
		for (j=0; j < image_width; j++) 
		{
			edge_pointer->pointer_1 = pixel_pointer;
			edge_pointer->pointer_2 = (pixel_pointer + image_width);
			edge_pointer->reliab = pixel_pointer->reliability + (pixel_pointer + image_width)->reliability;
			edge_pointer->increment = find_wrap(pixel_pointer->value, (pixel_pointer + image_width)->value);
			pixel_pointer++;
			edge_pointer++;
			edges_counter++;
		} //j loop
	} // i loop

	if (y_connectivity == 1)
	{
		pixel_pointer = pixel + image_width *(image_height - 1);
		for (i = 0; i < image_width; i++)
		{
			edge_pointer->pointer_1 = pixel_pointer;
			edge_pointer->pointer_2 = (pixel_pointer - image_width *(image_height - 1));
			edge_pointer->reliab = pixel_pointer->reliability + (pixel_pointer - image_width *(image_height - 1))->reliability;
			edge_pointer->increment = find_wrap(pixel_pointer->value, (pixel_pointer - image_width *(image_height - 1))->value);
			pixel_pointer++;
			edge_pointer++;
			edges_counter++;
		}
	}
}

//gather the pixels of the image into groups 
void  gatherPIXELs(EDGE *edge, int image_width, int image_height)
{
	int k;
	
	//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); 
	PIXEL *PIXEL1;   
	PIXEL *PIXEL2;

	PIXEL *group1;
	PIXEL *group2;
	EDGE *pointer_edge = edge;
	int incremento;

	for (k = 0; k < No_of_Edges; k++)
	{
		PIXEL1 = pointer_edge->pointer_1;
		PIXEL2 = 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 (PIXEL2->head != 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 ((PIXEL2->next == NULL) && (PIXEL2->head == PIXEL2))
			{
				PIXEL1->head->last->next = PIXEL2;
				PIXEL1->head->last = PIXEL2;
				(PIXEL1->head->number_of_pixels_in_group)++;
				PIXEL2->head=PIXEL1->head;
				PIXEL2->increment = PIXEL1->increment-pointer_edge->increment;
			}

			//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
			else if ((PIXEL1->next == NULL) && (PIXEL1->head == PIXEL1))
			{
				PIXEL2->head->last->next = PIXEL1;
				PIXEL2->head->last = PIXEL1;
				(PIXEL2->head->number_of_pixels_in_group)++;
				PIXEL1->head = PIXEL2->head;
				PIXEL1->increment = PIXEL2->increment+pointer_edge->increment;
			} 

			//PIXEL 1 and PIXEL 2 both have groups
			else
            {
				group1 = PIXEL1->head;
                group2 = 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 = PIXEL1->increment-pointer_edge->increment - 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 = PIXEL2->increment + pointer_edge->increment - 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 
void  unwrapImage(PIXEL *pixel, int image_width, int image_height)
{
	int i;
	int image_size = image_width * image_height;
	PIXEL *pixel_pointer=pixel;


	for (i = 0; i < image_size; i++)
	{
		pixel_pointer->value += TWOPI * (float)(pixel_pointer->increment);
        pixel_pointer++;
    }
}

//the input to this unwrapper is an array that contains the wrapped phase map. 
//copy the image on the buffer passed to this unwrapper to over-write the unwrapped 
//phase map on the buffer of the wrapped phase map.
void  returnImage(PIXEL *pixel, float *unwrappedImage, int image_width, int image_height)
{
	int i;
	int image_size = image_width * image_height;
    float *unwrappedImage_pointer = unwrappedImage;
    PIXEL *pixel_pointer = pixel;

    for (i=0; i < image_size; i++) 
	{
        *unwrappedImage_pointer = pixel_pointer->value;
        pixel_pointer++;
		unwrappedImage_pointer++;
	}
}

//the main function of the unwrapper
int main(int argc, char *argv[])
{  
	double *dblImage;
	float *WrappedImage, *UnwrappedImage;
	int image_width;
	int image_height;
	int image_size;
	int i,ret;

	if(argc < 3){
		printf("Usage: %s infile outfile\n\n", argv[0]);
		return -1;
	}

	ret = bmfLoad(argv[1], &image_height, &image_width, &dblImage, 0);
	if(ret){ printf("Could not read binary matrix file '%s': ret=%i\n", argv[1], ret); return ret; }


	image_size = image_height * image_width;

	printf("Loaded image %i x %i\n",image_height, image_width);
	WrappedImage = (float *) calloc(image_size, sizeof(float));
	for(i=0; i < image_size; i++)
		WrappedImage[i] = (float)dblImage[i];
		

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

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

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

	UnwrappedImage = (float *) calloc(image_size, sizeof(float));
	PIXEL *pixel = (PIXEL *) calloc(image_size, sizeof(PIXEL));
	EDGE *edge = (EDGE *) calloc(No_of_Edges, sizeof(EDGE));
	
	initialisePIXELs(WrappedImage, pixel, image_width, image_height);

	calculate_reliability(WrappedImage, pixel, image_width, image_height);

	horizontalEDGEs(pixel, edge, image_width, image_height);

	verticalEDGEs(pixel, edge, image_width, image_height);

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

	//gather PIXELs into groups
	gatherPIXELs(edge, image_width, image_height);

	//unwrap the whole image
	unwrapImage(pixel, image_width, image_height);
	//copy the image from PIXEL structure to the wrapped phase array passed to this function
	returnImage(pixel, UnwrappedImage, image_width, image_height);

	free(edge);
	free(pixel);
	
	for(i=0; i < image_size; i++)
		dblImage[i] = (double)UnwrappedImage[i];

	ret = bmfWrite(argv[2], image_height, image_width, NULL, NULL, dblImage, 0);
	if(ret){ printf("Could not write to binary matrix file '%s': ret=%i\n", argv[2], ret); }

	free(UnwrappedImage);
	free(WrappedImage);
	free(dblImage);

	return ret;
}