unwrap2D.c

/* 2D phase unwrapping, modified for inclusion in scipy by Gregor Thalhammer

This program was written by Munther Gdeisat and Miguel Arevallilo Herraez 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 Herraez, 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 26th 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.
The mask is of byte data type.
When the mask is 255 this means that the pixel is valid
When the mask is 0 this means that the pixel is invalid (noisy or corrupted pixel)
This program takes into consideration the image wrap around problem encountered in MRI imaging. */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "mex.h"

/*
TODO: remove global variables
TODO: make thresholds independent
*/

static double PI = 3.141592654f;
static double TWOPI = 6.283185307f;

#define NOMASK 0
#define MASK 1

typedef struct
{
  double mod;
  int x_connectivity;
  int y_connectivity;
  int no_of_edges;
} params_t;

struct PIXELM
{
  int increment;		/*No. of 2*pi to add to the pixel to unwrap it*/
  int number_of_pixels_in_group; /*No. of pixel in the pixel group*/
  double value;			/*value of the pixel*/
  double reliability;
  unsigned char input_mask;	/*0 pixel is masked. NOMASK pixel is not masked*/
  unsigned char extended_mask;	/*0 pixel is masked. NOMASK pixel is not masked*/
  int group;			/*group No.*/
  int new_group;
  struct PIXELM *head;		/*pointer to the first pixel in the group in the linked list*/
  struct PIXELM *last;		/*pointer to the last pixel in the group*/
  struct PIXELM *next;		/*pointer to the next pixel in the group*/
};

typedef struct PIXELM PIXELM;
/*
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 EDGE
{
  double reliab;			    /*reliabilty of the edge and it depends on the two pixels*/
  PIXELM *pointer_1;		/*pointer to the first pixel*/
  PIXELM *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 EDGE 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;

yes_no find_pivot(EDGE *left, EDGE *right, double *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;
}

EDGE *partition(EDGE *left, EDGE *right, double 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;
  double 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 initialize pixels ----------------------------------*/
/*initialize 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(double *wrapped_image, unsigned char *input_mask, unsigned char *extended_mask, PIXELM *pixel, int image_width, int image_height)
{
  PIXELM *pixel_pointer = pixel;
  double *wrapped_image_pointer = wrapped_image;
  unsigned char *input_mask_pointer = input_mask;
  unsigned char *extended_mask_pointer = extended_mask;
  int i, j;

  for (i=0; i < image_height; i++)
    {
      for (j=0; j < image_width; j++)
	{
	  pixel_pointer->increment = 0;
	  pixel_pointer->number_of_pixels_in_group = 1;
	  pixel_pointer->value = *wrapped_image_pointer;
	  pixel_pointer->reliability = 9999999.f + rand();
	  pixel_pointer->input_mask = *input_mask_pointer;
	  pixel_pointer->extended_mask = *extended_mask_pointer;
	  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++;
	  input_mask_pointer++;
	  extended_mask_pointer++;
	}
    }
}
/*-------------------end initialize pixels -----------*/

/*gamma function in the paper*/
double wrap(double pixel_value)
{
  double 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(double pixelL_value, double pixelR_value)
{
  double 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 extend_mask(unsigned char *input_mask, unsigned char *extended_mask,
		 int image_width, int image_height,
		 params_t *params)
{
  int i,j;
  int image_width_plus_one = image_width + 1;
  int image_width_minus_one = image_width - 1;
  unsigned char *IMP = input_mask    + image_width + 1;	/*input mask pointer*/
  unsigned char *EMP = extended_mask + image_width + 1;	/*extended mask pointer*/

  /*extend the mask for the image except borders*/
  for (i=1; i < image_height - 1; ++i)
    {
      for (j=1; j < image_width - 1; ++j)
	{
	  if ( (*IMP) == NOMASK && (*(IMP + 1) == NOMASK) && (*(IMP - 1) == NOMASK) &&
	       (*(IMP + image_width) == NOMASK) && (*(IMP - image_width) == NOMASK) &&
	       (*(IMP - image_width_minus_one) == NOMASK) && (*(IMP - image_width_plus_one) == NOMASK) &&
	       (*(IMP + image_width_minus_one) == NOMASK) && (*(IMP + image_width_plus_one) == NOMASK) )
	    {
	      *EMP = NOMASK;
	    }
	  ++EMP;
	  ++IMP;
	}
      EMP += 2;
      IMP += 2;
    }

  if (params->x_connectivity == 1)
    {
      /*extend the mask for the right border of the image*/
      IMP = input_mask    + 2 * image_width - 1;
      EMP = extended_mask + 2 * image_width -1;
      for (i=1; i < image_height - 1; ++ i)
	{
	  if ( (*IMP) == NOMASK && (*(IMP - 1) == NOMASK) &&  (*(IMP + 1) == NOMASK) &&
	       (*(IMP + image_width) == NOMASK) && (*(IMP - image_width) == NOMASK) &&
	       (*(IMP - image_width - 1) == NOMASK) && (*(IMP - image_width + 1) == NOMASK) &&
	       (*(IMP + image_width - 1) == NOMASK) && (*(IMP - 2 * image_width + 1) == NOMASK) )
	    {
	      *EMP = NOMASK;
	    }
	  EMP += image_width;
	  IMP += image_width;
	}

      /*extend the mask for the left border of the image*/
      IMP = input_mask    + image_width;
      EMP = extended_mask + image_width;
      for (i=1; i < image_height - 1; ++i)
	{
	  if ( (*IMP) == NOMASK && (*(IMP - 1) == NOMASK) && (*(IMP + 1) == NOMASK) &&
	       (*(IMP + image_width) == NOMASK) && (*(IMP - image_width) == NOMASK) &&
	       (*(IMP - image_width + 1) == NOMASK) && (*(IMP + image_width + 1) == NOMASK) &&
	       (*(IMP + image_width - 1) == NOMASK) && (*(IMP + 2 * image_width - 1) == NOMASK) )
	    {
	      *EMP = NOMASK;
	    }
	  EMP += image_width;
	  IMP += image_width;
	}
    }

  if (params->y_connectivity == 1)
    {
      /*extend the mask for the top border of the image*/
      IMP = input_mask    + 1;
      EMP = extended_mask + 1;
      for (i=1; i < image_width - 1; ++i)
	{
	  if ( (*IMP) == NOMASK && (*(IMP - 1) == NOMASK) && (*(IMP + 1) == NOMASK) &&
	       (*(IMP + image_width) == NOMASK) && (*(IMP + image_width * (image_height - 1)) == NOMASK) &&
	       (*(IMP + image_width + 1) == NOMASK) && (*(IMP + image_width - 1) == NOMASK) &&
	       (*(IMP + image_width * (image_height - 1) - 1) == NOMASK) && (*(IMP + image_width * (image_height - 1) + 1) == NOMASK) )
	    {
	      *EMP = NOMASK;
	    }
	  EMP++;
	  IMP++;
	}

      /*extend the mask for the bottom border of the image*/
      IMP = input_mask    + image_width * (image_height - 1) + 1;
      EMP = extended_mask + image_width * (image_height - 1) + 1;
      for (i=1; i < image_width - 1; ++i)
	{
	  if ( (*IMP) == NOMASK && (*(IMP - 1) == NOMASK) && (*(IMP + 1) == NOMASK) &&
	       (*(IMP - image_width) == NOMASK) && (*(IMP - image_width - 1) == NOMASK) && (*(IMP - image_width + 1) == NOMASK) &&
	       (*(IMP - image_width * (image_height - 1)    ) == NOMASK) &&
	       (*(IMP - image_width * (image_height - 1) - 1) == NOMASK) &&
	       (*(IMP - image_width * (image_height - 1) + 1) == NOMASK) )
	    {
	      *EMP = NOMASK;
	    }
	  EMP++;
	  IMP++;
	}
    }
}

void calculate_reliability(double *wrappedImage, PIXELM *pixel,
			   int image_width, int image_height,
			   params_t *params)
{
  int image_width_plus_one = image_width + 1;
  int image_width_minus_one = image_width - 1;
  PIXELM *pixel_pointer = pixel + image_width_plus_one;
  double *WIP = wrappedImage + image_width_plus_one; /*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)
	{
	  if (pixel_pointer->extended_mask == NOMASK)
	    {
	      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 (params->x_connectivity == 1)
    {
      /*calculating the reliability for the left border of the image*/
      PIXELM *pixel_pointer = pixel + image_width;
      double *WIP = wrappedImage + image_width;

      for (i = 1; i < image_height - 1; ++i)
	{
	  if (pixel_pointer->extended_mask == NOMASK)
	    {
	      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)
	{
	  if (pixel_pointer->extended_mask == NOMASK)
	    {
	      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 (params->y_connectivity == 1)
    {
      /*calculating the reliability for the top border of the image*/
      PIXELM *pixel_pointer = pixel + 1;
      double *WIP = wrappedImage + 1;

      for (i = 1; i < image_width - 1; ++i)
	{
	  if (pixel_pointer->extended_mask == NOMASK)
	    {
	      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)
	{
	  if (pixel_pointer->extended_mask == NOMASK)
	    {
	      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 pixel and the relibility of*/
/*its right-hand neighbour*/
/*edge is calculated between a pixel and its next neighbour*/
void  horizontalEDGEs(PIXELM *pixel, EDGE *edge,
		      int image_width, int image_height,
		      params_t *params)
{
  int i, j;
  EDGE *edge_pointer = edge;
  PIXELM *pixel_pointer = pixel;
  int no_of_edges = params->no_of_edges;

  for (i = 0; i < image_height; i++)
    {
      for (j = 0; j < image_width - 1; j++)
	{
	  if (pixel_pointer->input_mask == NOMASK && (pixel_pointer + 1)->input_mask == NOMASK)
	    {
	      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);
	      edge_pointer++;
	      no_of_edges++;
	    }
	  pixel_pointer++;
	}
      pixel_pointer++;
    }
  /*construct edges at the right border of the image*/
  if (params->x_connectivity == 1)
    {
      pixel_pointer = pixel + image_width - 1;
      for (i = 0; i < image_height; i++)
	{
	  if (pixel_pointer->input_mask == NOMASK && (pixel_pointer - image_width + 1)->input_mask == NOMASK)
	    {
	      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);
	      edge_pointer++;
	      no_of_edges++;
	    }
	  pixel_pointer+=image_width;
	}
    }
  params->no_of_edges = no_of_edges;
}

/*calculate the reliability of the vertical edges of the image*/
/*it is calculated by adding the reliability of pixel and the relibility of*/
/*its lower neighbour in the image.*/
void  verticalEDGEs(PIXELM *pixel, EDGE *edge,
		    int image_width, int image_height,
		    params_t *params)
{
  int i, j;
  int no_of_edges = params->no_of_edges;
  PIXELM *pixel_pointer = pixel;
  EDGE *edge_pointer = edge + no_of_edges;

  for (i=0; i < image_height - 1; i++)
    {
      for (j=0; j < image_width; j++)
	{
	  if (pixel_pointer->input_mask == NOMASK && (pixel_pointer + image_width)->input_mask == NOMASK)
	    {
	      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);
	      edge_pointer++;
	      no_of_edges++;
	    }
	  pixel_pointer++;
	} /*j loop*/
    } /* i loop*/

  /*construct edges that connect at the bottom border of the image*/
  if (params->y_connectivity == 1)
    {
      pixel_pointer = pixel + image_width *(image_height - 1);
      for (i = 0; i < image_width; i++)
	{
	  if (pixel_pointer->input_mask == NOMASK && (pixel_pointer - image_width *(image_height - 1))->input_mask == NOMASK)
	    {
	      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);
	      edge_pointer++;
	      no_of_edges++;
	    }
	  pixel_pointer++;
	}
    }
  params->no_of_edges = no_of_edges;
}

/*gather the pixels of the image into groups*/
void  gatherPIXELs(EDGE *edge, params_t *params)
{
  int k;
  PIXELM *PIXEL1;
  PIXELM *PIXEL2;
  PIXELM *group1;
  PIXELM *group2;
  EDGE *pointer_edge = edge;
  int incremento;

  for (k = 0; k < params->no_of_edges; k++)
    {
      PIXEL1 = pointer_edge->pointer_1;
      PIXEL2 = pointer_edge->pointer_2;

      /*PIXELM 1 and PIXELM 2 belong to different groups*/
      /*initially each pixel is a group by it self and one pixel can construct a group*/
      /*no else or else if to this if*/
      if (PIXEL2->head != PIXEL1->head)
	{
	  /*PIXELM 2 is alone in its group*/
	  /*merge this pixel with PIXELM 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;
	    }

	  /*PIXELM 1 is alone in its group*/
	  /*merge this pixel with PIXELM 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;
	    }

	  /*PIXELM 1 and PIXELM 2 both have groups*/
	  else
            {
	      group1 = PIXEL1->head;
	      group2 = PIXEL2->head;
	      /*if the no. of pixels in PIXELM 1 group is larger than the*/
	      /*no. of pixels in PIXELM 2 group.  Merge PIXELM 2 group to*/
	      /*PIXELM 1 group and find the number of wraps between PIXELM 2*/
	      /*group and PIXELM 1 group to unwrap PIXELM 2 group with respect*/
	      /*to PIXELM 1 group.  the no. of wraps will be added to PIXELM 2*/
	      /*group in the future*/
	      if (group1->number_of_pixels_in_group > group2->number_of_pixels_in_group)
		{
		  /*merge PIXELM 2 with PIXELM 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 PIXELM 2 group to PIXELM 1 group*/
		  while (group2 != NULL)
		    {
		      group2->head = group1;
		      group2->increment += incremento;
		      group2 = group2->next;
		    }
		}

	      /*if the no. of pixels in PIXELM 2 group is larger than the*/
	      /*no. of pixels in PIXELM 1 group.  Merge PIXELM 1 group to*/
	      /*PIXELM 2 group and find the number of wraps between PIXELM 2*/
	      /*group and PIXELM 1 group to unwrap PIXELM 1 group with respect*/
	      /*to PIXELM 2 group.  the no. of wraps will be added to PIXELM 1*/
	      /*group in the future*/
	      else
                {
		  /*merge PIXELM 1 with PIXELM 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 PIXELM 2 group to PIXELM 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(PIXELM *pixel, int image_width, int image_height)
{
  int i;
  int image_size = image_width * image_height;
  PIXELM *pixel_pointer=pixel;

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

/*set the masked pixels (mask = 0) to the minimum of the unwrapper phase*/
void  maskImage(PIXELM *pixel, unsigned char *input_mask, int image_width, int image_height)
{
  PIXELM *pointer_pixel = pixel;
  unsigned char *IMP = input_mask;	/*input mask pointer*/
  double min=99999999.f;
  int i;
  int image_size = image_width * image_height;

  /*find the minimum of the unwrapped phase*/
  for (i = 0; i < image_size; i++)
    {
      if ((pointer_pixel->value < min) && (*IMP == NOMASK))
	min = pointer_pixel->value;

      pointer_pixel++;
      IMP++;
    }

  pointer_pixel = pixel;
  IMP = input_mask;

  /*set the masked pixels to minimum*/
  for (i = 0; i < image_size; i++)
    {
      if ((*IMP) == MASK)
	{
	  pointer_pixel->value = min;
	}
      pointer_pixel++;
      IMP++;
    }
}

/*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(PIXELM *pixel, double *unwrapped_image, int image_width, int image_height)
{
  int i;
  int image_size = image_width * image_height;
  double *unwrapped_image_pointer = unwrapped_image;
  PIXELM *pixel_pointer = pixel;

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

/*the main function of the unwrapper*/
void
unwrap2D(double* wrapped_image, double* UnwrappedImage, unsigned char* input_mask,
	 int image_width, int image_height,
	 int wrap_around_x, int wrap_around_y)
{
  params_t params = {TWOPI, wrap_around_x, wrap_around_y, 0};
  unsigned char *extended_mask;
  PIXELM *pixel;
  EDGE *edge;
  int image_size = image_height * image_width;
  int No_of_Edges_initially = 2 * image_width * image_height;

  extended_mask = (unsigned char *) calloc(image_size, sizeof(unsigned char));
  pixel = (PIXELM *) calloc(image_size, sizeof(PIXELM));
  edge = (EDGE *) calloc(No_of_Edges_initially, sizeof(EDGE));

  extend_mask(input_mask, extended_mask, image_width, image_height, &params);
  initialisePIXELs(wrapped_image, input_mask, extended_mask, pixel, image_width, image_height);
  calculate_reliability(wrapped_image, pixel, image_width, image_height, &params);
  horizontalEDGEs(pixel, edge, image_width, image_height, &params);
  verticalEDGEs(pixel, edge, image_width, image_height, &params);

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

  /*gather PIXELs into groups*/
  gatherPIXELs(edge, &params);

  unwrapImage(pixel, image_width, image_height);
  maskImage(pixel, input_mask, image_width, image_height);

  /*copy the image from PIXELM structure to the unwrapped phase array*/
  /*passed to this function*/
  /*TODO: replace by (cython?) function to directly write into numpy array ?*/
  returnImage(pixel, UnwrappedImage, image_width, image_height);

  free(edge);
  free(pixel);
  free(extended_mask);
}

void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
	double sum; 	
	/* Check for proper number of arguments. */
	if(nrhs < 2) {
		mexErrMsgTxt("Too few inputs.");
	} else if(nrhs > 3){
		mexErrMsgTxt("Too many inputs.");
	} else if(nlhs>1) {
		mexErrMsgTxt("Too many outputs.");
	}

	/*Find number of rows and columns in input data*/
	int rows = mxGetM(prhs[0]);		
	int cols = mxGetN(prhs[0]);
	const int image_size = cols * rows;
	int maxNIter = 1; /* the default value */
	//if( nrhs > 2 ){
	//	maxNIter = mxGetScalar( prhs[2] );
	//}

	/*Get pointer to input data*/
	double* wrapped = mxGetPr(prhs[0]);

	/*bool mask  = mxGetPr(prhs[1]);*/
	double* inputMask =  mxGetPr( prhs[1] );
	

	unsigned char* mask = calloc(image_size, sizeof(unsigned char));


	if( mxGetM(prhs[1]) != rows || mxGetN(prhs[1]) != cols )
		mexErrMsgTxt("Phase and mask array have to be of the same size.");

	int ii=0, count=0;
	for(ii=0; ii<image_size; ++ii)
		if(inputMask[ii] > 0 ){
			mask[ii] = 255;
		}else{
			mask[ii] = 0; /*zero means no mask*/
			++count;
		}

	if(count < 4)
		mexErrMsgTxt("Too few unmasked input.");

	/*create space for output*/
	plhs[0] = mxCreateDoubleMatrix(rows, cols, mxREAL);

	/*Get pointer to output array*/
	double* unwrapped = mxGetPr(plhs[0]);

	double* tmp = (double *) calloc(image_size, sizeof(double));
	memcpy(tmp, wrapped, image_size * sizeof(double) );

	unwrap2D(wrapped, unwrapped, mask, rows, cols, false, false); 

	int iter = 0, i=0;
	for(; iter < maxNIter; ++iter){
		/*unwrap2D(tmp, unwrapped, mask, cols, rows, false, false);*/
		unwrap2D(tmp, unwrapped, mask, rows, cols, false, false); 
		if( memcmp(tmp, unwrapped, image_size*sizeof(double)) == 0)
			break;
		memcpy(tmp, unwrapped, image_size * sizeof(double));
	}
	/*if(iter == maxNIter)
		printf("unwrap2D reached maximum number of iterations, %d.", maxNIter);
	*/

	free(tmp);
	
}